diff --git a/conf/airframes/gorazoptere_brushless_ANALOG.xml b/conf/airframes/gorazoptere_brushless_ANALOG.xml index 48814bcdf8..14955f3e68 100644 --- a/conf/airframes/gorazoptere_brushless_ANALOG.xml +++ b/conf/airframes/gorazoptere_brushless_ANALOG.xml @@ -1,4 +1,4 @@ - +
diff --git a/sw/airborne/autopilot/mainloop.c b/sw/airborne/autopilot/mainloop.c index ff923f9cb6..a8b703970a 100644 --- a/sw/airborne/autopilot/mainloop.c +++ b/sw/airborne/autopilot/mainloop.c @@ -65,6 +65,8 @@ int main( void ) { ir_init(); estimator_init(); + uart0_init(); + /** - start interrupt task */ sei(); @@ -101,6 +103,16 @@ int main( void ) { #ifdef IMU_TYPE_3DMG DOWNLINK_SEND_IMU_3DMG(&from_fbw.euler_dot[0], &from_fbw.euler_dot[1], &from_fbw.euler_dot[2], &from_fbw.euler[0], &from_fbw.euler[1], &from_fbw.euler[2]); estimator_update_state_3DMG(); +#endif +#ifdef SECTION_IMU_ANALOG + uart0_transmit('G'); + uart0_transmit(' '); + uart0_print_hex16(from_fbw.euler_dot[0]); + uart0_transmit(','); + uart0_print_hex16(from_fbw.euler_dot[1]); + uart0_transmit(','); + uart0_print_hex16(from_fbw.euler_dot[2]); + uart0_transmit('\n'); #endif } } diff --git a/sw/airborne/fly_by_wire/imu.c b/sw/airborne/fly_by_wire/imu.c index 81fffc38b8..831fd5415e 100644 --- a/sw/airborne/fly_by_wire/imu.c +++ b/sw/airborne/fly_by_wire/imu.c @@ -25,7 +25,7 @@ #include "airframe.h" #if defined(SECTION_IMU_3DMG) && defined(SECTION_IMU_ANALOG) -#error "IMU_3DMG and IMU_ANALOG cannot be defined simultaneously +#error "IMU_3DMG and IMU_ANALOG cannot be defined simultaneously" #endif #include "imu.h" diff --git a/sw/airborne/fly_by_wire/imu.h b/sw/airborne/fly_by_wire/imu.h index 74a9a912fd..8b76d870b9 100644 --- a/sw/airborne/fly_by_wire/imu.h +++ b/sw/airborne/fly_by_wire/imu.h @@ -3,7 +3,7 @@ #include -#ifdef IMU_TYPE_3DMG +#ifdef SECTION_IMU_3DMG extern int16_t roll, pitch, yaw; #endif extern int16_t roll_dot, pitch_dot, yaw_dot; diff --git a/sw/airborne/fly_by_wire/link_autopilot.h b/sw/airborne/fly_by_wire/link_autopilot.h index 976e2ac050..3ea53052a1 100644 --- a/sw/airborne/fly_by_wire/link_autopilot.h +++ b/sw/airborne/fly_by_wire/link_autopilot.h @@ -54,10 +54,10 @@ struct inter_mcu_msg { uint8_t status; uint8_t nb_err; uint8_t vsupply; /* 1e-1 V */ -#if defined IMU_TYPE_3DMG || defined IMU_TYPE_ANALOG +#if defined SECTION_IMU_ANALOG || defined SECTION_IMU_3DMG int16_t euler_dot[3]; #endif -#ifdef IMU_TYPE_3DMG +#ifdef SECTION_IMU_3DMG int16_t euler[3]; #endif }; diff --git a/sw/airborne/fly_by_wire/main.c b/sw/airborne/fly_by_wire/main.c index 10895dcc10..028abff67d 100644 --- a/sw/airborne/fly_by_wire/main.c +++ b/sw/airborne/fly_by_wire/main.c @@ -44,11 +44,11 @@ #include "uart.h" -#ifdef IMU_TYPE_3DMG +#ifdef SECTION_IMU_3DMG #include "3dmg.h" #endif -#if defined IMU_TYPE_ANALOG || defined IMU_TYPE_3DMG +#if defined SECTION_IMU_ANALOG || defined SECTION_IMU_3DMG #include "imu.h" #include "control.h" #endif @@ -85,14 +85,14 @@ static inline void to_autopilot_from_last_radio (void) { } to_mega128.ppm_cpt = last_ppm_cpt; to_mega128.vsupply = VoltageOfAdc(vsupply_adc_buf.sum/AV_NB_SAMPLE) * 10; -#if defined IMU_TYPE_3DMG || defined IMU_TYPE_ANALOG +#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG to_mega128.euler_dot[0] = roll_dot; to_mega128.euler_dot[1] = pitch_dot; to_mega128.euler_dot[2] = yaw_dot; #endif -#ifdef IMU_TYPE_3DMG +#ifdef SECTION_IMU_3DMG to_mega128.euler[0] = roll; - to_mega128.euler[1] = 777; //pitch; + to_mega128.euler[1] = pitch; to_mega128.euler[2] = yaw; #endif } @@ -107,9 +107,7 @@ inline void radio_control_task(void) { mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]); } if (mode == MODE_MANUAL) { -#if defined IMU_TYPE_3DMG - // control_set_desired(last_radio); -#elif defined IMU_TYPE_ANALOG +#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG control_set_desired(last_radio); #else servo_set(last_radio); @@ -121,12 +119,13 @@ inline void spi_task(void) { if (mega128_receive_valid) { time_since_last_mega128 = 0; mega128_ok = TRUE; - if (mode == MODE_AUTO) -#if defined IMU_TYPE_ANALOG || defined IMU_TYPE_3DMG + if (mode == MODE_AUTO) { +#if defined SECTION_IMU_ANALOG || defined SECTION_IMU_3DMG control_set_desired(from_mega128.channels); #else servo_set(from_mega128.channels); #endif + } } to_autopilot_from_last_radio(); spi_reset(); @@ -135,7 +134,7 @@ inline void spi_task(void) { int main( void ) { uart_init_tx(); -#if defined IMU_TYPE_3DMG +#if defined SECTION_IMU_3DMG uart_init_rx(); #else uart_print_string("FBW Booting $Id$\n"); @@ -143,7 +142,7 @@ int main( void ) adc_init(); adc_buf_channel(3, &vsupply_adc_buf); adc_buf_channel(6, &vservos_adc_buf); -#if defined IMU_TYPE_3DMG || defined IMU_TYPE_ANALOG +#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG imu_init(); #endif timer_init(); @@ -170,7 +169,7 @@ int main( void ) spi_was_interrupted = FALSE; spi_task(); } -#ifdef IMU_TYPE_3DMG +#ifdef SECTION_IMU_3DMG if (_3dmg_data_ready) { imu_update(); RED_LED_TOGGLE(); @@ -202,8 +201,25 @@ int main( void ) static uint8_t _20Hz; _1Hz++; _20Hz++; -#ifdef IMU_TYPE_3DMG - // control_run(); +#if defined SECTION_IMU_ANALOG + imu_update(); +#if 0 + { + static uint8_t foo = 0; + foo++; + if (!(foo%10)) { + uart_print_hex16(roll_dot); + uart_transmit(','); + uart_print_hex16(pitch_dot); + uart_transmit(','); + uart_print_hex16(yaw_dot); + uart_transmit('\n'); + } + } +#endif /* 0 */ +#endif +#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG + control_run(); #endif if (_1Hz >= 60) { _1Hz = 0; @@ -212,8 +228,8 @@ int main( void ) } if (_20Hz >= 3) { _20Hz = 0; -#ifndef IMU_TYPE_3DMG - servo_transmit(); +#ifndef SECTION_IMU_3DMG + // servo_transmit(); #endif } if (time_since_last_mega128 < STALLED_TIME)