From f3ed3356401723e62574f2dbdd7d2de473edf81b Mon Sep 17 00:00:00 2001 From: podhrmic Date: Mon, 25 Nov 2013 18:19:06 -0700 Subject: [PATCH] [rt_paparazzi] cleanup Based on @flixr's comments 1) [i2c|uart|spi]_pprzi.c renamed to _pprz.c 2) fixed doxygen comments, cleaned up code style 3) cleaned ms5611 periph to be more better integrated with ChibiOS 4) ImuEvent moved into main_chibios.c 5) main_chibios.c cleaned 6) supressed warnings for unused arguments in arch functions 7) fixed callback for threads (see example with GPS thread) 8) uart_receive_x macro changed into a function (look at GPS thread) 9) defined SPI_VOLATILE qualifier for SPI volatile buffers --- conf/firmwares/rotorcraft_rt.makefile | 5 +- .../subsystems/shared/spi_master.makefile | 2 +- sw/airborne/arch/chibios/STM32F107xC.ld | 10 +- sw/airborne/arch/chibios/led_hw.h | 5 +- sw/airborne/arch/chibios/mcu_arch.c | 8 +- sw/airborne/arch/chibios/mcu_arch.h | 8 +- .../arch/chibios/mcu_periph/adc_arch.h | 4 +- .../arch/chibios/mcu_periph/gpio_arch.c | 6 +- .../arch/chibios/mcu_periph/gpio_arch.h | 6 +- .../arch/chibios/mcu_periph/i2c_arch.c | 67 ++- .../arch/chibios/mcu_periph/spi_arch.c | 113 ++--- .../arch/chibios/mcu_periph/spi_arch.h | 6 +- .../arch/chibios/mcu_periph/sys_time_arch.c | 12 +- .../arch/chibios/mcu_periph/sys_time_arch.h | 21 +- .../arch/chibios/mcu_periph/uart_arch.c | 75 ++- .../arch/chibios/mcu_periph/uart_arch.h | 39 +- .../subsystems/actuators/actuators_pwm_arch.c | 4 +- .../subsystems/radio_control/ppm_arch.h | 4 +- sw/airborne/boards/baro_board_ms5611_spi.c | 29 +- .../firmwares/rotorcraft/main_chibios.c | 440 ++++++++++-------- .../mcu_periph/{i2c_pprzi.c => i2c_pprz.c} | 2 +- sw/airborne/mcu_periph/spi.h | 16 +- .../mcu_periph/{spi_pprzi.c => spi_pprz.c} | 2 +- sw/airborne/mcu_periph/uart.h | 2 + .../mcu_periph/{uart_pprzi.c => uart_pprz.c} | 2 + sw/airborne/peripherals/mpu60x0_spi.h | 9 +- sw/airborne/peripherals/ms5611_spi.c | 101 ++-- sw/airborne/peripherals/ms5611_spi.h | 15 +- sw/airborne/subsystems/gps.h | 1 - sw/airborne/subsystems/gps/gps_ubx.c | 13 +- .../subsystems/imu/imu_aspirin_2_spi.c | 4 - .../subsystems/imu/imu_aspirin_2_spi.h | 9 +- sw/airborne/subsystems/imu/imu_gx3.c | 12 +- 33 files changed, 555 insertions(+), 497 deletions(-) rename sw/airborne/mcu_periph/{i2c_pprzi.c => i2c_pprz.c} (98%) rename sw/airborne/mcu_periph/{spi_pprzi.c => spi_pprz.c} (98%) rename sw/airborne/mcu_periph/{uart_pprzi.c => uart_pprz.c} (98%) diff --git a/conf/firmwares/rotorcraft_rt.makefile b/conf/firmwares/rotorcraft_rt.makefile index ce31b7c5c42..4a82d3eacc4 100644 --- a/conf/firmwares/rotorcraft_rt.makefile +++ b/conf/firmwares/rotorcraft_rt.makefile @@ -43,7 +43,6 @@ ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD) ap.ARCHDIR = $(ARCH) #we are using normal rotorcraft firmware, just different arch -$(TARGET).CFLAGS += -DFIRMWARE=ROTORCRAFT $(TARGET).CFLAGS += -DUSE_CHIBIOS_RTOS ap.CFLAGS += $(ROTORCRAFT_INC) ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT @@ -91,12 +90,12 @@ endif ap.srcs += subsystems/settings.c #ap.srcs += mcu_periph/uart.c -> two file problem, see: http://www.cplusplus.com/forum/general/35718/ -ap.srcs += mcu_periph/uart_pprzi.c +ap.srcs += mcu_periph/uart_pprz.c ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # I2C ifeq ($(TARGET), ap) - $(TARGET).srcs += mcu_periph/i2c_pprzi.c + $(TARGET).srcs += mcu_periph/i2c_pprz.c $(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c endif diff --git a/conf/firmwares/subsystems/shared/spi_master.makefile b/conf/firmwares/subsystems/shared/spi_master.makefile index b0e2f95674c..1a2e1cb0d6c 100644 --- a/conf/firmwares/subsystems/shared/spi_master.makefile +++ b/conf/firmwares/subsystems/shared/spi_master.makefile @@ -6,7 +6,7 @@ SPI_INCLUDED = 1 #generic spi master driver SPI_CFLAGS = -DUSE_SPI -DSPI_MASTER -SPI_SRCS = mcu_periph/spi_pprzi.c $(SRC_ARCH)/mcu_periph/spi_arch.c +SPI_SRCS = mcu_periph/spi_pprz.c $(SRC_ARCH)/mcu_periph/spi_arch.c ap.CFLAGS += $(SPI_CFLAGS) ap.srcs += $(SPI_SRCS) diff --git a/sw/airborne/arch/chibios/STM32F107xC.ld b/sw/airborne/arch/chibios/STM32F107xC.ld index a735ac5f13c..8b4f4983153 100644 --- a/sw/airborne/arch/chibios/STM32F107xC.ld +++ b/sw/airborne/arch/chibios/STM32F107xC.ld @@ -25,11 +25,15 @@ for full details of how and when the exception can be applied. */ -/* - * ST32F107xC memory setup. +/** + * @file arch/chibios/STM32F107xC.ld + * ST32F107xC memory setup and linker script + * * Modified by Michal Podhradsky, 2013 - * Flash addr is shifted by 8kb (luftboot) + * Flash address is shifted by 8kb (Luftboot) * Note: probably not the cleanest solution, but it works + * + * __process_stack_size__ determines stack size of main() thread */ __main_stack_size__ = 0x0400; __process_stack_size__ = 0x0400; diff --git a/sw/airborne/arch/chibios/led_hw.h b/sw/airborne/arch/chibios/led_hw.h index 7a9f2c20018..652159db2bf 100644 --- a/sw/airborne/arch/chibios/led_hw.h +++ b/sw/airborne/arch/chibios/led_hw.h @@ -24,7 +24,8 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant led manipulation macros + * @file arch/chibios/led_hw.h + * Led macro implementation for ChibiOS arch */ #ifndef LED_HW_H #define LED_HW_H @@ -35,7 +36,7 @@ #include "hal.h" /* - * Regular GPIO driven LEDs + * Regular GPIO driven LEDs */ #define _LED_GPIO(i) i #define _LED_GPIO_PIN(i) i diff --git a/sw/airborne/arch/chibios/mcu_arch.c b/sw/airborne/arch/chibios/mcu_arch.c index b937d9cdd4f..65b686560ae 100644 --- a/sw/airborne/arch/chibios/mcu_arch.c +++ b/sw/airborne/arch/chibios/mcu_arch.c @@ -24,9 +24,11 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant microcontroller initialisation functions. - * @details Mostly empty defines, because Chibios takes - * care of mcu initialization + * @file arch/chibios/mcu_arch.c + * Microcontroller initialization function for ChibiOS + * + * ChibiOS initialized peripherals by itself, only interrupt + * vector has to be relocated for Luftboot */ #include "mcu.h" #include "ch.h" diff --git a/sw/airborne/arch/chibios/mcu_arch.h b/sw/airborne/arch/chibios/mcu_arch.h index cd5ad00b131..16552d270c7 100644 --- a/sw/airborne/arch/chibios/mcu_arch.h +++ b/sw/airborne/arch/chibios/mcu_arch.h @@ -24,9 +24,11 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant microcontroller initialisation functions. - * @details Mostly empty defines, because Chibios takes - * care of mcu initialization + * @file arch/chibios/mcu_arch.h + * Microcontroller initialization function for ChibiOS + * + * ChibiOS initialized peripherals by itself, hence empty + * functions for Paparazzi compatibility. */ #ifndef CHIBIOS_MCU_ARCH_H #define CHIBIOS_MCU_ARCH_H diff --git a/sw/airborne/arch/chibios/mcu_periph/adc_arch.h b/sw/airborne/arch/chibios/mcu_periph/adc_arch.h index 53a3e0b4481..75aee1d8a9b 100644 --- a/sw/airborne/arch/chibios/mcu_periph/adc_arch.h +++ b/sw/airborne/arch/chibios/mcu_periph/adc_arch.h @@ -37,11 +37,11 @@ #include "hal.h" -/// ADC error flags +// ADC error flags extern uint8_t adc_error_flag; extern ADCDriver* adcp_err; -/// NB_ADCx_CHANNELS +// NB_ADCx_CHANNELS enum adc1_channels { #if USE_AD1_1 ADC1_C1, diff --git a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c index 6233d0a8373..8ffe8ccacb8 100644 --- a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c @@ -24,9 +24,7 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant gpio functions - * @details In Chibios palSet/Clear/Toggle(port, pin) - * replaces gpio functions - * + * @file arch/chibios/mcu_periph/gpio_arch.c + * gpio functions implemented for ChibiOS arch */ #include "mcu_periph/gpio.h" diff --git a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h index 6733caf33b1..87f1a0d34ff 100644 --- a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h +++ b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h @@ -24,10 +24,8 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant gpio functions - * @details In Chibios palSet/Clear/Toggle(port, pin) - * replaces gpio functions - * + * @file arch/chibios/mcu_periph/gpio_arch.h + * gpio functions implemented for ChibiOS arch */ #ifndef GPIO_ARCH_H #define GPIO_ARCH_H diff --git a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c index 50635b8d260..a48d8e0b74b 100644 --- a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c @@ -89,10 +89,7 @@ void i2c_event(void){} * Empty, for paparazzi compatibility only. Bitrate is already * set in i2cX_hw_init() */ -void i2c_setbitrate(struct i2c_periph* p, int bitrate){ - (void) p; - (void) bitrate; -} +void i2c_setbitrate(struct i2c_periph* p __attribute__((unused)), int bitrate __attribute__((unused))){} /** * i2c_submit() function @@ -104,6 +101,10 @@ void i2c_setbitrate(struct i2c_periph* p, int bitrate){ * Note that we are using the same buffer for transmit and recevive. It is * OK because in i2c transaction is Tx always before Rx. * + * I2C calls are synchronous, timeout is set to 1/PERIODIC_FREQUENCY seconds + * TODO: Note that on STM32F1xx such as Lia board I2C bus can easily hang in + * an interrupt (see issue #531). Use I2C bus with care and caution. + * * @param[in] p pointer to a @p i2c_periph struct * @param[in] t pointer to a @p i2c_transaction struct */ @@ -117,42 +118,41 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t){ i2cReleaseBus((I2CDriver*)p->reg_addr); if (status != RDY_OK) { - t->status = I2CTransFailed; - static i2cflags_t errors = 0; - errors = i2cGetErrors((I2CDriver*)p->reg_addr); - if (errors & I2CD_BUS_ERROR) { - p->errors->miss_start_stop_cnt++; - } - if (errors & I2CD_ARBITRATION_LOST) { - p->errors->arb_lost_cnt++; - } - if (errors & I2CD_ACK_FAILURE) { - p->errors->ack_fail_cnt++; - } - if (errors & I2CD_OVERRUN) { - p->errors->over_under_cnt++; - } - if (errors & I2CD_PEC_ERROR) { - p->errors->pec_recep_cnt++; - } - if (errors & I2CD_TIMEOUT) { - p->errors->timeout_tlow_cnt++; - } - if (errors & I2CD_SMB_ALERT) { - p->errors->smbus_alert_cnt++; - } - return FALSE; + t->status = I2CTransFailed; + static i2cflags_t errors = 0; + errors = i2cGetErrors((I2CDriver*)p->reg_addr); + if (errors & I2CD_BUS_ERROR) { + p->errors->miss_start_stop_cnt++; + } + if (errors & I2CD_ARBITRATION_LOST) { + p->errors->arb_lost_cnt++; + } + if (errors & I2CD_ACK_FAILURE) { + p->errors->ack_fail_cnt++; + } + if (errors & I2CD_OVERRUN) { + p->errors->over_under_cnt++; + } + if (errors & I2CD_PEC_ERROR) { + p->errors->pec_recep_cnt++; + } + if (errors & I2CD_TIMEOUT) { + p->errors->timeout_tlow_cnt++; + } + if (errors & I2CD_SMB_ALERT) { + p->errors->smbus_alert_cnt++; + } + return FALSE; } else { - t->status = I2CTransSuccess; - return TRUE; + t->status = I2CTransSuccess; + return TRUE; } #else (void) p; (void) t; return FALSE; #endif - } /** @@ -160,7 +160,6 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t){ * * Empty, for paparazzi compatibility only */ -bool_t i2c_idle(struct i2c_periph* p){ - (void) p; +bool_t i2c_idle(struct i2c_periph* p __attribute__((unused))){ return FALSE; } diff --git a/sw/airborne/arch/chibios/mcu_periph/spi_arch.c b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c index e5f25cab2f2..5a0867dc9ea 100644 --- a/sw/airborne/arch/chibios/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c @@ -23,8 +23,10 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant implementation of SPI interface - * @note Assume SPI master for now + * @file arch/chibios/mcu_periph/spi_arch.c + * Implementation of SPI interface for ChibiOS arch + * + * Only Master mode is allowed in ChibiOS. */ #include "mcu_periph/spi.h" @@ -98,7 +100,7 @@ static inline ioportid_t spi_resolve_slave_port(uint8_t slave) { } /** - * Resolve slave port + * Resolve slave pin * * Given the slave number and the board config file, returns the right * pin (i.e. 12) @@ -149,12 +151,13 @@ static inline uint16_t spi_resolve_slave_pin(uint8_t slave) { * Given the transaction settings, returns the right configuration of * SPIx_CR1 register. * + * This function is currently architecture dependent (for STM32F1xx only) + * TODO: implement for STM32F4 and possible other architectures too + * * @param[in] t pointer to a @p spi_transaction struct */ static inline uint16_t spi_resolve_CR1(struct spi_transaction* t){ uint16_t CR1 = 0; - /// The settings are architecture dependent - /// TODO: Now for STM32F1xx only #ifdef __STM32F10x_H if (t->dss == SPIDss16bit) { CR1 |= SPI_CR1_DFF; @@ -169,35 +172,35 @@ static inline uint16_t spi_resolve_CR1(struct spi_transaction* t){ CR1 |= SPI_CR1_CPOL; } - switch (t->cdiv) { - case SPIDiv2://000 - break; - case SPIDiv4://001 - CR1 |= SPI_CR1_BR_0; - break; - case SPIDiv8://010 - CR1 |= SPI_CR1_BR_1; - break; - case SPIDiv16://011 - CR1 |= SPI_CR1_BR_1 | SPI_CR1_BR_0; - break; - case SPIDiv32://100 - CR1 |= SPI_CR1_BR_2; - break; - case SPIDiv64://101 - CR1 |= SPI_CR1_BR_2 | SPI_CR1_BR_0; - break; - case SPIDiv128://110 - CR1 |= SPI_CR1_BR_2 | SPI_CR1_BR_1; - break; - case SPIDiv256://111 - CR1 |= SPI_CR1_BR; - break; - default: - break; - } + switch (t->cdiv) { + case SPIDiv2://000 + break; + case SPIDiv4://001 + CR1 |= SPI_CR1_BR_0; + break; + case SPIDiv8://010 + CR1 |= SPI_CR1_BR_1; + break; + case SPIDiv16://011 + CR1 |= SPI_CR1_BR_1 | SPI_CR1_BR_0; + break; + case SPIDiv32://100 + CR1 |= SPI_CR1_BR_2; + break; + case SPIDiv64://101 + CR1 |= SPI_CR1_BR_2 | SPI_CR1_BR_0; + break; + case SPIDiv128://110 + CR1 |= SPI_CR1_BR_2 | SPI_CR1_BR_1; + break; + case SPIDiv256://111 + CR1 |= SPI_CR1_BR; + break; + default: + break; + } #endif - return CR1; + return CR1; } @@ -209,7 +212,11 @@ static inline uint16_t spi_resolve_CR1(struct spi_transaction* t){ * callbacks are called accordingly. * * ChibiOS doesn't provide error checking for the SPI transactions, - * since all spi functions are return void. The API is asynchronous. + * since all spi functions are return void. The SPI transaction is + * synchronous, so we always assume success if the transaction finishes. + * + * There is no explicit timeout on SPI transaction. + * TODO: Timeout on SPI trans and error detection. * * @param[in] p pointer to a @p spi_periph struct * @param[in] t pointer to a @p spi_transaction struct @@ -217,7 +224,7 @@ static inline uint16_t spi_resolve_CR1(struct spi_transaction* t){ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) { SPIConfig spi_cfg = { - NULL, /// no callback + NULL, // no callback spi_resolve_slave_port(t->slave_idx), spi_resolve_slave_pin(t->slave_idx), spi_resolve_CR1(t) @@ -235,32 +242,34 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) t_length = (size_t)t->output_length; } - /// Acquire exclusive access to the spi bus + // Acquire exclusive access to the SPI bus spiAcquireBus((SPIDriver*)p->reg_addr); - /// Configure spi bus with the current slave select pin + // Configure SPI bus with the current slave select pin spiStart((SPIDriver*)p->reg_addr, &spi_cfg); spiSelect((SPIDriver*)p->reg_addr); - /// Run the callback AFTER selecting the slave + // Run the callback after selecting the slave if (t->before_cb != 0) { t->before_cb(t); } - /// Start asynchronous data transfer + // Start synchronous data transfer spiExchange((SPIDriver*)p->reg_addr, t_length, t->output_buf, t->input_buf); + // Unselect the slave spiUnselect((SPIDriver*)p->reg_addr); - /// Release the exclusive access to the bus + // Release the exclusive access to the bus spiReleaseBus((SPIDriver*)p->reg_addr); - - /// Report the transaction as success + // Report the transaction as success t->status = SPITransSuccess; - /// Run the callback AFTER deselecting the slave - /// to avoid recursion and/or concurency over the bus + /* + * Run the callback after deselecting the slave + * to avoid recursion and/or concurency over the bus + */ if (t->after_cb != 0) { t->after_cb(t); } @@ -274,27 +283,21 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) * * Empty, for paparazzi compatibility only */ -void spi_slave_select(uint8_t slave) { - (void) slave; -} +void spi_slave_select(uint8_t slave __attribute__((unused))) {} /** * spi_slave_unselect() function * * Empty, for paparazzi compatibility only */ -void spi_slave_unselect(uint8_t slave) { - (void) slave; -} +void spi_slave_unselect(uint8_t slave __attribute__((unused))) {} /** * spi_lock() function * * Empty, for paparazzi compatibility only */ -bool_t spi_lock(struct spi_periph* p, uint8_t slave) { - (void) slave; - (void) p; +bool_t spi_lock(struct spi_periph* p __attribute__((unused)), uint8_t slave __attribute__((unused))) { return TRUE; } @@ -303,9 +306,7 @@ bool_t spi_lock(struct spi_periph* p, uint8_t slave) { * * Empty, for paparazzi compatibility only */ -bool_t spi_resume(struct spi_periph* p, uint8_t slave) { - (void) slave; - (void) p; +bool_t spi_resume(struct spi_periph* p __attribute__((unused)), uint8_t slave __attribute__((unused))) { return TRUE; } diff --git a/sw/airborne/arch/chibios/mcu_periph/spi_arch.h b/sw/airborne/arch/chibios/mcu_periph/spi_arch.h index 395a3f9032e..bfae4b9aa6b 100644 --- a/sw/airborne/arch/chibios/mcu_periph/spi_arch.h +++ b/sw/airborne/arch/chibios/mcu_periph/spi_arch.h @@ -23,10 +23,10 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant implementation of SPI interface + * @file arch/chibios/mcu_periph/spi_arch.h + * Implementation of SPI interface for ChibiOS arch */ #ifndef SPI_ARCH_H #define SPI_ARCH_H - -#endif // SPI_ARCH_H +#endif /* SPI_ARCH_H */ diff --git a/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c b/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c index 636f4e13a51..697530179d7 100644 --- a/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c @@ -24,13 +24,17 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant implementation of sys time functions - * @note Partially implemented (no CPU monitor), some extra variables - * for monitoring number of threads and free memory. + * @file arch/chibios/mcu_periph/sys_time_arch.c + * Implementation of system time functions for ChibiOS arch * + * Mostly empty functions for Paparazzi compatibility, + * since ChibiOS uses different system time functions. */ -#include "mcu_periph/sys_time.h" +#include "mcu_periph/sys_time_arch.h" +/* + * Extra defines for ChibiOS CPU monitoring + */ uint32_t core_free_memory; uint8_t thread_counter; uint32_t cpu_counter; diff --git a/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.h b/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.h index 9e35d63ca49..fb5c991001f 100644 --- a/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.h @@ -24,16 +24,20 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant implementation of sys time functions - * @note Partially implemented (no CPU monitor), some extra variables - * for monitoring number of threads and free memory. + * @file arch/chibios/mcu_periph/sys_time_arch.h + * Implementation of system time functions for ChibiOS arch * + * Mostly empty functions for Paparazzi compatibility, + * since ChibiOS uses different system time functions. */ #ifndef SYS_TIME_ARCH_H #define SYS_TIME_ARCH_H #include "mcu_periph/sys_time.h" +/* + * Extra defines for ChibiOS CPU monitoring + */ extern uint32_t core_free_memory; extern uint8_t thread_counter; extern uint32_t cpu_counter; @@ -42,17 +46,10 @@ extern uint8_t cpu_frequency; #define SysTimeTimerStart(_t) {} -/* - * FIXME: Not implemented - */ static inline uint32_t get_sys_time_usec(void) { return 0; } -/* - * FIXME: Not implemented - */ -static inline void sys_time_usleep(uint32_t us) { - (void)us; -} +static inline void sys_time_usleep(uint32_t us __attribute__((unused))) {} + #endif /* SYS_TIME_ARCH_H */ diff --git a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c index 5032bffe78b..c6e92bb28e9 100644 --- a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c @@ -24,17 +24,15 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant implementation of uart/serial drivers - * @details Partially implemented (no error callbacks yet). Since ChibiOs - * has SerialDriver, it is better to use that one instead of Uart - * drivers, although it makes backward compatibility more difficutl. - * DMA is already implemented. - * @note The peripheral settings should use config (baudrate etc) from - * makefiles, some makro should be used. + * @file arch/chibios/mcu_periph/uart_arch.c + * UART/Serial driver implementation for ChibiOS arch + * + * ChibiOS has a high level Serial Driver, for Paparazzi it is more convenient + * than pure UART driver (which needs callbacks etc.). This implementation is + * asynchronous and the RX thread has to use event flags. See ChibiOS documen- + * tation. */ -#include "mcu_periph/uart.h" -#include "hal.h" - +#include "mcu_periph/uart_arch.h" #ifdef USE_UART1 static const SerialConfig usart1_config = @@ -107,34 +105,28 @@ void uart5_init(void) { } #endif -/* +/** * Set baudrate (from the serialConfig) * @note Baudrate is set in sdStart, no need for implementation */ -void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) { - (void) p; - (void) baud; -} +void uart_periph_set_baudrate(struct uart_periph* p __attribute__((unused)), uint32_t baud __attribute__((unused))) {} -/* +/** * Set mode (not necessary, or can be set by SerialConfig) */ -void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) { - (void) p; - (void) tx_enabled; - (void) rx_enabled; - (void) hw_flow_control; -} +void uart_periph_set_mode(struct uart_periph* p __attribute__((unused)), bool_t tx_enabled __attribute__((unused)), + bool_t rx_enabled __attribute__((unused)), bool_t hw_flow_control __attribute__((unused))) {} -/* +/** * Uart transmit implementation */ void uart_transmit(struct uart_periph* p, uint8_t data ) { sdWrite((SerialDriver*)p->reg_addr, &data, sizeof(data)); } -/* -* Uart transmit buffer implementation +/** +* Uart/SerialDriver transmit buffer implementation +* * Typical use: * uint8_t tx_switch[10] = { 0x01, 0x08, 0x12, 0x34, 0x56, 0x78, 0x12, 0x34, '\r' }; * uart_transmit_buffer(&uart2, tx_switch, sizeof(tx_switch)); @@ -142,3 +134,36 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) { void uart_transmit_buffer(struct uart_periph* p, uint8_t* data_buffer, size_t length) { sdWrite((SerialDriver*)p->reg_addr, data_buffer, length); } + +/** + * Uart/SerialDriver receive loop implementation + * + * @param[in] p pointer to a @p uart_periph object + * @param[in] flags flagmask for SD event flags + * @param[in] on_receive_callback pointer to a callback function + */ +void uart_receive_buffer(struct uart_periph* p, flagsmask_t flags, void *on_receive_callback){ + if ((flags & (SD_FRAMING_ERROR | SD_OVERRUN_ERROR | SD_NOISE_ERROR)) != 0) { + if (flags & SD_OVERRUN_ERROR) { + p->ore++; + } + if (flags & SD_NOISE_ERROR) { + p->ne_err++; + } + if (flags & SD_FRAMING_ERROR) { + p->fe_err++; + } + } + if (flags & CHN_INPUT_AVAILABLE) { + msg_t charbuf; + do { + charbuf = sdGetTimeout((SerialDriver*)p->reg_addr, TIME_IMMEDIATE); + if ( charbuf != Q_TIMEOUT ) { + if (on_receive_callback != NULL) { + ((void(*)(uint8_t))on_receive_callback)((uint8_t) charbuf); + } + } + } + while (charbuf != Q_TIMEOUT); + } +} diff --git a/sw/airborne/arch/chibios/mcu_periph/uart_arch.h b/sw/airborne/arch/chibios/mcu_periph/uart_arch.h index 118d0579634..d7969545831 100644 --- a/sw/airborne/arch/chibios/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/chibios/mcu_periph/uart_arch.h @@ -24,15 +24,20 @@ * Boston, MA 02111-1307, USA. */ /** - * @brief chibios arch dependant implementation of uart/serial drivers - * @details Partially implemented (no error callbacks yet). Since ChibiOs - * has SerialDriver, it is better to use that one instead of Uart - * drivers, although it makes backward compatibility more difficutl. - * DMA is already implemented. + * @file arch/chibios/mcu_periph/uart_arch.h + * UART/Serial driver implementation for ChibiOS arch + * + * ChibiOS has a high level Serial Driver, for Paparazzi it is more convenient + * than pure UART driver (which needs callbacks etc.). This implementation is + * asynchronous and the RX thread has to use event flags. See ChibiOS documen- + * tation. */ #ifndef CHIBIOS_UART_ARCH_H #define CHIBIOS_UART_ARCH_H +#include "mcu_periph/uart.h" +#include "hal.h" + #define B1200 1200 #define B2400 2400 #define B4800 4800 @@ -70,29 +75,5 @@ } \ } -#define ch_uart_receive(_port, _flag, _callback) { \ - if ((_flag & (SD_FRAMING_ERROR | SD_OVERRUN_ERROR | \ - SD_NOISE_ERROR)) != 0) { \ - if (_flag & SD_OVERRUN_ERROR) { \ - _port.ore++; \ - } \ - if (_flag & SD_NOISE_ERROR) { \ - _port.ne_err++; \ - } \ - if (_flag & SD_FRAMING_ERROR) { \ - _port.fe_err++; \ - } \ - } \ - if (_flag & CHN_INPUT_AVAILABLE) { \ - msg_t charbuf; \ - do { \ - charbuf = sdGetTimeout((SerialDriver*)_port.reg_addr, TIME_IMMEDIATE);\ - if ( charbuf != Q_TIMEOUT ) { \ - _callback(charbuf); \ - } \ - } \ - while (charbuf != Q_TIMEOUT); \ - } \ -} #endif /* STM32_UART_ARCH_H */ \ No newline at end of file diff --git a/sw/airborne/arch/chibios/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/chibios/subsystems/actuators/actuators_pwm_arch.c index 6e4dd3fde56..c7080a47f03 100644 --- a/sw/airborne/arch/chibios/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/chibios/subsystems/actuators/actuators_pwm_arch.c @@ -57,9 +57,7 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; * * @param[in] pwmp pointer to a @p PWMDriver object */ -static void pwmpcb(PWMDriver *pwmp) { - (void)pwmp; -} +static void pwmpcb(PWMDriver *pwmp __attribute__((unused))) {} #if PWM_CONF_TIM1 static PWMConfig pwmcfg1 = PWM_CONF1_DEF; diff --git a/sw/airborne/arch/chibios/subsystems/radio_control/ppm_arch.h b/sw/airborne/arch/chibios/subsystems/radio_control/ppm_arch.h index 560143a8255..e4361643386 100644 --- a/sw/airborne/arch/chibios/subsystems/radio_control/ppm_arch.h +++ b/sw/airborne/arch/chibios/subsystems/radio_control/ppm_arch.h @@ -34,7 +34,9 @@ #include BOARD_CONFIG -/// The ppm counter desired resolution is 1/6 us. +/* + * The ppm counter desired resolution is 1/6 us. + */ #ifndef RC_PPM_TICKS_PER_USEC #error "RC_PPM_TICKS_PER_USEC not set in board.h file" #endif diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c index 558e5351971..18241984ed4 100644 --- a/sw/airborne/boards/baro_board_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -56,13 +56,30 @@ void baro_init(void) { void baro_periodic(void) { if (sys_time.nb_sec > 1) { +#ifdef USE_CHIBIOS_RTOS + ms5611_spi_synchronous_periodic_check(&bb_ms5611); + + if (bb_ms5611.data_available) { + float pressure = (float)bb_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + bb_ms5611.data_available = FALSE; +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); +#endif /* BARO_LED */ + +#if DEBUG + float ftempms = bb_ms5611.data.temperature / 100.; + float fbaroms = bb_ms5611.data.pressure / 100.; + DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, + &bb_ms5611.data.d1, &bb_ms5611.data.d2, + &fbaroms, &ftempms); +#endif /* DEBUG */ + } +#else /* call the convenience periodic that initializes the sensor and starts reading*/ ms5611_spi_periodic(&bb_ms5611); - -#ifdef USE_CHIBIOS_RTOS - baro_event(); -#endif +#endif /* USE_CHIBIOS_RTOS */ #if DEBUG if (bb_ms5611.initialized) @@ -75,15 +92,13 @@ void baro_periodic(void) { &bb_ms5611.data.c[5], &bb_ms5611.data.c[6], &bb_ms5611.data.c[7])); -#endif +#endif /* DEBUG */ } } void baro_event(void) { if (sys_time.nb_sec > 1) { -#ifndef USE_CHIBIOS_RTOS ms5611_spi_event(&bb_ms5611); -#endif if (bb_ms5611.data_available) { float pressure = (float)bb_ms5611.data.pressure; diff --git a/sw/airborne/firmwares/rotorcraft/main_chibios.c b/sw/airborne/firmwares/rotorcraft/main_chibios.c index 0a79639f413..6c4afc9c8a7 100644 --- a/sw/airborne/firmwares/rotorcraft/main_chibios.c +++ b/sw/airborne/firmwares/rotorcraft/main_chibios.c @@ -25,21 +25,22 @@ */ /** * @file main_chibios.c + * Main file for ChibiOS/RT Paparazzi rotocraft * - * @brief Main file for ChibiOS/RT Paparazzi - * @details Includes both Paparazzi and ChibiOs files - * Threads are static, the memory allocation is - * just approximate at this point. Eventually - * most of the variables should be static. + * Includes both Paparazzi and ChibiOs files, threads aare static. * * @author {Michal Podhradsky, Calvin Coopmans} */ -/// Chibios includes +/* + * Chibios includes + */ #include "ch.h" #include "hal.h" -/// Paparazzi includes +/* + * Paparazzi includes + */ #include "led.h" #include "mcu.h" @@ -86,32 +87,38 @@ #include "subsystems/abi.h" -/// Thread definitions +/* + * Thread Area Definitions + */ #define CH_THREAD_AREA_HEARTBEAT 128 #define CH_THREAD_AREA_FAILSAFE 256 #define CH_THREAD_AREA_ELECTRICAL 256 #define CH_THREAD_AREA_RADIO_CONTROL 256 #define CH_THREAD_AREA_RADIO_EVENT 512 -static inline void failsafe_check(void); +/* + * Thread Area Initialization + */ +static WORKING_AREA(wa_thd_heartbeat, CH_THREAD_AREA_HEARTBEAT); +static WORKING_AREA(wa_thd_failsafe, CH_THREAD_AREA_FAILSAFE); +static WORKING_AREA(wa_thd_electrical, CH_THREAD_AREA_ELECTRICAL); +static WORKING_AREA(wa_thd_radio_control, CH_THREAD_AREA_RADIO_CONTROL); +static WORKING_AREA(wa_thd_radio_event, CH_THREAD_AREA_RADIO_EVENT); +/* + * Static Thread Definitions + */ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg); static __attribute__((noreturn)) msg_t thd_failsafe(void *arg); static __attribute__((noreturn)) msg_t thd_electrical(void *arg); static __attribute__((noreturn)) msg_t thd_radio_control(void *arg); static __attribute__((noreturn)) msg_t thd_radio_event(void *arg); -#if USE_BARO_BOARD -#define CH_THREAD_AREA_BARO 512 -static __attribute__((noreturn)) msg_t thd_baro(void *arg); -#endif +/* + * Static Auxilliary Functions Definitions + */ +static inline void failsafe_check(void); -#ifdef DOWNLINK -#define CH_THREAD_AREA_DOWNLINK_TX 1024 -#define CH_THREAD_AREA_DOWNLINK_RX 1024 -__attribute__((noreturn)) msg_t thd_telemetry_tx(void *arg); -__attribute__((noreturn)) msg_t thd_telemetry_rx(void *arg); -#endif #if USE_GPS static WORKING_AREA(wa_thd_gps_rx, CH_THREAD_AREA_GPS_RX); @@ -122,82 +129,77 @@ static WORKING_AREA(wa_thd_gps_rx, CH_THREAD_AREA_GPS_RX); * The same functionality as on_gps_event() in * standard paparazzi */ -void on_gps_event(void) { - //chMtxLock(&ahrs_states_mutex_flag); +void on_gps_event(void) +{ ahrs_update_gps(); - //chMtxUnlock(); - - //chMtxLock(&ins_data_flag); ins_update_gps(); - //chMtxUnlock(); } -#endif +#endif /* USE_GPS */ #ifdef USE_IMU #ifdef INIT_IMU_THREAD static WORKING_AREA(wa_thd_imu_rx, CH_THREAD_AREA_IMU_RX); #endif - /** - * IMU Accel callback - */ - void on_accel_event( void ) { - ImuScaleAccel(imu); - if (ahrs.status != AHRS_UNINIT) { - // chMtxLock(&ahrs_states_mutex_flag); - ahrs_update_accel(); - // chMtxUnlock(); - } +/** + * IMU Accel callback + */ +void on_accel_event( void ) +{ + ImuScaleAccel(imu); + if (ahrs.status != AHRS_UNINIT) + { + ahrs_update_accel(); } +} - /** - * IMU Gyro callback - */ - void on_gyro_event( void ) { - ImuScaleGyro(imu); - if (ahrs.status == AHRS_UNINIT) { - ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) - ahrs_align(); - } - else { - // chMtxLock(&ahrs_states_mutex_flag); - ahrs_propagate(); - //chMtxUnlock(); - - //chMtxLock(&ins_data_flag); - ins_propagate(); - //chMtxUnlock(); - } +/** + * IMU Gyro callback + */ +void on_gyro_event( void ) +{ + ImuScaleGyro(imu); + if (ahrs.status == AHRS_UNINIT) + { + ahrs_aligner_run(); + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + ahrs_align(); } + else { + ahrs_propagate(); + ins_propagate(); + } +} - /** - * IMU Mag callback - */ - void on_mag_event(void) { - ImuScaleMag(imu); - #if USE_MAGNETOMETER - if (ahrs.status == AHRS_RUNNING) { - ahrs_update_mag(); - } - #endif +/** + * IMU Mag callback + */ +void on_mag_event(void) +{ + ImuScaleMag(imu); +#if USE_MAGNETOMETER + if (ahrs.status == AHRS_RUNNING) + { + ahrs_update_mag(); } - #endif +#endif +} +#endif /* USE_IMU */ #ifdef MODULES_C -__attribute__((noreturn)) msg_t thd_modules_periodic(void *arg); + __attribute__((noreturn)) msg_t thd_modules_periodic(void *arg); #endif /* if PRINT_CONFIG is defined, print some config options */ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) -/** +/* * TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file */ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY) -/** +/* * MODULES_FREQUENCY is defined in generated/modules.h * according to main_freq parameter set for modules in airframe file */ @@ -228,7 +230,6 @@ PRINT_CONFIG_VAR(RADIO_CONTROL_FREQ) * * Blinks LED and logs the cpu usage and other system info */ -static WORKING_AREA(wa_thd_heartbeat, 128); static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) { chRegSetThreadName("pprz_heartbeat"); @@ -237,14 +238,16 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) static uint32_t last_idle_counter = 0; static uint32_t last_nb_sec = 0; - while (TRUE) { + while (TRUE) + { time += S2ST(1); LED_TOGGLE(SYS_TIME_LED); sys_time.nb_sec++; - if (autopilot_in_flight) { - autopilot_flight_time++; - datalink_time++; + if (autopilot_in_flight) + { + autopilot_flight_time++; + datalink_time++; } core_free_memory = chCoreStatus(); @@ -252,13 +255,16 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) Thread *tp; tp = chRegFirstThread(); - do { + do + { thread_counter++; - if (tp ==chSysGetIdleThread()) { - idle_counter = (uint32_t)tp->p_time; + if (tp ==chSysGetIdleThread()) + { + idle_counter = (uint32_t)tp->p_time; } tp = chRegNextThread(tp); - } while (tp != NULL); + } + while (tp != NULL); cpu_counter = (idle_counter-last_idle_counter)/(sys_time.nb_sec-last_nb_sec); cpu_frequency = (1 - (float)cpu_counter/CH_FREQUENCY)*100; @@ -270,111 +276,132 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) } } -/* +/** * Failsafe Thread - * @note: Replaces failsafe_periodic(), eventually - * it will check also other threads (~hypervisor) + * + * Replaces failsafe_periodic() + * + * TODO: ChibiOS/RT failsafe check (hypervisor thread) */ -static WORKING_AREA(wa_thd_failsafe, CH_THREAD_AREA_FAILSAFE); static __attribute__((noreturn)) msg_t thd_failsafe(void *arg) { chRegSetThreadName("pprz_failsafe"); (void) arg; systime_t time = chTimeNow(); - while (TRUE) { + while (TRUE) + { time += US2ST(1000000/FAILSAFE_FREQUENCY); failsafe_check(); - //TODO: ChibiOS/RT failsafe check chThdSleepUntil(time); } } /* * Electrical Periodic Thread - * @note: Calls electrical_periodic() + * + * Calls electrical_periodic() */ - static WORKING_AREA(wa_thd_electrical, CH_THREAD_AREA_ELECTRICAL); - static __attribute__((noreturn)) msg_t thd_electrical(void *arg) - { - chRegSetThreadName("pprz_electrical"); - (void) arg; - systime_t time = chTimeNow(); - while (TRUE) { - time += US2ST(1000000/ELECTRICAL_PERIODIC_FREQ); - electrical_periodic(); - chThdSleepUntil(time); - } - } - - /* - * Radio Control Periodic Thread - * @note: Calls radio_control_periodic() - */ - static WORKING_AREA(wa_thd_radio_control, CH_THREAD_AREA_RADIO_CONTROL); - static __attribute__((noreturn)) msg_t thd_radio_control(void *arg) +static __attribute__((noreturn)) msg_t thd_electrical(void *arg) +{ + chRegSetThreadName("pprz_electrical"); + (void) arg; + systime_t time = chTimeNow(); + while (TRUE) { - chRegSetThreadName("pprz_radio_control"); - (void) arg; - systime_t time = chTimeNow(); - while (TRUE) { - time += US2ST(1000000/RADIO_CONTROL_FREQ); - radio_control_periodic_task(); - chThdSleepUntil(time); - } + time += US2ST(1000000/ELECTRICAL_PERIODIC_FREQ); + electrical_periodic(); + chThdSleepUntil(time); } +} - /** - * Radio Control Event Thread - * - * Waits for EVT_PPM_FRAME event flag to be broadcasted, - * then executes RadioControlEvent() - * - * @note: It is a nice example how to use event listeners. - * Optionally after the frame is processed, another event can be - * broadcasted, so it is possible to chain data processing (i.e. in AHRS) - * Maybe a similar structure can be used for GPS events etc. - */ - static WORKING_AREA(wa_thd_radio_event, CH_THREAD_AREA_RADIO_EVENT); - static __attribute__((noreturn)) msg_t thd_radio_event(void *arg) - { - chRegSetThreadName("pprz_radio_event"); - (void) arg; - EventListener elRadioEvt; - chEvtRegister(&eventPpmFrame, &elRadioEvt, EVT_PPM_FRAME); - flagsmask_t rc_flags; - while (TRUE) { - chEvtWaitOne(EVENT_MASK(EVT_PPM_FRAME)); - rc_flags = chEvtGetAndClearFlags(&elRadioEvt); - if (rc_flags & EVT_PPM_FRAME) { - if (autopilot_rc) { - RadioControlEvent(autopilot_on_rc_frame); - ///chEvtBroadcastFlags(&initializedEventSource, SOME_DEFINED_EVENT); - } - } - } - } +/* + * Radio Control Periodic Thread + * + * Calls radio_control_periodic() + */ +static __attribute__((noreturn)) msg_t thd_radio_control(void *arg) +{ + chRegSetThreadName("pprz_radio_control"); + (void) arg; + systime_t time = chTimeNow(); + while (TRUE) + { + time += US2ST(1000000/RADIO_CONTROL_FREQ); + radio_control_periodic_task(); + chThdSleepUntil(time); + } +} + +/** + * Radio Control Event Thread + * + * Waits for EVT_PPM_FRAME event flag to be broadcasted, + * then executes RadioControlEvent() + * + * @note: It is a nice example how to use event listeners. + * Optionally after the frame is processed, another event can be + * broadcasted, so it is possible to chain data processing (i.e. in AHRS) + * Maybe a similar structure can be used for GPS events etc. + * + * after receiving EVT_PPM_FRAM and processing it, we can call + * chEvtBroadcastFlags(&initializedEventSource, SOME_DEFINED_EVENT); + * to propagate event further + */ +static __attribute__((noreturn)) msg_t thd_radio_event(void *arg) +{ + chRegSetThreadName("pprz_radio_event"); + (void) arg; + + EventListener elRadioEvt; + chEvtRegister(&eventPpmFrame, &elRadioEvt, EVT_PPM_FRAME); + flagsmask_t rc_flags; + + while (TRUE) + { + chEvtWaitOne(EVENT_MASK(EVT_PPM_FRAME)); + rc_flags = chEvtGetAndClearFlags(&elRadioEvt); + if (rc_flags & EVT_PPM_FRAME) + { + if (autopilot_rc) + { + RadioControlEvent(autopilot_on_rc_frame); + } + } + } +} #if USE_BARO_BOARD - /** - * Baro thread - */ - static WORKING_AREA(wa_thd_baro, CH_THREAD_AREA_BARO); - static __attribute__((noreturn)) msg_t thd_baro(void *arg) { - chRegSetThreadName("pprz_baro"); - (void) arg; - baro_init(); - systime_t time = chTimeNow(); - while (TRUE) { - time += US2ST(1000000/BARO_PERIODIC_FREQUENCY); - baro_periodic(); - chThdSleepUntil(time); - } - } +#define CH_THREAD_AREA_BARO 512 +static WORKING_AREA(wa_thd_baro, CH_THREAD_AREA_BARO); +static __attribute__((noreturn)) msg_t thd_baro(void *arg); -#endif +/** + * Baro thread + */ +static __attribute__((noreturn)) msg_t thd_baro(void *arg) +{ + chRegSetThreadName("pprz_baro"); + (void) arg; + + baro_init(); + + systime_t time = chTimeNow(); + while (TRUE) + { + time += US2ST(1000000/BARO_PERIODIC_FREQUENCY); + baro_periodic(); + chThdSleepUntil(time); + } +} +#endif /* USE_BARO_BOARD */ #ifdef DOWNLINK +#define CH_THREAD_AREA_DOWNLINK_TX 1024 +#define CH_THREAD_AREA_DOWNLINK_RX 1024 +__attribute__((noreturn)) msg_t thd_telemetry_tx(void *arg); +__attribute__((noreturn)) msg_t thd_telemetry_rx(void *arg); + /** * Telemetry TX thread * @@ -386,10 +413,11 @@ __attribute__((noreturn)) msg_t thd_telemetry_tx(void *arg) chRegSetThreadName("pprz_telemetry_tx"); (void) arg; systime_t time = chTimeNow(); - while (TRUE) { - time += US2ST(1000000/TELEMETRY_FREQUENCY); - PeriodicSendMain(DefaultChannel,DefaultDevice); - chThdSleepUntil(time); + while (TRUE) + { + time += US2ST(1000000/TELEMETRY_FREQUENCY); + PeriodicSendMain(DefaultChannel,DefaultDevice); + chThdSleepUntil(time); } } @@ -414,7 +442,8 @@ __attribute__((noreturn)) msg_t thd_telemetry_rx(void *arg) chEvtWaitOneTimeout(EVENT_MASK(1), S2ST(1)); flags = chEvtGetAndClearFlags(&elTelemetryRx); ch_uart_receive_downlink(DOWNLINK_PORT, flags, parse_pprz, &pprz_tp); - if (pprz_tp.trans.msg_received) { + if (pprz_tp.trans.msg_received) + { pprz_parse_payload(&(pprz_tp)); pprz_tp.trans.msg_received = FALSE; dl_parse_msg(); @@ -422,7 +451,7 @@ __attribute__((noreturn)) msg_t thd_telemetry_rx(void *arg) } } } -#endif +#endif /* DOWNLINK */ #ifdef MODULES_C /** @@ -434,29 +463,31 @@ __attribute__((noreturn)) msg_t thd_modules_periodic(void *arg) chRegSetThreadName("pprz_modules_periodic"); (void) arg; systime_t time = chTimeNow(); - while (TRUE) { + while (TRUE) + { time += MS2ST(1000/MODULES_FREQUENCY); modules_periodic_task(); chThdSleepUntil(time); } } -#endif +#endif /* MODULES_C */ /** * Paparazzi failsafe thread */ -static inline void failsafe_check(void) { +static inline void failsafe_check(void) +{ if (radio_control.status != RC_OK && - autopilot_mode != AP_MODE_KILL && - autopilot_mode != AP_MODE_NAV) + autopilot_mode != AP_MODE_KILL && + autopilot_mode != AP_MODE_NAV) { autopilot_set_mode(AP_MODE_FAILSAFE); } #if FAILSAFE_ON_BAT_CRITICAL if (autopilot_mode != AP_MODE_KILL && - electrical.bat_critical) + electrical.bat_critical) { autopilot_set_mode(AP_MODE_FAILSAFE); } @@ -464,11 +495,11 @@ static inline void failsafe_check(void) { #if USE_GPS if (autopilot_mode == AP_MODE_NAV && - autopilot_motors_on && + autopilot_motors_on && #if NO_GPS_LOST_WITH_RC_VALID - radio_control.status != RC_OK && + radio_control.status != RC_OK && #endif - GpsIsLost()) + GpsIsLost()) { autopilot_set_mode(AP_MODE_FAILSAFE); } @@ -478,9 +509,10 @@ static inline void failsafe_check(void) { } -/* +/** * Thread initialization - * @note: Done here, not in the submodules, so we don't + * + * Done here, not in the submodules, so we don't * have to include ChibiOs headers to each submodule */ static void thread_init(void) { @@ -499,35 +531,37 @@ chThdCreateStatic(wa_thd_radio_event, sizeof(wa_thd_radio_event), NORMALPRIO, th #else imu_init(); #if USE_IMU_FLOAT - imu_float_init(); + imu_float_init(); #endif -#endif -#endif +#endif /* INIT_IMU_THREAD */ +#endif /* USE_IMU */ #ifdef DOWNLINK -chThdCreateStatic(wa_thd_telemetry_tx, sizeof(wa_thd_telemetry_tx),NORMALPRIO, thd_telemetry_tx, NULL); -chThdCreateStatic(wa_thd_telemetry_rx, sizeof(wa_thd_telemetry_rx),NORMALPRIO, thd_telemetry_rx, NULL); + chThdCreateStatic(wa_thd_telemetry_tx, sizeof(wa_thd_telemetry_tx),NORMALPRIO, thd_telemetry_tx, NULL); + chThdCreateStatic(wa_thd_telemetry_rx, sizeof(wa_thd_telemetry_rx),NORMALPRIO, thd_telemetry_rx, NULL); #endif #ifdef USE_GPS - chThdCreateStatic(wa_thd_gps_rx, sizeof(wa_thd_gps_rx),NORMALPRIO, thd_gps_rx, NULL); + chThdCreateStatic(wa_thd_gps_rx, sizeof(wa_thd_gps_rx),NORMALPRIO, thd_gps_rx, &on_gps_event); #endif #ifdef MODULES_C -chThdCreateStatic(wa_thd_modules_periodic, sizeof(wa_thd_modules_periodic),LOWPRIO, thd_modules_periodic, NULL); + chThdCreateStatic(wa_thd_modules_periodic, sizeof(wa_thd_modules_periodic),LOWPRIO, thd_modules_periodic, NULL); #endif chThdCreateStatic(wa_thd_failsafe, sizeof(wa_thd_failsafe), HIGHPRIO, thd_failsafe, NULL); } + /** * Main loop * * Initializes system (both chibios and paparazzi), * then turns into main thread - main_periodic() */ -int main(void) { - /** +int main(void) +{ + /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. @@ -537,7 +571,9 @@ int main(void) { halInit(); chSysInit(); - /// Paparazzi initialization + /* + * Paparazzi initialization + */ mcu_init(); electrical_init(); @@ -553,20 +589,20 @@ int main(void) { radio_control_init(); air_data_init(); - #if USE_BARO_BOARD - baro_init(); - #endif +#if USE_BARO_BOARD + baro_init(); +#endif - imu_init(); - #if USE_IMU_FLOAT - imu_float_init(); - #endif +imu_init(); +#if USE_IMU_FLOAT + imu_float_init(); +#endif - ahrs_aligner_init(); - ahrs_init(); + ahrs_aligner_init(); + ahrs_init(); - ins_init(); + ins_init(); #if USE_GPS gps_init(); @@ -593,12 +629,14 @@ int main(void) { chThdSleep(MS2ST(1500)); systime_t main_time = chTimeNow(); - while (TRUE) { - main_time += US2ST(1000000/PERIODIC_FREQUENCY); - imu_periodic(); - autopilot_periodic(); - SetActuatorsFromCommands(commands, autopilot_mode); - chThdSleepUntil(main_time); + while (TRUE) + { + main_time += US2ST(1000000/PERIODIC_FREQUENCY); + imu_periodic(); + ImuEvent(on_gyro_event, on_accel_event, on_mag_event); + autopilot_periodic(); + SetActuatorsFromCommands(commands, autopilot_mode); + chThdSleepUntil(main_time); } return TRUE; diff --git a/sw/airborne/mcu_periph/i2c_pprzi.c b/sw/airborne/mcu_periph/i2c_pprz.c similarity index 98% rename from sw/airborne/mcu_periph/i2c_pprzi.c rename to sw/airborne/mcu_periph/i2c_pprz.c index a0dab30eb6e..8abb40bea99 100644 --- a/sw/airborne/mcu_periph/i2c_pprzi.c +++ b/sw/airborne/mcu_periph/i2c_pprz.c @@ -21,7 +21,7 @@ */ /** - * @file mcu_periph/i2c.c + * @file mcu_periph/i2c_pprz.c * Architecture independent I2C (Inter-Integrated Circuit Bus) API. */ diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h index 693c3f7c37e..fe97593ca0c 100644 --- a/sw/airborne/mcu_periph/spi.h +++ b/sw/airborne/mcu_periph/spi.h @@ -37,6 +37,9 @@ #ifdef USE_CHIBIOS_RTOS #include "hal.h" +#define SPI_VOLATILE +#else +#define SPI_VOLATILE volatile #endif /** @@ -144,13 +147,8 @@ typedef void (*SPICallback)( struct spi_transaction *trans ); * 0 is sent for the remaining words */ struct spi_transaction { -#ifdef USE_CHIBIOS_RTOS - uint8_t* input_buf; ///< pointer to receive buffer for DMA - uint8_t* output_buf; ///< pointer to transmit buffer for DMA -#else - volatile uint8_t* input_buf; ///< pointer to receive buffer for DMA - volatile uint8_t* output_buf; ///< pointer to transmit buffer for DMA -#endif + SPI_VOLATILE uint8_t* input_buf; ///< pointer to receive buffer for DMA + SPI_VOLATILE uint8_t* output_buf; ///< pointer to transmit buffer for DMA uint8_t input_length; ///< number of data words to read uint8_t output_length; ///< number of data words to write uint8_t slave_idx; ///< slave id: #SPI_SLAVE0 to #SPI_SLAVE4 @@ -176,8 +174,8 @@ struct spi_periph { uint8_t trans_extract_idx; /** internal state of the peripheral */ volatile enum SPIStatus status; - volatile uint8_t tx_idx_buf; - volatile uint8_t rx_idx_buf; + SPI_VOLATILE uint8_t tx_idx_buf; + SPI_VOLATILE uint8_t rx_idx_buf; void *reg_addr; void *init_struct; enum SPIMode mode; diff --git a/sw/airborne/mcu_periph/spi_pprzi.c b/sw/airborne/mcu_periph/spi_pprz.c similarity index 98% rename from sw/airborne/mcu_periph/spi_pprzi.c rename to sw/airborne/mcu_periph/spi_pprz.c index 3434711c33c..3837ea0f622 100644 --- a/sw/airborne/mcu_periph/spi_pprzi.c +++ b/sw/airborne/mcu_periph/spi_pprz.c @@ -21,7 +21,7 @@ */ /** - * @file mcu_periph/spi.c + * @file mcu_periph/spi_pprz.c * * Architecture independent SPI (Serial Peripheral Interface) API. */ diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index a8bc2c8d095..9fa36e2a236 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -81,7 +81,9 @@ extern bool_t uart_check_free_space(struct uart_periph* p, uint8_t len); extern uint8_t uart_getch(struct uart_periph* p); #ifdef USE_CHIBIOS_RTOS +/// Unfortunately has to be declared here, if declared in uart_arch.h compiler complains about uart_periph struct extern void uart_transmit_buffer(struct uart_periph* p, uint8_t* data_buffer, size_t length); +extern void uart_receive_buffer(struct uart_periph* p, flagsmask_t flags, void *on_receive_callback); #endif static inline bool_t uart_char_available(struct uart_periph* p) { diff --git a/sw/airborne/mcu_periph/uart_pprzi.c b/sw/airborne/mcu_periph/uart_pprz.c similarity index 98% rename from sw/airborne/mcu_periph/uart_pprzi.c rename to sw/airborne/mcu_periph/uart_pprz.c index 34f325c0914..5c38d0780d7 100644 --- a/sw/airborne/mcu_periph/uart_pprzi.c +++ b/sw/airborne/mcu_periph/uart_pprz.c @@ -21,6 +21,8 @@ */ /** + * @file mcu_periph/uart_pprz.c + * * @brief arch independent UART (Universal Asynchronous Receiver/Transmitter) API * @note Had to be renamed, since chibios is using the same filename for its own * UART drivers. Modified by Aggiear (http://aggieair.usu.edu/), 2013 diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h index 3d0435a4f2a..c4f41547ba9 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.h +++ b/sw/airborne/peripherals/mpu60x0_spi.h @@ -51,13 +51,8 @@ enum Mpu60x0SpiSlaveInitStatus { struct Mpu60x0_Spi { struct spi_periph *spi_p; struct spi_transaction spi_trans; -#ifdef USE_CHIBIOS_RTOS - uint8_t tx_buf[2]; - uint8_t rx_buf[MPU60X0_BUFFER_LEN]; -#else - volatile uint8_t tx_buf[2]; - volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; -#endif + SPI_VOLATILE uint8_t tx_buf[2]; + SPI_VOLATILE uint8_t rx_buf[MPU60X0_BUFFER_LEN]; volatile bool_t data_available; ///< data ready flag union { struct Int16Vect3 vect; ///< accel data vector in accel coordinate system diff --git a/sw/airborne/peripherals/ms5611_spi.c b/sw/airborne/peripherals/ms5611_spi.c index 0fcca58b1df..01ab22b5cc3 100644 --- a/sw/airborne/peripherals/ms5611_spi.c +++ b/sw/airborne/peripherals/ms5611_spi.c @@ -89,7 +89,60 @@ void ms5611_spi_start_conversion(struct Ms5611_Spi *ms) */ void ms5611_spi_periodic_check(struct Ms5611_Spi *ms) { + switch (ms->status) { + case MS5611_STATUS_RESET: + ms->status = MS5611_STATUS_RESET_OK; + break; + case MS5611_STATUS_RESET_OK: + if (ms->spi_trans.status == SPITransDone) { + /* start getting prom data */ + ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); + ms->status = MS5611_STATUS_PROM; + spi_submit(ms->spi_p, &(ms->spi_trans)); + } + break; + case MS5611_STATUS_PROM: + if (ms->prom_cnt < PROM_NB) { + /* get next prom data */ + ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); + spi_submit(ms->spi_p, &(ms->spi_trans)); + } + break; + case MS5611_STATUS_CONV_D1: + ms->status = MS5611_STATUS_CONV_D1_OK; + break; + case MS5611_STATUS_CONV_D1_OK: + if (ms->spi_trans.status == SPITransDone) { + /* read D1 adc */ + ms->tx_buf[0] = MS5611_ADC_READ; + ms->status = MS5611_STATUS_ADC_D1; + spi_submit(ms->spi_p, &(ms->spi_trans)); + } + break; + case MS5611_STATUS_CONV_D2: + ms->status = MS5611_STATUS_CONV_D2_OK; + break; + case MS5611_STATUS_CONV_D2_OK: + if (ms->spi_trans.status == SPITransDone) { + /* read D2 adc */ + ms->tx_buf[0] = MS5611_ADC_READ; + ms->status = MS5611_STATUS_ADC_D2; + spi_submit(ms->spi_p, &(ms->spi_trans)); + } + break; + default: + break; + } +} + #ifdef USE_CHIBIOS_RTOS +/** + * Synchronous periodic function to ensure prope delay between readings + * + * Includes also state machine logic for pressure sensor readings. + */ +void ms5611_spi_synchronous_periodic_check(struct Ms5611_Spi *ms) +{ if (ms->initialized) { if (ms->spi_trans.status == SPITransFailed) { ms->status = MS5611_STATUS_IDLE; @@ -162,7 +215,7 @@ void ms5611_spi_periodic_check(struct Ms5611_Spi *ms) break; case MS5611_STATUS_PROM: if (ms->spi_trans.status != SPITransFailed) { - ///No need to wait between EEPROM reads + /// No need to wait between EEPROM reads /// TODO: SPI status check after each transaction do { /* get prom data */ @@ -190,52 +243,8 @@ void ms5611_spi_periodic_check(struct Ms5611_Spi *ms) break; } } -#else - switch (ms->status) { - case MS5611_STATUS_RESET: - ms->status = MS5611_STATUS_RESET_OK; - break; - case MS5611_STATUS_RESET_OK: - if (ms->spi_trans.status == SPITransDone) { - /* start getting prom data */ - ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); - spi_submit(ms->spi_p, &(ms->spi_trans)); - } - break; - case MS5611_STATUS_PROM: - if (ms->prom_cnt < PROM_NB) { - /* get next prom data */ - ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); - spi_submit(ms->spi_p, &(ms->spi_trans)); - } - break; - case MS5611_STATUS_CONV_D1: - ms->status = MS5611_STATUS_CONV_D1_OK; - break; - case MS5611_STATUS_CONV_D1_OK: - if (ms->spi_trans.status == SPITransDone) { - /* read D1 adc */ - ms->tx_buf[0] = MS5611_ADC_READ; - spi_submit(ms->spi_p, &(ms->spi_trans)); - ms->status = MS5611_STATUS_ADC_D1; - } - break; - case MS5611_STATUS_CONV_D2: - ms->status = MS5611_STATUS_CONV_D2_OK; - break; - case MS5611_STATUS_CONV_D2_OK: - if (ms->spi_trans.status == SPITransDone) { - /* read D2 adc */ - ms->tx_buf[0] = MS5611_ADC_READ; - spi_submit(ms->spi_p, &(ms->spi_trans)); - ms->status = MS5611_STATUS_ADC_D2; - } - break; - default: - break; - } -#endif } +#endif /* USE_CHIBIOS_RTOS */ void ms5611_spi_event(struct Ms5611_Spi *ms) { if (ms->initialized) { diff --git a/sw/airborne/peripherals/ms5611_spi.h b/sw/airborne/peripherals/ms5611_spi.h index 88435d6d4ed..37d96e1256d 100644 --- a/sw/airborne/peripherals/ms5611_spi.h +++ b/sw/airborne/peripherals/ms5611_spi.h @@ -36,13 +36,8 @@ struct Ms5611_Spi { struct spi_periph *spi_p; struct spi_transaction spi_trans; - #ifdef USE_CHIBIOS_RTOS - uint8_t tx_buf[1]; - uint8_t rx_buf[4]; - #else - volatile uint8_t tx_buf[1]; - volatile uint8_t rx_buf[4]; - #endif + SPI_VOLATILE uint8_t tx_buf[1]; + SPI_VOLATILE uint8_t rx_buf[4]; enum Ms5611Status status; bool_t initialized; ///< config done flag @@ -57,6 +52,9 @@ extern void ms5611_spi_start_configure(struct Ms5611_Spi* ms); extern void ms5611_spi_start_conversion(struct Ms5611_Spi* ms); extern void ms5611_spi_periodic_check(struct Ms5611_Spi* ms); extern void ms5611_spi_event(struct Ms5611_Spi* ms); +#ifdef USE_CHIBIOS_RTOS +extern void ms5611_spi_synchronous_periodic_check(struct Ms5611_Spi* ms); +#endif /* USE_CHIBIOS_RTOS */ /** convenience function to trigger new measurement. * (or start configuration if not already initialized) @@ -71,11 +69,8 @@ static inline void ms5611_spi_read(struct Ms5611_Spi* ms) { /// convenience function static inline void ms5611_spi_periodic(struct Ms5611_Spi* ms) { -#ifndef USE_CHIBIOS_RTOS ms5611_spi_read(ms); -#else ms5611_spi_periodic_check(ms); -#endif } diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index ccad965926e..41f4e06816f 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -46,7 +46,6 @@ #ifdef USE_CHIBIOS_RTOS #include "ch.h" -extern void on_gps_event(void); extern Mutex gps_mutex_flag; extern __attribute__((noreturn)) msg_t thd_gps_rx(void *arg); #endif diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 755bc09f353..dbcddf383b3 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -288,17 +288,19 @@ void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) { * GPS Thread * Replaces GpsEvent() */ -__attribute__((noreturn)) msg_t thd_gps_rx(void *arg) +__attribute__((noreturn)) msg_t thd_gps_rx(void *gps_callback) { chRegSetThreadName("pprz_gps_rx"); - (void) arg; + + gps_init(); + EventListener elGPSdata; flagsmask_t flags; chEvtRegisterMask((EventSource *)chnGetEventSource((SerialDriver*)GPS_PORT.reg_addr), &elGPSdata, EVENT_MASK(1)); while (TRUE) { chEvtWaitOneTimeout(EVENT_MASK(1), S2ST(1)); flags = chEvtGetAndClearFlags(&elGPSdata); - ch_uart_receive(GPS_PORT, flags, gps_ubx_parse); + uart_receive_buffer(&GPS_PORT, flags, &gps_ubx_parse); if (gps_ubx.msg_available) { chMtxLock(&gps_mutex_flag); gps_ubx_read_message(); @@ -311,7 +313,10 @@ __attribute__((noreturn)) msg_t thd_gps_rx(void *arg) gps.last_fix_ticks = sys_time.nb_sec_rem; gps.last_fix_time = sys_time.nb_sec; } - on_gps_event(); + if (gps_callback != NULL) { + /// it can be probably prettified with macro GPS_CB(_x) (void(*)(void))_x(); + ((void(*)(void))gps_callback)(); + } } chMtxUnlock(); gps_ubx.msg_available = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index bf5967193a0..ef708b53ee5 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -151,10 +151,6 @@ void imu_impl_init(void) void imu_periodic(void) { mpu60x0_spi_periodic(&imu_aspirin2.mpu); - -#ifdef USE_CHIBIOS_RTOS -ImuEvent(on_gyro_event, on_accel_event, on_mag_event); -#endif } #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h index de391e1b428..4ca6079778a 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h @@ -57,13 +57,8 @@ struct ImuAspirin2Spi { struct Mpu60x0_Spi mpu; struct spi_transaction wait_slave4_trans; -#ifdef USE_CHIBIOS_RTOS - uint8_t wait_slave4_tx_buf[1]; - uint8_t wait_slave4_rx_buf[2]; -#else - volatile uint8_t wait_slave4_tx_buf[1]; - volatile uint8_t wait_slave4_rx_buf[2]; -#endif + SPI_VOLATILE uint8_t wait_slave4_tx_buf[1]; + SPI_VOLATILE uint8_t wait_slave4_rx_buf[2]; volatile bool_t slave4_ready; }; diff --git a/sw/airborne/subsystems/imu/imu_gx3.c b/sw/airborne/subsystems/imu/imu_gx3.c index e1a423c8c26..df99c6e9b1b 100644 --- a/sw/airborne/subsystems/imu/imu_gx3.c +++ b/sw/airborne/subsystems/imu/imu_gx3.c @@ -215,14 +215,12 @@ void imu_periodic(void) { } chMtxUnlock(); gx3_packet_read_message(); - //TODO: proper callbacks (ins, ahrs, etc.) - // Probably Mailboxes? or EventBroadcast? - ahrs_update_accel(); - //Lock mutex for system states (statesSetRates etc.) - chMtxLock(&states_mutex_flag); - ahrs_propagate(); - chMtxUnlock(); + + /// Callbacks + on_accel_event(); + on_gyro_event(); + on_mag_event(); } else { chMtxUnlock();