mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 09:36:19 +08:00
a little progress on beth
This commit is contained in:
Antoine Drouin
@@ -1,22 +1,19 @@
|
||||
#include "bench_sensors.h"
|
||||
|
||||
#include "i2c.h"
|
||||
struct BenchSensors bench_sensors;
|
||||
|
||||
bool_t bench_sensors_available;
|
||||
uint16_t bench_sensors_angle_1;
|
||||
uint16_t bench_sensors_angle_2;
|
||||
uint16_t bench_sensors_angle_3;
|
||||
uint16_t bench_sensors_current;
|
||||
|
||||
void bench_sensors_init(void) {
|
||||
bench_sensors_available = FALSE;
|
||||
bench_sensors.status = BS_IDLE;
|
||||
bench_sensors.i2c_done = TRUE;
|
||||
}
|
||||
|
||||
|
||||
void read_bench_sensors(void) {
|
||||
|
||||
const uint8_t bench_addr = 0x52;
|
||||
|
||||
i2c1_receive(bench_addr, 4, &bench_sensors_available);
|
||||
const uint8_t bench_addr = 0x30;
|
||||
bench_sensors.status = BS_BUSY;
|
||||
bench_sensors.i2c_done = FALSE;
|
||||
i2c2_receive(bench_addr, 4, &bench_sensors.i2c_done);
|
||||
|
||||
}
|
||||
|
||||
@@ -2,24 +2,31 @@
|
||||
#define BENCH_SENSORS_H
|
||||
|
||||
#include "std.h"
|
||||
#include "i2c.h"
|
||||
|
||||
extern void bench_sensors_init(void);
|
||||
extern void read_bench_sensors(void);
|
||||
extern bool_t bench_sensors_available;
|
||||
extern uint16_t bench_sensors_angle_1;
|
||||
extern uint16_t bench_sensors_angle_2;
|
||||
extern uint16_t bench_sensors_angle_3;
|
||||
extern uint16_t bench_sensors_current;
|
||||
|
||||
enum BenchSensorsStatus { BS_IDLE, BS_BUSY, BS_AVAILABLE};
|
||||
|
||||
struct BenchSensors {
|
||||
enum BenchSensorsStatus status;
|
||||
uint16_t angle_1;
|
||||
uint16_t angle_2;
|
||||
uint16_t angle_3;
|
||||
uint16_t current;
|
||||
bool_t i2c_done;
|
||||
};
|
||||
|
||||
extern struct BenchSensors bench_sensors;
|
||||
|
||||
#define BenchSensorsEvent( _handler) { \
|
||||
if (bench_sensors_available) { \
|
||||
bench_sensors_angle_1 = i2c1_buf[0]; \
|
||||
bench_sensors_angle_2 = i2c1_buf[1]; \
|
||||
bench_sensors_angle_3 = i2c1_buf[2]; \
|
||||
bench_sensors_current = i2c1_buf[3]; \
|
||||
_handler(); \
|
||||
bench_sensors_available = FALSE; \
|
||||
} \
|
||||
if (bench_sensors.status == BS_BUSY && bench_sensors.i2c_done) { \
|
||||
bench_sensors.angle_1 = i2c2.buf[0] + (i2c2.buf[1] << 8); \
|
||||
bench_sensors.angle_2 = i2c2.buf[2] + (i2c2.buf[3] << 8); \
|
||||
bench_sensors.status = IDLE; \
|
||||
_handler(); \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,283 @@
|
||||
#include BOARD_CONFIG
|
||||
#include "init_hw.h"
|
||||
#include "sys_time.h"
|
||||
#include "downlink.h"
|
||||
|
||||
#include <stm32/rcc.h>
|
||||
#include <stm32/gpio.h>
|
||||
#include <stm32/flash.h>
|
||||
#include <stm32/misc.h>
|
||||
#include <stm32/dma.h>
|
||||
#include <stm32/adc.h>
|
||||
#include <stm32/i2c.h>
|
||||
#include "interrupt_hw.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
/*
|
||||
*
|
||||
* PC.01 (ADC Channel11) ext1-20 coder_values[1]
|
||||
* PC.04 (ADC Channel14) ext2-12 coder_values[0]
|
||||
*
|
||||
* PB.10 I2C2 SCL ext2-14
|
||||
* PB.11 I2C2 SDA ext2-15
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
static inline void main_init( void );
|
||||
static inline void main_periodic( void );
|
||||
static inline void main_event( void );
|
||||
|
||||
static inline void main_init_adc(void);
|
||||
//static inline void main_init_i2c2(void);
|
||||
//void i2c2_ev_irq_handler(void);
|
||||
//void i2c2_er_irq_handler(void);
|
||||
|
||||
#define ADC1_DR_Address ((uint32_t)0x4001244C)
|
||||
static uint16_t coder_values[3];
|
||||
|
||||
#define I2C2_SLAVE_ADDRESS7 0x30
|
||||
#define I2C2_ClockSpeed 200000
|
||||
|
||||
#define MY_I2C2_BUF_LEN 4
|
||||
static uint8_t i2c2_idx;
|
||||
static uint8_t i2c2_buf[MY_I2C2_BUF_LEN];
|
||||
|
||||
int main(void) {
|
||||
|
||||
main_init();
|
||||
|
||||
while (1) {
|
||||
if (sys_time_periodic())
|
||||
main_periodic();
|
||||
main_event();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static inline void main_init( void ) {
|
||||
hw_init();
|
||||
sys_time_init();
|
||||
main_init_adc();
|
||||
// main_init_i2c2();
|
||||
int_enable();
|
||||
|
||||
}
|
||||
|
||||
|
||||
static inline void main_periodic( void ) {
|
||||
|
||||
RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
|
||||
|
||||
RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &coder_values[0], &coder_values[1]);});
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
static inline void main_event( void ) {
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* I2C2 : autopilot link
|
||||
*
|
||||
*/
|
||||
void i2c2_init(void) {
|
||||
//static inline void main_init_i2c2(void) {
|
||||
|
||||
/* System clocks configuration ---------------------------------------------*/
|
||||
/* Enable I2C2 clock */
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
|
||||
/* Enable GPIOB clock */
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
||||
|
||||
/* NVIC configuration ------------------------------------------------------*/
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
|
||||
/* Configure and enable I2C2 event interrupt -------------------------------*/
|
||||
NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
/* Configure and enable I2C2 error interrupt -------------------------------*/
|
||||
NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
|
||||
/* GPIO configuration ------------------------------------------------------*/
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
/* Configure I2C2 pins: SCL and SDA ----------------------------------------*/
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
|
||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
/* Enable I2C2 -------------------------------------------------------------*/
|
||||
I2C_Cmd(I2C2, ENABLE);
|
||||
/* I2C2 configuration ------------------------------------------------------*/
|
||||
I2C_InitTypeDef I2C_InitStructure;
|
||||
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
|
||||
I2C_InitStructure.I2C_OwnAddress1 = I2C2_SLAVE_ADDRESS7;
|
||||
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
|
||||
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
||||
I2C_InitStructure.I2C_ClockSpeed = I2C2_ClockSpeed;
|
||||
I2C_Init(I2C2, &I2C_InitStructure);
|
||||
|
||||
/* Enable I2C1 event and buffer interrupts */
|
||||
// I2C_ITConfig(I2C2, I2C_IT_EVT | I2C_IT_BUF, ENABLE);
|
||||
I2C_ITConfig(I2C2, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void i2c2_ev_irq_handler(void) {
|
||||
switch (I2C_GetLastEvent(I2C2))
|
||||
{
|
||||
/* Slave Transmitter ---------------------------------------------------*/
|
||||
case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: /* EV1 */
|
||||
memcpy(i2c2_buf, coder_values, MY_I2C2_BUF_LEN);
|
||||
i2c2_idx = 0;
|
||||
|
||||
case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: /* EV3 */
|
||||
/* Transmit I2C2 data */
|
||||
if (i2c2_idx < MY_I2C2_BUF_LEN) {
|
||||
I2C_SendData(I2C2, i2c2_buf[i2c2_idx]);
|
||||
i2c2_idx++;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case I2C_EVENT_SLAVE_STOP_DETECTED: /* EV4 */
|
||||
LED_ON(1);
|
||||
/* Clear I2C2 STOPF flag: read of I2C_SR1 followed by a write on I2C_CR1 */
|
||||
(void)(I2C_GetITStatus(I2C2, I2C_IT_STOPF));
|
||||
I2C_Cmd(I2C2, ENABLE);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void i2c2_er_irq_handler(void) {
|
||||
/* Check on I2C2 AF flag and clear it */
|
||||
if (I2C_GetITStatus(I2C2, I2C_IT_AF)) {
|
||||
I2C_ClearITPendingBit(I2C2, I2C_IT_AF);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* ADC : coders
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
static inline void main_init_adc(void) {
|
||||
|
||||
/* Enable DMA1 clock */
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
|
||||
|
||||
/* Enable ADC1 and GPIOC clock */
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 |
|
||||
RCC_APB2Periph_GPIOC, ENABLE);
|
||||
|
||||
/* Configure PC.01 (ADC Channel11) and PC.04 (ADC Channel14) as analog input-*/
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
|
||||
GPIO_Init(GPIOC, &GPIO_InitStructure);
|
||||
|
||||
/* DMA1 channel1 configuration ----------------------------------------------*/
|
||||
DMA_InitTypeDef DMA_InitStructure;
|
||||
DMA_DeInit(DMA1_Channel1);
|
||||
DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address;
|
||||
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&coder_values;
|
||||
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
|
||||
DMA_InitStructure.DMA_BufferSize = 1;
|
||||
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
|
||||
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Disable;
|
||||
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
|
||||
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
|
||||
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
|
||||
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
|
||||
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
|
||||
DMA_Init(DMA1_Channel1, &DMA_InitStructure);
|
||||
|
||||
/* Enable DMA1 channel1 */
|
||||
DMA_Cmd(DMA1_Channel1, ENABLE);
|
||||
|
||||
/* ADC1 configuration ------------------------------------------------------*/
|
||||
ADC_InitTypeDef ADC_InitStructure;
|
||||
ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
|
||||
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
|
||||
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
|
||||
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
|
||||
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
|
||||
ADC_InitStructure.ADC_NbrOfChannel = 1;
|
||||
ADC_Init(ADC1, &ADC_InitStructure);
|
||||
|
||||
/* ADC1 regular channel14 configuration */
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 1, ADC_SampleTime_239Cycles5);
|
||||
|
||||
/* Enable ADC1 DMA */
|
||||
ADC_DMACmd(ADC1, ENABLE);
|
||||
|
||||
|
||||
/* ADC2 configuration ------------------------------------------------------*/
|
||||
ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
|
||||
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
|
||||
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
|
||||
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
|
||||
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
|
||||
ADC_InitStructure.ADC_NbrOfChannel = 1;
|
||||
ADC_Init(ADC2, &ADC_InitStructure);
|
||||
/* ADC2 regular channels configuration */
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_239Cycles5);
|
||||
/* Enable ADC2 external trigger conversion */
|
||||
ADC_ExternalTrigConvCmd(ADC2, ENABLE);
|
||||
|
||||
|
||||
/* Enable ADC1 */
|
||||
ADC_Cmd(ADC1, ENABLE);
|
||||
|
||||
/* Enable ADC1 reset calibaration register */
|
||||
ADC_ResetCalibration(ADC1);
|
||||
/* Check the end of ADC1 reset calibration register */
|
||||
while(ADC_GetResetCalibrationStatus(ADC1));
|
||||
|
||||
/* Start ADC1 calibaration */
|
||||
ADC_StartCalibration(ADC1);
|
||||
/* Check the end of ADC1 calibration */
|
||||
while(ADC_GetCalibrationStatus(ADC1));
|
||||
|
||||
/* Enable ADC2 */
|
||||
ADC_Cmd(ADC2, ENABLE);
|
||||
|
||||
/* Enable ADC2 reset calibaration register */
|
||||
ADC_ResetCalibration(ADC2);
|
||||
/* Check the end of ADC2 reset calibration register */
|
||||
while(ADC_GetResetCalibrationStatus(ADC2));
|
||||
|
||||
/* Start ADC2 calibaration */
|
||||
ADC_StartCalibration(ADC2);
|
||||
/* Check the end of ADC2 calibration */
|
||||
while(ADC_GetCalibrationStatus(ADC2));
|
||||
|
||||
|
||||
/* Start ADC1 Software Conversion */
|
||||
ADC_SoftwareStartConvCmd(ADC1, ENABLE);
|
||||
|
||||
}
|
||||
|
||||
@@ -59,12 +59,12 @@ static void send_message() {
|
||||
static uint32_t foo = 0;
|
||||
|
||||
spi_link_send(msg_out, sizeof(union AutopilotMessageBeth), msg_in);
|
||||
if (!foo%100) {
|
||||
printf("%d -> %d %d %d %d %d %d %d %d %d\n", foo,
|
||||
msg_in->bench_sensor.x, msg_in->bench_sensor.y, msg_in->bench_sensor.z,
|
||||
msg_in->gyro.x, msg_in->gyro.y, msg_in->gyro.z,
|
||||
msg_in->accel.x, msg_in->accel.y, msg_in->accel.z);
|
||||
}
|
||||
// if (!foo%100) {
|
||||
printf("%d -> %d %d %d %d %d %d %d %d %d\n", foo,
|
||||
msg_in->bench_sensor.x, msg_in->bench_sensor.y, msg_in->bench_sensor.z,
|
||||
msg_in->gyro.x, msg_in->gyro.y, msg_in->gyro.z,
|
||||
msg_in->accel.x, msg_in->accel.y, msg_in->accel.z);
|
||||
// }
|
||||
foo++;
|
||||
}
|
||||
|
||||
|
||||
@@ -21,19 +21,17 @@
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#include <stm32/rcc.h>
|
||||
#include <stm32/gpio.h>
|
||||
|
||||
#include <stm32/flash.h>
|
||||
#include <stm32/misc.h>
|
||||
|
||||
#include BOARD_CONFIG
|
||||
#include "init_hw.h"
|
||||
#include "sys_time.h"
|
||||
#include "downlink.h"
|
||||
#include "booz/booz2_commands.h"
|
||||
#include "booz/booz_actuators.h"
|
||||
//#include "booz/booz_radio_control.h"
|
||||
#include "booz/booz_imu.h"
|
||||
#include "lisa/lisa_overo_link.h"
|
||||
|
||||
#include "beth/bench_sensors.h"
|
||||
|
||||
static inline void main_init( void );
|
||||
static inline void main_periodic( void );
|
||||
@@ -44,6 +42,7 @@ static inline void on_mag_event(void);
|
||||
|
||||
static inline void main_on_overo_msg_received(void);
|
||||
static inline void main_on_overo_link_lost(void);
|
||||
static inline void main_on_bench_sensors( void );
|
||||
|
||||
static int16_t my_cnt;
|
||||
|
||||
@@ -62,20 +61,29 @@ int main(void) {
|
||||
static inline void main_init( void ) {
|
||||
hw_init();
|
||||
sys_time_init();
|
||||
booz_imu_init();
|
||||
overo_link_init();
|
||||
actuators_init();
|
||||
// radio_control_init();
|
||||
// booz_imu_init();
|
||||
// overo_link_init();
|
||||
bench_sensors_init();
|
||||
}
|
||||
|
||||
static inline void main_periodic( void ) {
|
||||
booz_imu_periodic();
|
||||
OveroLinkPperiodic(main_on_overo_link_lost)
|
||||
// booz_imu_periodic();
|
||||
actuators_set(FALSE);
|
||||
// OveroLinkPeriodic(main_on_overo_link_lost)
|
||||
RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
|
||||
|
||||
|
||||
read_bench_sensors();
|
||||
|
||||
}
|
||||
|
||||
static inline void main_event( void ) {
|
||||
BoozImuEvent(on_gyro_accel_event, on_mag_event);
|
||||
OveroLinkEvent(main_on_overo_msg_received);
|
||||
// BoozImuEvent(on_gyro_accel_event, on_mag_event);
|
||||
// OveroLinkEvent(main_on_overo_msg_received);
|
||||
|
||||
BenchSensorsEvent(main_on_bench_sensors);
|
||||
|
||||
}
|
||||
|
||||
static inline void main_on_overo_msg_received(void) {
|
||||
@@ -151,3 +159,12 @@ static inline void on_mag_event(void) {
|
||||
&booz_imu.mag_unscaled.z);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static inline void main_on_bench_sensors( void ) {
|
||||
|
||||
DOWNLINK_SEND_ADC_GENERIC(DefaultChannel,
|
||||
&bench_sensors.angle_1,
|
||||
&bench_sensors.angle_2);
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user