diff --git a/sw/airborne/beth/bench_sensors.c b/sw/airborne/beth/bench_sensors.c index bfa85b2773..5a1269feda 100644 --- a/sw/airborne/beth/bench_sensors.c +++ b/sw/airborne/beth/bench_sensors.c @@ -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); } diff --git a/sw/airborne/beth/bench_sensors.h b/sw/airborne/beth/bench_sensors.h index d22abca81f..c260c233a2 100644 --- a/sw/airborne/beth/bench_sensors.h +++ b/sw/airborne/beth/bench_sensors.h @@ -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(); \ + } \ } diff --git a/sw/airborne/beth/main_coders.c b/sw/airborne/beth/main_coders.c new file mode 100644 index 0000000000..709f696a98 --- /dev/null +++ b/sw/airborne/beth/main_coders.c @@ -0,0 +1,283 @@ +#include BOARD_CONFIG +#include "init_hw.h" +#include "sys_time.h" +#include "downlink.h" + +#include +#include +#include +#include +#include +#include +#include +#include "interrupt_hw.h" + +#include + +/* + * + * 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); + +} + diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c index c598bdf033..55c35826c0 100644 --- a/sw/airborne/beth/main_overo.c +++ b/sw/airborne/beth/main_overo.c @@ -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++; } diff --git a/sw/airborne/beth/main_stm32.c b/sw/airborne/beth/main_stm32.c index a76803ed09..3c2e7413a5 100644 --- a/sw/airborne/beth/main_stm32.c +++ b/sw/airborne/beth/main_stm32.c @@ -21,19 +21,17 @@ * Boston, MA 02111-1307, USA. */ -#include -#include - -#include -#include #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); + +}