From ff161f2d4680af437fa0184f418109402bc92bdb Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Thu, 15 Dec 2005 17:40:32 +0000 Subject: [PATCH] common main() --- Doxyfile | 2 +- conf/autopilot/twin_avr.makefile | 6 +- sw/airborne/main.c | 32 ++++++ sw/airborne/main_fbw.c | 163 ++++++++++++++----------------- sw/airborne/mainloop.c | 105 ++++++++++---------- 5 files changed, 159 insertions(+), 149 deletions(-) create mode 100644 sw/airborne/main.c diff --git a/Doxyfile b/Doxyfile index 6510743f83..5747c8fe8b 100644 --- a/Doxyfile +++ b/Doxyfile @@ -450,7 +450,7 @@ WARN_LOGFILE = # directories like "/usr/src/myproject". Separate the files or directories # with spaces. -INPUT = sw/airborne/autopilot sw/airborne/fly_by_wire var/include var/Twin1 sw/airborne/avr sw/airborne +INPUT = var/include var/Twin1 sw/airborne/avr sw/airborne # If the value of the INPUT tag contains directories, you can use the # FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp diff --git a/conf/autopilot/twin_avr.makefile b/conf/autopilot/twin_avr.makefile index d91c4e072e..5fa626928a 100644 --- a/conf/autopilot/twin_avr.makefile +++ b/conf/autopilot/twin_avr.makefile @@ -8,7 +8,8 @@ ap.LOW_FUSE = a0 ap.HIGH_FUSE = 99 ap.EXT_FUSE = ff ap.LOCK_FUSE = ff -ap.srcs = main_ap.c $(SRC_ARCH)/modem.c link_mcu.c $(SRC_ARCH)/link_mcu_ap.c $(SRC_ARCH)/spi_ap.c $(SRC_ARCH)/adc_ap.c gps_ubx.c infrared.c pid.c nav.c $(SRC_ARCH)/uart_ap.c estimator.c if_calib.c mainloop.c cam.c +ap.CFLAGS += -DAP +ap.srcs = main.c main_ap.c $(SRC_ARCH)/modem.c link_mcu.c $(SRC_ARCH)/link_mcu_ap.c $(SRC_ARCH)/spi_ap.c $(SRC_ARCH)/adc_ap.c gps_ubx.c infrared.c pid.c nav.c $(SRC_ARCH)/uart_ap.c estimator.c if_calib.c mainloop.c cam.c fbw.ARCHDIR = $(ARCHI) fbw.ARCH = atmega8 @@ -18,4 +19,5 @@ fbw.LOW_FUSE = 2e fbw.HIGH_FUSE = cb fbw.EXT_FUSE = ff fbw.LOCK_FUSE = ff -fbw.srcs = main_fbw.c $(SRC_ARCH)/ppm.c $(SRC_ARCH)/servo.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/uart_fbw.c $(SRC_ARCH)/adc_fbw.c +fbw.CFLAGS += -DFBW +fbw.srcs = main.c main_fbw.c $(SRC_ARCH)/ppm.c $(SRC_ARCH)/servo.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/uart_fbw.c $(SRC_ARCH)/adc_fbw.c diff --git a/sw/airborne/main.c b/sw/airborne/main.c new file mode 100644 index 0000000000..347cbdeff3 --- /dev/null +++ b/sw/airborne/main.c @@ -0,0 +1,32 @@ +#ifdef FBW +extern void init_fbw( void ); +extern void periodic_task_fbw( void ); +extern void event_task_fbw( void ); + +#define Fbw(f) f ## _fbw() +#else +#define Fbw(f) +#endif + +#ifdef AP +extern void init_ap( void ); +extern void periodic_task_ap( void ); +extern void event_task_ap( void ); + +#define Ap(f) f ## _ap() +#else +#define Ap(f) +#endif + +int main( void ) __attribute__ ((noreturn)); + +int main( void ) { + Fbw(init); + Ap(init); + while (1) { + Fbw(periodic_task); + Ap(periodic_task); + Fbw(event_task); + Ap(event_task); + } +} diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c index dbe5bbabee..7d18047a71 100644 --- a/sw/airborne/main_fbw.c +++ b/sw/airborne/main_fbw.c @@ -68,7 +68,7 @@ static uint8_t ppm_cpt, last_ppm_cpt; static inline void to_autopilot_from_last_radio (void) { uint8_t i; for(i = 0; i < RADIO_CTL_NB; i++) - to_mega128.channels[i] = last_radio[i]; + to_mega128.channels[i] = last_radio[i]; to_mega128.status = (radio_ok ? _BV(STATUS_RADIO_OK) : 0); to_mega128.status |= (radio_really_lost ? _BV(RADIO_REALLY_LOST) : 0); to_mega128.status |= (mode == MODE_AUTO ? _BV(STATUS_MODE_AUTO) : 0); @@ -129,7 +129,7 @@ inline void spi_task(void) { #if defined IMU_ANALOG || defined IMU_3DMG control_set_desired(from_mega128.channels); #else - command_set(from_mega128.channels); + command_set(from_mega128.channels); #endif } } @@ -142,8 +142,7 @@ inline void spi_task(void) { // for compatibility #endif -int main( void ) -{ +void init_fbw( void ) { { uint8_t foo1 = 25; while (foo1--) { @@ -175,92 +174,76 @@ int main( void ) #warning IMU_RESET_ON_BOOT imu_capture_neutral(); #endif - - while( 1 ) { - if( ppm_valid ) { - ppm_valid = FALSE; - radio_control_task(); - } - else if (mode == MODE_MANUAL && radio_really_lost) { - mode = MODE_AUTO; - } - if ( !SpiIsSelected() && spi_was_interrupted ) { - spi_was_interrupted = FALSE; - spi_task(); - } -#ifdef IMU_3DMG - if (_3dmg_data_ready) { - imu_update(); - } -#endif - if (time_since_last_ppm >= STALLED_TIME) { - radio_ok = FALSE; - } - if (time_since_last_ppm >= REALLY_STALLED_TIME) { - radio_really_lost = TRUE; - } - if (time_since_last_mega128 == STALLED_TIME) { - mega128_ok = FALSE; - } - - failsafe_mode = FALSE; - if ((mode == MODE_MANUAL && !radio_ok) || - (mode == MODE_AUTO && !mega128_ok)) { - failsafe_mode = TRUE; - command_set(failsafe); - } - - if(timer_periodic()) { - static uint8_t _1Hz; - static uint8_t _20Hz; - _1Hz++; - _20Hz++; -#if defined IMU_ANALOG - imu_update(); -#if 0 - { - static uint8_t foo = 0; - foo++; - if (!(foo%10)) { - uart0_print_hex16(roll_dot); - uart0_transmit(','); - uart0_print_hex16(pitch_dot); - uart0_transmit(','); - uart0_print_hex16(yaw_dot); - uart0_transmit('\n'); - } - } -#endif /* 0 */ -#endif -#if defined IMU_3DMG || defined IMU_ANALOG - control_run(); - if (radio_ok) { - if (last_radio[RADIO_THROTTLE] > 0.1*MAX_PPRZ) { - command_set(control_commands); - } - else { - command_set(failsafe); - } - } -#endif - if (_1Hz >= 60) { - _1Hz = 0; - last_ppm_cpt = ppm_cpt; - ppm_cpt = 0; - } - if (_20Hz >= 3) { - _20Hz = 0; -#ifndef IMU_3DMG -/* servo_transmit(); */ -#endif - } - if (time_since_last_mega128 < STALLED_TIME) - time_since_last_mega128++; - if (time_since_last_ppm < REALLY_STALLED_TIME) - time_since_last_ppm++; - } - } - return 0; } +void event_task_fbw( void) { + if( ppm_valid ) { + ppm_valid = FALSE; + radio_control_task(); + } + else if (mode == MODE_MANUAL && radio_really_lost) { + mode = MODE_AUTO; + } + if ( !SpiIsSelected() && spi_was_interrupted ) { + spi_was_interrupted = FALSE; + spi_task(); + } +#ifdef IMU_3DMG + if (_3dmg_data_ready) { + imu_update(); + } +#endif + if (time_since_last_ppm >= STALLED_TIME) { + radio_ok = FALSE; + } + if (time_since_last_ppm >= REALLY_STALLED_TIME) { + radio_really_lost = TRUE; + } + if (time_since_last_mega128 == STALLED_TIME) { + mega128_ok = FALSE; + } + + failsafe_mode = FALSE; + if ((mode == MODE_MANUAL && !radio_ok) || + (mode == MODE_AUTO && !mega128_ok)) { + failsafe_mode = TRUE; + command_set(failsafe); + } +} + +void periodic_task_fbw( void ) { + static uint8_t _1Hz; + static uint8_t _20Hz; + _1Hz++; + _20Hz++; +#if defined IMU_ANALOG + imu_update(); +#endif +#if defined IMU_3DMG || defined IMU_ANALOG + control_run(); + if (radio_ok) { + if (last_radio[RADIO_THROTTLE] > 0.1*MAX_PPRZ) { + command_set(control_commands); + } + else { + command_set(failsafe); + } + } +#endif + if (_1Hz >= 60) { + _1Hz = 0; + last_ppm_cpt = ppm_cpt; + ppm_cpt = 0; + } + if (_20Hz >= 3) { + _20Hz = 0; +#ifndef IMU_3DMG + /* servo_transmit(); */ +#endif + } + if (time_since_last_mega128 < STALLED_TIME) + time_since_last_mega128++; + if (time_since_last_ppm < REALLY_STALLED_TIME) + time_since_last_ppm++; +} diff --git a/sw/airborne/mainloop.c b/sw/airborne/mainloop.c index 396d2d9b30..88e873aa38 100644 --- a/sw/airborne/mainloop.c +++ b/sw/airborne/mainloop.c @@ -24,12 +24,8 @@ * \brief Main loop used in the autopilot microcontroler */ -#include "std.h" - #include "timer_ap.h" -#include "modem.h" #include "adc_ap.h" -#include "airframe.h" #include "autopilot.h" #include "spi_ap.h" #include "link_mcu_ap.h" @@ -38,7 +34,6 @@ #include "infrared.h" #include "estimator.h" #include "downlink.h" -#include "uart_ap.h" #include "datalink.h" #include "wavecard.h" @@ -51,10 +46,7 @@ #endif // AHRS -/** \fn int main( void ) - * \brief Main and @@@@@ unique @@@@@ function \n - */ -int main( void ) { +void init_ap( void ) { /** - init peripherals: * - \a timer * - \a modem @@ -113,68 +105,70 @@ int main( void ) { * - receive radio control task from fbw and use it with * \a radio_control_task */ - - while( 1 ) { - if(timer_periodic()) - /* do periodic task */ - periodic_task(); - if (gps_msg_received) { - /* parse and use GPS messages */ - parse_gps_msg(); - gps_msg_received = FALSE; - if (gps_pos_available) { - use_gps_pos(); - gps_pos_available = FALSE; - } +} + +void periodic_task_ap( void) { + periodic_task(); +} + +void event_task_ap( void ) { + if (gps_msg_received) { + /* parse and use GPS messages */ + parse_gps_msg(); + gps_msg_received = FALSE; + if (gps_pos_available) { + use_gps_pos(); + gps_pos_available = FALSE; } + } #ifdef WAVECARD - if (wc_msg_received) { - wc_parse_payload(); - wc_msg_received = FALSE; - } + if (wc_msg_received) { + wc_parse_payload(); + wc_msg_received = FALSE; + } #endif #ifdef DATALINK - if (dl_msg_available) { - dl_parse_msg(); - dl_msg_available = FALSE; - } + if (dl_msg_available) { + dl_parse_msg(); + dl_msg_available = FALSE; + } #endif #ifdef TELEMETER - /** Handling of data sent by the device (initiated by srf08_receive() */ - if (srf08_received) { - srf08_received = FALSE; - srf08_read(); - } - if (srf08_got) { - srf08_got = FALSE; - srf08_copy(); - DOWNLINK_SEND_RANGEFINDER(&srf08_range); - } + /** Handling of data sent by the device (initiated by srf08_receive() */ + if (srf08_received) { + srf08_received = FALSE; + srf08_read(); + } + if (srf08_got) { + srf08_got = FALSE; + srf08_copy(); + DOWNLINK_SEND_RANGEFINDER(&srf08_range); + } #endif - if (link_fbw_receive_complete) { - /* receive radio control task from fbw */ - link_fbw_receive_complete = FALSE; - radio_control_task(); + if (link_fbw_receive_complete) { + /* receive radio control task from fbw */ + link_fbw_receive_complete = FALSE; + radio_control_task(); #ifdef IMU_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(); + 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(); #elif defined IMU_ANALOG - /** -Saving now the pqr values from the fbw struct since + /** -Saving now the pqr values from the fbw struct since * -it's not safe always * only if gyro are connected to fbw */ #if defined AHRS && ((!defined IMU_GYROS_CONNECTED_TO_AP) || (!IMU_GYROS_CONNECTED_TO_AP)) - /* it can be called at 20 hz and gyros data come from the fbw so call have to be here */ - ahrs_gyro_update(); + /* it can be called at 20 hz and gyros data come from the fbw so call have to be here */ + ahrs_gyro_update(); #endif //!IMU_GYROS_CONNECTED_TO_AP - int16_t dummy; - // DOWNLINK_SEND_IMU_3DMG(&from_fbw.euler_dot[0], &from_fbw.euler_dot[1], &from_fbw.euler_dot[2], &dummy, &dummy, &dummy); + int16_t dummy; + // DOWNLINK_SEND_IMU_3DMG(&from_fbw.euler_dot[0], &from_fbw.euler_dot[1], &from_fbw.euler_dot[2], &dummy, &dummy, &dummy); #endif //IMU #if defined IMU_3DMG || defined IMU_ANALOG - /*uart0_transmit('G'); + /*uart0_transmit('G'); uart0_transmit(' '); uart0_print_hex16(from_fbw.euler_dot[0]); uart0_transmit(','); @@ -183,10 +177,9 @@ int main( void ) { uart0_print_hex16(from_fbw.euler_dot[2]); uart0_transmit('\n');*/ #endif - } - } - return 0; -} + } +} +