diff --git a/sw/airborne/fly_by_wire/Makefile b/sw/airborne/fly_by_wire/Makefile index 09c2e044bf..e518678576 100644 --- a/sw/airborne/fly_by_wire/Makefile +++ b/sw/airborne/fly_by_wire/Makefile @@ -43,11 +43,6 @@ ifeq ($(CTL_BRD_VERSION),V1_2) CTL_BRD_FLAGS=-DCTL_BRD_V1_2 endif -ifeq ($(CTL_BRD_VERSION),V1_1) -CTL_BRD_FLAGS=-DCTL_BRD_V1_1 -endif - - $(TARGET).srcs = \ main.c \ ppm.c \ diff --git a/sw/airborne/fly_by_wire/control.c b/sw/airborne/fly_by_wire/control.c new file mode 100644 index 0000000000..4f21f3cd59 --- /dev/null +++ b/sw/airborne/fly_by_wire/control.c @@ -0,0 +1,75 @@ +#include "control.h" +#include "imu.h" +#include "airframe.h" + +/* WARNING : taken from ../autopilot/autopilot.h */ +#define TRIM_PPRZ(pprz) (pprz < MIN_PPRZ ? MIN_PPRZ : \ + (pprz > MAX_PPRZ ? MAX_PPRZ : \ + pprz)) + +int16_t desired_roll_dot; +float roll_dot_pgain = ROLL_DOT_PGAIN; +float roll_dot_igain = ROLL_DOT_IGAIN; +float roll_dot_dgain = ROLL_DOT_DGAIN; +int16_t roll_dot_sum_err = 0; +int16_t roll_dot_last_err = 0; +pprz_t command_roll; + +int16_t desired_pitch_dot; +float pitch_dot_pgain = PITCH_DOT_PGAIN; +float pitch_dot_igain = PITCH_DOT_IGAIN; +float pitch_dot_dgain = PITCH_DOT_DGAIN; +int16_t pitch_dot_sum_err = 0; +int16_t pitch_dot_last_err = 0; +pprz_t command_pitch; + +int16_t desired_yaw_dot; +float yaw_dot_pgain = YAW_DOT_PGAIN; +float yaw_dot_igain = YAW_DOT_IGAIN; +float yaw_dot_dgain = YAW_DOT_DGAIN; +int16_t yaw_dot_sum_err = 0; +int16_t yaw_dot_last_err = 0; +pprz_t command_yaw; + +int16_t desired_throttle; +pprz_t command_throttle; + +pprz_t control_commands[] = {0, 0, 0, 0}; + + +#define TRIM(a, m) ((a) > m ? m : ((a) < -m ? -m : (a))) + +#define Int16(x) (x > 32768. ? 32768 : x < -32768 ? -32768 : (int16_t)x) + + +void control_run ( void ) { + int16_t err = roll_dot - desired_roll_dot; + int16_t d_err = err - roll_dot_last_err; + roll_dot_last_err = err; + // roll_dot_sum_err += err; + // control_commands[RADIO_ROLL] = TRIM_PPRZ((err + d_err * roll_dot_dgain + roll_dot_sum_err * roll_dot_igain) * roll_dot_pgain); + control_commands[RADIO_ROLL] = TRIM_PPRZ(Int16((err +d_err * roll_dot_dgain) * roll_dot_pgain)); + + err = pitch_dot - desired_pitch_dot; + d_err = err - pitch_dot_last_err; + pitch_dot_last_err = err; + // pitch_dot_sum_err += err; + // control_commands[RADIO_PITCH] = TRIM_PPRZ((err + d_err * pitch_dot_dgain + pitch_dot_sum_err * pitch_dot_igain) * pitch_dot_pgain); + control_commands[RADIO_PITCH] = TRIM_PPRZ(Int16((err + d_err * pitch_dot_dgain) * pitch_dot_pgain)); + + err = yaw_dot - desired_yaw_dot; + // d_err = err - yaw_dot_last_err; + // yaw_dot_last_err = err; + // yaw_dot_sum_err += err; + // control_commands[RADIO_YAW] = TRIM_PPRZ((err + d_err * yaw_dot_dgain + yaw_dot_sum_err * yaw_dot_igain) * yaw_dot_pgain); + control_commands[RADIO_YAW] = TRIM_PPRZ(Int16(err * yaw_dot_pgain)); + + control_commands[RADIO_THROTTLE] = TRIM_PPRZ(desired_throttle); +} + +void control_set_desired ( const pprz_t values[] ) { + desired_roll_dot = DESIRED_OF_RADIO_ROLL_DOT*values[RADIO_ROLL]; + desired_pitch_dot = DESIRED_OF_RADIO_PITCH_DOT*values[RADIO_PITCH]; + desired_yaw_dot = DESIRED_OF_RADIO_YAW_DOT*values[RADIO_YAW]; + desired_throttle = values[RADIO_THROTTLE]; +} diff --git a/sw/airborne/fly_by_wire/control.h b/sw/airborne/fly_by_wire/control.h new file mode 100644 index 0000000000..fbec4d4c3e --- /dev/null +++ b/sw/airborne/fly_by_wire/control.h @@ -0,0 +1,26 @@ +#ifndef CONTROL_H +#define CONTROL_H + +#include +#include "link_autopilot.h" + +extern int16_t desired_roll_dot; +extern float roll_dot_pgain; +extern float roll_dot_dgain; + +extern int16_t desired_pitch_dot; +extern float pitch_dot_pgain; +extern float pitch_dot_dgain; + +extern int16_t desired_yaw_dot; +extern float yaw_dot_dgain; +extern float yaw_dot_pgain; + +extern int16_t desired_throttle; + +extern pprz_t control_commands[]; + +void control_run ( void ); +void control_set_desired ( const pprz_t values[] ); + +#endif /* CONTROL_H */ diff --git a/sw/airborne/fly_by_wire/imu.c b/sw/airborne/fly_by_wire/imu.c new file mode 100644 index 0000000000..9c0080f223 --- /dev/null +++ b/sw/airborne/fly_by_wire/imu.c @@ -0,0 +1,67 @@ +#include "imu.h" + +#include "airframe.h" + +int16_t roll_dot, pitch_dot, yaw_dot; + +#ifdef IMU_TYPE_3DMG +#include "3dmg.h" +int16_t roll, pitch, yaw; + +void imu_init ( void ) { + _3dmg_set_continuous_mode(); +} + +void imu_update ( void ) { + roll_dot = _3dmg_roll_dot; + pitch_dot = _3dmg_pitch_dot; + yaw_dot = _3dmg_yaw_dot; + + roll = _3dmg_roll; + pitch = _3dmg_pitch; + yaw = _3dmg_yaw; +} + +void imu_capture_neutral ( void ) { + _3dmg_capture_neutral(); +} + +#endif + +#ifdef IMU_TYPE_ANALOG +#include "adc_fbw.h" + +struct adc_buf buf_roll_dot; +struct adc_buf buf_pitch_dot; +struct adc_buf buf_yaw_dot; + +uint16_t raw_roll_dot = 512*AV_NB_SAMPLE; +uint16_t raw_pitch_dot = 512*AV_NB_SAMPLE; +uint16_t raw_yaw_dot = 512*AV_NB_SAMPLE; + +uint16_t raw_roll_dot_neutral = 512*AV_NB_SAMPLE; +uint16_t raw_pitch_dot_neutral = 512*AV_NB_SAMPLE; +uint16_t raw_yaw_dot_neutral = 512*AV_NB_SAMPLE; + +void imu_init ( void ) { + adc_buf_channel(IMU_ADC_ROLL_DOT, &buf_roll_dot); + adc_buf_channel(IMU_ADC_PITCH_DOT, &buf_pitch_dot); + adc_buf_channel(IMU_ADC_YAW_DOT, &buf_yaw_dot); +} + +void imu_update ( void ) { + raw_roll_dot = buf_roll_dot.sum; + raw_pitch_dot = buf_pitch_dot.sum; + raw_yaw_dot = buf_yaw_dot.sum; + roll_dot = (int16_t)(raw_roll_dot - raw_roll_dot_neutral) / AV_NB_SAMPLE; + pitch_dot = (int16_t)(raw_pitch_dot - raw_pitch_dot_neutral) / AV_NB_SAMPLE; + yaw_dot = (int16_t)(raw_yaw_dot - raw_yaw_dot_neutral) / AV_NB_SAMPLE;; +} + +void imu_capture_neutral ( void ) { + raw_roll_dot_neutral = raw_roll_dot; + raw_pitch_dot_neutral = raw_pitch_dot; + raw_yaw_dot_neutral = raw_yaw_dot; +} + +#endif diff --git a/sw/airborne/fly_by_wire/imu.h b/sw/airborne/fly_by_wire/imu.h new file mode 100644 index 0000000000..74a9a912fd --- /dev/null +++ b/sw/airborne/fly_by_wire/imu.h @@ -0,0 +1,15 @@ +#ifndef IMU_H +#define IMU_H + +#include + +#ifdef IMU_TYPE_3DMG +extern int16_t roll, pitch, yaw; +#endif +extern int16_t roll_dot, pitch_dot, yaw_dot; + +extern void imu_init ( void ); +extern void imu_update ( void ); +extern void imu_capture_neutral ( void ); + +#endif /* IMU_H */ diff --git a/sw/airborne/fly_by_wire/led.h b/sw/airborne/fly_by_wire/led.h new file mode 100644 index 0000000000..979d44b8bf --- /dev/null +++ b/sw/airborne/fly_by_wire/led.h @@ -0,0 +1,26 @@ +/* 3 leds plugged on rc receiver port for debug purpose */ + +#ifndef LED_H +#define LED_H +#include + +#define LEDS_INIT() { \ + DDRB |= _BV(0) | _BV(1); \ + DDRC |= _BV(0); \ + } +#define RED_LED_ON() PORTC &= ~_BV(0) +#define RED_LED_OFF() PORTC |= _BV(0) +#define RED_LED_TOGGLE() PORTC ^= _BV(0) + +#define YELLOW_LED_ON() PORTB &= ~_BV(0) +#define YELLOW_LED_OFF() PORTB |= _BV(0) +#define YELLOW_LED_TOGGLE() PORTB ^= _BV(0) + +#define GREEN_LED_ON() PORTB &= ~_BV(1) +#define GREEN_LED_OFF() PORTB |= _BV(1) +#define GREEN_LED_TOGGLE() PORTB ^= _BV(1) + + + + +#endif /* LED_H */ diff --git a/sw/airborne/fly_by_wire/link_autopilot.h b/sw/airborne/fly_by_wire/link_autopilot.h index de0a861b20..bf2ad8d7fa 100644 --- a/sw/airborne/fly_by_wire/link_autopilot.h +++ b/sw/airborne/fly_by_wire/link_autopilot.h @@ -47,6 +47,12 @@ 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 + int16_t euler_dot[3]; +#endif +#ifdef IMU_TYPE_3DMG + int16_t euler[3]; +#endif }; // Status bits from FBW to AUTOPILOT diff --git a/sw/airborne/fly_by_wire/main.c b/sw/airborne/fly_by_wire/main.c index c0dd428c08..10895dcc10 100644 --- a/sw/airborne/fly_by_wire/main.c +++ b/sw/airborne/fly_by_wire/main.c @@ -22,11 +22,18 @@ * */ +//#define LED_DEBUG + #include #include #include #include + +#ifdef LED_DEBUG +#include "led.h" +#endif + #include "timer.h" #include "servo.h" #include "ppm.h" @@ -37,12 +44,18 @@ #include "uart.h" +#ifdef IMU_TYPE_3DMG +#include "3dmg.h" +#endif + +#if defined IMU_TYPE_ANALOG || defined IMU_TYPE_3DMG +#include "imu.h" +#include "control.h" +#endif -#ifndef CTL_BRD_V1_1 #include "adc_fbw.h" struct adc_buf vsupply_adc_buf; struct adc_buf vservos_adc_buf; -#endif uint8_t mode; static uint8_t time_since_last_mega128; @@ -57,26 +70,6 @@ static uint8_t ppm_cpt, last_ppm_cpt; #define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer -/* static inline void status_transmit( void ) { */ -/* uint8_t i; */ -/* uart_transmit(7); */ -/* uart_transmit(7); */ -/* for (i=0; i>8); */ -/* uart_transmit(ppm_pulses[i] & 0xff); */ -/* } */ -/* uart_transmit('\n'); */ -/* } */ - - /* Prepare data to be sent to mcu0 */ static inline void to_autopilot_from_last_radio (void) { uint8_t i; @@ -91,59 +84,102 @@ static inline void to_autopilot_from_last_radio (void) { last_radio_contains_avg_channels = FALSE; } to_mega128.ppm_cpt = last_ppm_cpt; -#ifndef CTL_BRD_V1_1 to_mega128.vsupply = VoltageOfAdc(vsupply_adc_buf.sum/AV_NB_SAMPLE) * 10; -#else - to_mega128.vsupply = 0; +#if defined IMU_TYPE_3DMG || defined IMU_TYPE_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 + to_mega128.euler[0] = roll; + to_mega128.euler[1] = 777; //pitch; + to_mega128.euler[2] = yaw; #endif } +inline void radio_control_task(void) { + ppm_cpt++; + radio_ok = TRUE; + radio_really_lost = FALSE; + time_since_last_ppm = 0; + last_radio_from_ppm(); + if (last_radio_contains_avg_channels) { + 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 + control_set_desired(last_radio); +#else + servo_set(last_radio); +#endif + } +} + +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 + control_set_desired(from_mega128.channels); +#else + servo_set(from_mega128.channels); +#endif + } + to_autopilot_from_last_radio(); + spi_reset(); +} int main( void ) { uart_init_tx(); +#if defined IMU_TYPE_3DMG + uart_init_rx(); +#else uart_print_string("FBW Booting $Id$\n"); - -#ifndef CTL_BRD_V1_1 +#endif 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 + imu_init(); #endif timer_init(); +#if defined LED_DEBUG + LEDS_INIT(); + RED_LED_ON(); + GREEN_LED_ON(); + YELLOW_LED_ON(); +#else servo_init(); ppm_init(); +#endif spi_init(); sei(); while( 1 ) { if( ppm_valid ) { ppm_valid = FALSE; - ppm_cpt++; - radio_ok = TRUE; - radio_really_lost = FALSE; - time_since_last_ppm = 0; - last_radio_from_ppm(); - if (last_radio_contains_avg_channels) { - mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]); - } - if (mode == MODE_MANUAL) { - servo_set(last_radio); - } - } else if (mode == MODE_MANUAL && radio_really_lost) { + radio_control_task(); + } + else if (mode == MODE_MANUAL && radio_really_lost) { mode = MODE_AUTO; } - if ( !SpiIsSelected() && spi_was_interrupted ) { spi_was_interrupted = FALSE; - if (mega128_receive_valid) { - time_since_last_mega128 = 0; - mega128_ok = TRUE; - if (mode == MODE_AUTO) - servo_set(from_mega128.channels); - } - to_autopilot_from_last_radio(); - spi_reset(); + spi_task(); } - +#ifdef IMU_TYPE_3DMG + if (_3dmg_data_ready) { + imu_update(); + RED_LED_TOGGLE(); + if (roll>0) {GREEN_LED_ON();} + else {GREEN_LED_OFF();} + if (pitch>0) {YELLOW_LED_ON();} + else {YELLOW_LED_OFF();} + } +#endif if (time_since_last_ppm >= STALLED_TIME) { radio_ok = FALSE; } @@ -166,6 +202,9 @@ int main( void ) static uint8_t _20Hz; _1Hz++; _20Hz++; +#ifdef IMU_TYPE_3DMG + // control_run(); +#endif if (_1Hz >= 60) { _1Hz = 0; last_ppm_cpt = ppm_cpt; @@ -173,8 +212,9 @@ int main( void ) } if (_20Hz >= 3) { _20Hz = 0; +#ifndef IMU_TYPE_3DMG servo_transmit(); - // status_transmit(); +#endif } if (time_since_last_mega128 < STALLED_TIME) time_since_last_mega128++; diff --git a/sw/airborne/fly_by_wire/servo.c b/sw/airborne/fly_by_wire/servo.c index 86cd0d21ff..bab2f48575 100644 --- a/sw/airborne/fly_by_wire/servo.c +++ b/sw/airborne/fly_by_wire/servo.c @@ -40,12 +40,6 @@ */ #define _4017_NB_CHANNELS 10 -#ifdef CTL_BRD_V1_1 -#define _4017_RESET_PORT PORTD -#define _4017_RESET_DDR DDRD -#define _4017_RESET_PIN 5 -#endif /* CTL_BRD_V1_1 */ - #ifdef CTL_BRD_V1_2 #define _4017_RESET_PORT PORTC #define _4017_RESET_DDR DDRC diff --git a/sw/airborne/fly_by_wire/uart.h b/sw/airborne/fly_by_wire/uart.h index 9c95d4e9e6..3c01ca1942 100644 --- a/sw/airborne/fly_by_wire/uart.h +++ b/sw/airborne/fly_by_wire/uart.h @@ -36,4 +36,10 @@ void uart_print_hex16 ( uint16_t c ); void uart_print_string(const uint8_t* s); void uart_print_float( const float * f); +#define ReceiveUart(cb) \ + SIGNAL( SIG_UART_RECV ) { \ + uint8_t c = UDR; \ + cb(c); \ +} + #endif