From f2ef4339577d14651a4662f0d43ea119237f437c Mon Sep 17 00:00:00 2001 From: podhrmic Date: Sun, 24 Nov 2013 22:18:56 -0700 Subject: [PATCH] [rt_paparazzi] update 0.3.1. aka big fat attempt to finally clean up the callbacks, threads, io and other stuff 1) Two sample configuration files: * hexarotor Lia with GX3 * hexarotor Lia with Aspirin 2) Clean up in the rotorcraft_rt.makefile 3) Aspirin and MS5611 SPI drivers are now compatible with ChibiOS 4) Improved default chibios telemetry (more information) 5) Cleaned up drivers (ADC, SPI, I2C, UART) 6) Cleaned up board config files 7) Cleaned up error logging for uart (GPS/Telemetry threads) 8) Improved thread structure in main_chibios.c --- .../examples/rt_hexarotor_lia_aspirin.xml | 216 +++++++++ .../examples/rt_hexarotor_lia_gx3.xml | 219 ++++++++++ conf/firmwares/rotorcraft_rt.makefile | 42 +- .../shared/imu_aspirin_v2_common.makefile | 5 + .../subsystems/shared/spi_master.makefile | 2 +- conf/telemetry/default_chibios.xml | 13 +- .../arch/chibios/mcu_periph/adc_arch.c | 45 +- .../arch/chibios/mcu_periph/i2c_arch.c | 72 +-- .../arch/chibios/mcu_periph/spi_arch.c | 317 ++++++++++++++ .../arch/chibios/mcu_periph/spi_arch.h | 32 ++ .../arch/chibios/mcu_periph/sys_time_arch.h | 2 + .../arch/chibios/mcu_periph/uart_arch.c | 4 +- .../arch/chibios/mcu_periph/uart_arch.h | 31 +- sw/airborne/boards/baro_board_ms5611_spi.c | 6 + sw/airborne/boards/lia/baro_board.h | 15 + sw/airborne/boards/lia/chibios/board.h | 85 +++- sw/airborne/boards/lia/chibios/halconf.h | 4 +- sw/airborne/boards/lia/chibios/mcuconf.h | 16 +- .../firmwares/rotorcraft/main_chibios.c | 409 ++++++++++++++---- sw/airborne/firmwares/rotorcraft/telemetry.h | 3 +- sw/airborne/mcu_periph/spi.h | 9 + sw/airborne/mcu_periph/spi_pprzi.c | 135 ++++++ sw/airborne/peripherals/mpu60x0_spi.h | 5 + sw/airborne/peripherals/ms5611_spi.c | 138 +++++- sw/airborne/peripherals/ms5611_spi.h | 9 + sw/airborne/subsystems/ahrs/ahrs_aggienav.c | 4 +- sw/airborne/subsystems/gps.c | 7 + sw/airborne/subsystems/gps.h | 6 + sw/airborne/subsystems/gps/gps_ubx.c | 10 +- sw/airborne/subsystems/gps/gps_ubx.h | 8 - .../subsystems/imu/imu_aspirin_2_spi.c | 4 + .../subsystems/imu/imu_aspirin_2_spi.h | 6 + 32 files changed, 1631 insertions(+), 248 deletions(-) create mode 100644 conf/airframes/examples/rt_hexarotor_lia_aspirin.xml create mode 100644 conf/airframes/examples/rt_hexarotor_lia_gx3.xml create mode 100644 sw/airborne/arch/chibios/mcu_periph/spi_arch.c create mode 100644 sw/airborne/arch/chibios/mcu_periph/spi_arch.h create mode 100644 sw/airborne/boards/lia/baro_board.h create mode 100644 sw/airborne/mcu_periph/spi_pprzi.c diff --git a/conf/airframes/examples/rt_hexarotor_lia_aspirin.xml b/conf/airframes/examples/rt_hexarotor_lia_aspirin.xml new file mode 100644 index 00000000000..a402236ed3c --- /dev/null +++ b/conf/airframes/examples/rt_hexarotor_lia_aspirin.xml @@ -0,0 +1,216 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/airframes/examples/rt_hexarotor_lia_gx3.xml b/conf/airframes/examples/rt_hexarotor_lia_gx3.xml new file mode 100644 index 00000000000..3a50dca9666 --- /dev/null +++ b/conf/airframes/examples/rt_hexarotor_lia_gx3.xml @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/firmwares/rotorcraft_rt.makefile b/conf/firmwares/rotorcraft_rt.makefile index e11c093930d..ce31b7c5c42 100644 --- a/conf/firmwares/rotorcraft_rt.makefile +++ b/conf/firmwares/rotorcraft_rt.makefile @@ -40,18 +40,14 @@ SRC_ARCH=arch/$(ARCH) ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD) - ap.ARCHDIR = $(ARCH) -# would be better to auto-generate this -$(TARGET).CFLAGS += -DFIRMWARE=ROTORCRAFT#we are using normal rotorcraft firmware, just different arch -$(TARGET).CFLAGS += -DRTOS#hack for distinquisthing between RT and normal paparazzi config - -#Hack for paparazzi sw/include/std.h +#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 + # # Main (Using RTOS) # @@ -64,31 +60,13 @@ ap.srcs += $(SRC_ARCH)/mcu_arch.c # ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c -ifeq ($(ARCH), stm32) -ap.srcs += lisa/plug_sys.c -endif -# -# Interrupts -# -ifeq ($(ARCH), lpc21) -ap.srcs += $(SRC_ARCH)/armVIC.c -endif - -ifeq ($(ARCH), stm32) -ap.srcs += $(SRC_ARCH)/mcu_periph/gpio_arch.c -endif - # # LEDs # ap.CFLAGS += -DUSE_LED -ifeq ($(ARCH), stm32) -ap.srcs += $(SRC_ARCH)/led_hw.c -endif - -ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw) -ap.srcs += $(SRC_BOARD)/gpio_ardrone.c -endif +#ifeq ($(ARCH), chibios) +#ap.srcs += $(SRC_ARCH)/led_hw.c +#endif # frequency of main periodic PERIODIC_FREQUENCY ?= 500 @@ -110,17 +88,13 @@ endif # or # include subsystems/rotorcraft/telemetry_xbee_api.makefile # -ap.srcs += subsystems/settings.c#includes all generated settings -#ap.srcs += $(SRC_ARCH)/subsystems/settings_arch.c -> not necessary +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 += $(SRC_ARCH)/mcu_periph/uart_arch.c -ifeq ($(ARCH), omap) -ap.srcs += $(SRC_ARCH)/serial_port.c -endif -# I2C is needed for speed controllers and barometers on lisa +# I2C ifeq ($(TARGET), ap) $(TARGET).srcs += mcu_periph/i2c_pprzi.c $(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile index f8b3560ed58..2dfdf4f5364 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile @@ -61,6 +61,11 @@ else ifeq ($(ARCH), stm32) # SLAVE2 is on PB12 (NSS) (MPU600 CS) ASPIRIN_2_SPI_DEV ?= spi2 ASPIRIN_2_SPI_SLAVE_IDX ?= SPI_SLAVE2 +else ifeq ($(ARCH), chibios) +# Slave select configuration +# SLAVE2 is on PB12 (NSS) (MPU600 CS) +ASPIRIN_2_SPI_DEV ?= spi2 +ASPIRIN_2_SPI_SLAVE_IDX ?= SPI_SLAVE2 endif ifeq ($(TARGET), ap) diff --git a/conf/firmwares/subsystems/shared/spi_master.makefile b/conf/firmwares/subsystems/shared/spi_master.makefile index 3dea6f2fbb9..b0e2f95674c 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.c $(SRC_ARCH)/mcu_periph/spi_arch.c +SPI_SRCS = mcu_periph/spi_pprzi.c $(SRC_ARCH)/mcu_periph/spi_arch.c ap.CFLAGS += $(SPI_CFLAGS) ap.srcs += $(SPI_SRCS) diff --git a/conf/telemetry/default_chibios.xml b/conf/telemetry/default_chibios.xml index 4e91339e07e..8605f51de7e 100644 --- a/conf/telemetry/default_chibios.xml +++ b/conf/telemetry/default_chibios.xml @@ -18,16 +18,21 @@ + + - - - - + + + + + + + diff --git a/sw/airborne/arch/chibios/mcu_periph/adc_arch.c b/sw/airborne/arch/chibios/mcu_periph/adc_arch.c index 37df922e080..756d1f2c8c9 100644 --- a/sw/airborne/arch/chibios/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/adc_arch.c @@ -54,10 +54,10 @@ uint8_t adc_error_flag = 0; ADCDriver* adcp_err = NULL; -#define ADC_NUM_CHANNELS 9 +#define ADC_NUM_CHANNELS 5 #ifndef ADC_BUF_DEPTH -#define ADC_BUF_DEPTH MAX_AV_NB_SAMPLE +#define ADC_BUF_DEPTH MAX_AV_NB_SAMPLE/2 #endif #define ADC_V_REF_MV 3300 @@ -90,24 +90,24 @@ static uint8_t adc1_samples_tmp[ADC_NUM_CHANNELS] = {0}; void adc1callback(ADCDriver *adcp, adcsample_t *buffer, size_t n) { if (adcp->state != ADC_STOP) { #if USE_AD1 - for(uint8_t channel = 0; channel < ADC_NUM_CHANNELS; channel+=2) { - if (adc1_buffers[channel/2] != NULL) { - adc1_sum_tmp[channel/2] = 0; - adc1_samples_tmp[channel/2] = n; - for (uint8_t sample = 0; sample < n; sample++) { - adc1_sum_tmp[channel/2] += buffer[channel + sample*ADC_NUM_CHANNELS]; - } - adc1_sum_tmp[channel/2] = (adc1_sum_tmp[channel/2])*ADC_V_REF_MV/ADC_12_BIT_RESOLUTION; - } - } - chSysLockFromIsr(); - for(uint8_t channel = 0; channel < ADC_NUM_CHANNELS; channel+=2) { - if (adc1_buffers[channel/2] != NULL) { - adc1_buffers[channel/2]->sum = adc1_sum_tmp[channel/2]; - adc1_buffers[channel/2]->av_nb_sample = adc1_samples_tmp[channel/2]; - } - } - chSysUnlockFromIsr(); + for(uint8_t channel = 0; channel < ADC_NUM_CHANNELS; channel++) { + if (adc1_buffers[channel] != NULL) { + adc1_sum_tmp[channel] = 0; + adc1_samples_tmp[channel] = n; + for (uint8_t sample = 0; sample < n; sample++) { + adc1_sum_tmp[channel] += buffer[channel + sample*ADC_NUM_CHANNELS]; + } + adc1_sum_tmp[channel] = (adc1_sum_tmp[channel])*ADC_V_REF_MV/ADC_12_BIT_RESOLUTION; + } + } + chSysLockFromIsr(); + for(uint8_t channel = 0; channel < ADC_NUM_CHANNELS; channel++) { + if (adc1_buffers[channel] != NULL) { + adc1_buffers[channel]->sum = adc1_sum_tmp[channel]; + adc1_buffers[channel]->av_nb_sample = adc1_samples_tmp[channel]; + } + } + chSysUnlockFromIsr(); #endif } } @@ -145,8 +145,9 @@ static const ADCConversionGroup adcgrpcfg = { ADC_SMPR1_SMP_AN13(ADC_SAMPLE_41P5) | ADC_SMPR1_SMP_AN14(ADC_SAMPLE_41P5) | ADC_SMPR1_SMP_SENSOR(ADC_SAMPLE_7P5), 0, ADC_SQR1_NUM_CH(ADC_NUM_CHANNELS), - ADC_SQR2_SQ9_N(ADC_CHANNEL_SENSOR) |ADC_SQR2_SQ7_N(ADC_CHANNEL_IN14), - ADC_SQR3_SQ5_N(ADC_CHANNEL_IN11) | ADC_SQR3_SQ3_N(ADC_CHANNEL_IN10) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN13) + 0, + ADC_SQR3_SQ5_N(ADC_CHANNEL_SENSOR) | ADC_SQR3_SQ4_N(ADC_CHANNEL_IN11) | + ADC_SQR3_SQ3_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ2_N(ADC_CHANNEL_IN10) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN13) }; /** diff --git a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c index a5b6884ba0b..50635b8d260 100644 --- a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c @@ -101,54 +101,64 @@ void i2c_setbitrate(struct i2c_periph* p, int bitrate){ * (such as i2c_transmit(), i2c_transcieve()) and ChibiOS i2c * transmit function i2cMasterTransmitTimeout() * + * Note that we are using the same buffer for transmit and recevive. It is + * OK because in i2c transaction is Tx always before Rx. + * * @param[in] p pointer to a @p i2c_periph struct * @param[in] t pointer to a @p i2c_transaction struct */ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t){ +#if USE_I2C1 || USE_I2C2 || USE_I2C3 static msg_t status = RDY_OK; static systime_t tmo = US2ST(1000000/PERIODIC_FREQUENCY); - i2cAcquireBus(&I2CD2); + i2cAcquireBus((I2CDriver*)p->reg_addr)); status = i2cMasterTransmitTimeout((I2CDriver*)p->reg_addr, (i2caddr_t)((t->slave_addr)>>1), - t->buf, (size_t)(t->len_w), t->buf, (size_t)(t->len_r), tmo); - i2cReleaseBus(&I2CD2); + t->buf, (size_t)(t->len_w), t->buf, (size_t)(t->len_r), tmo); + 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 + } /** * i2c_idle() function * - * * Empty, for paparazzi compatibility only + * Empty, for paparazzi compatibility only */ bool_t i2c_idle(struct i2c_periph* p){ (void) p; diff --git a/sw/airborne/arch/chibios/mcu_periph/spi_arch.c b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c new file mode 100644 index 00000000000..e5f25cab2f2 --- /dev/null +++ b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c @@ -0,0 +1,317 @@ +/* + * Copyright (C) 2013 AggieAir, A Remote Sensing Unmanned Aerial System for Scientific Applications + * Utah State University, http://aggieair.usu.edu/ + * + * Michal Podhradsky (michal.podhradsky@aggiemail.usu.edu) + * Calvin Coopmans (c.r.coopmans@ieee.org) + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ +/** + * @brief chibios arch dependant implementation of SPI interface + * @note Assume SPI master for now + */ +#include "mcu_periph/spi.h" + +#if SPI_SLAVE +#error "ChibiOS operates only in SPI_MASTER mode" +#endif + +#if USE_SPI0 +#error "ChibiOS architectures don't have SPI0" +#endif +#if USE_SPI1 +void spi1_arch_init(void) { + spi1.reg_addr = &SPID1; +} +#endif +#if USE_SPI2 +void spi2_arch_init(void) { + spi2.reg_addr = &SPID2; +} +#endif +#if USE_SPI3 +void spi3_arch_init(void) { + spi3.reg_addr = &SPID3; +} +#endif + +/** + * Resolve slave port + * + * Given the slave number and the board config file, returns the right + * port (i.e. GPIOC) + * + * @param[in] slave index number of a slave + */ +static inline ioportid_t spi_resolve_slave_port(uint8_t slave) { + switch(slave) { +#if USE_SPI_SLAVE0 + case 0: + return SPI_SELECT_SLAVE0_PORT; + break; +#endif // USE_SPI_SLAVE0 +#if USE_SPI_SLAVE1 + case 1: + return SPI_SELECT_SLAVE1_PORT; + break; +#endif //USE_SPI_SLAVE1 +#if USE_SPI_SLAVE2 + case 2: + return SPI_SELECT_SLAVE2_PORT; + break; +#endif //USE_SPI_SLAVE2 +#if USE_SPI_SLAVE3 + case 3: + return SPI_SELECT_SLAVE3_PORT; + break; +#endif //USE_SPI_SLAVE3 +#if USE_SPI_SLAVE4 + case 4: + return SPI_SELECT_SLAVE4_PORT; + break; +#endif //USE_SPI_SLAVE4 +#if USE_SPI_SLAVE5 + case 5: + return SPI_SELECT_SLAVE5_PORT; + break; +#endif //USE_SPI_SLAVE5 + default: + return 0; + break; + } +} + +/** + * Resolve slave port + * + * Given the slave number and the board config file, returns the right + * pin (i.e. 12) + * + * @param[in] slave index number of a slave + */ +static inline uint16_t spi_resolve_slave_pin(uint8_t slave) { + switch(slave) { +#if USE_SPI_SLAVE0 + case 0: + return SPI_SELECT_SLAVE0_PIN; + break; +#endif // USE_SPI_SLAVE0 +#if USE_SPI_SLAVE1 + case 1: + return SPI_SELECT_SLAVE1_PIN; + break; +#endif //USE_SPI_SLAVE1 +#if USE_SPI_SLAVE2 + case 2: + return SPI_SELECT_SLAVE2_PIN; + break; +#endif //USE_SPI_SLAVE2 +#if USE_SPI_SLAVE3 + case 3: + return SPI_SELECT_SLAVE3_PIN; + break; +#endif //USE_SPI_SLAVE3 +#if USE_SPI_SLAVE4 + case 4: + return SPI_SELECT_SLAVE4_PIN; + break; +#endif //USE_SPI_SLAVE4 +#if USE_SPI_SLAVE5 + case 5: + return SPI_SELECT_SLAVE5_PIN; + break; +#endif //USE_SPI_SLAVE5 + default: + return 0; + break; + } +} + +/** + * Resolve CR1 + * + * Given the transaction settings, returns the right configuration of + * SPIx_CR1 register. + * + * @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; + } + if (t->bitorder == SPILSBFirst) { + CR1 |= SPI_CR1_LSBFIRST; + } + if (t->cpha == SPICphaEdge2) { + CR1 |= SPI_CR1_CPHA; + } + if (t->cpol == SPICpolIdleHigh) { + 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; + } +#endif + return CR1; +} + + +/** + * Submit SPI transaction + * + * Interafces Paparazzi SPI code with ChibiOS SPI driver. + * The transaction length is max(rx,tx), before and after + * 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. + * + * @param[in] p pointer to a @p spi_periph struct + * @param[in] t pointer to a @p spi_transaction struct + */ +bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) +{ + SPIConfig spi_cfg = { + NULL, /// no callback + spi_resolve_slave_port(t->slave_idx), + spi_resolve_slave_pin(t->slave_idx), + spi_resolve_CR1(t) +#ifdef __STM32F4xx_H + ,spi_resolve_CR2(t) +#endif + }; + + // find max transaction length + static size_t t_length; + if (t->input_length >= t->output_length) { + t_length = (size_t)t->input_length; + } + else { + t_length = (size_t)t->output_length; + } + + /// Acquire exclusive access to the spi bus + spiAcquireBus((SPIDriver*)p->reg_addr); + + /// 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 + if (t->before_cb != 0) { + t->before_cb(t); + } + + /// Start asynchronous data transfer + spiExchange((SPIDriver*)p->reg_addr, t_length, t->output_buf, t->input_buf); + + spiUnselect((SPIDriver*)p->reg_addr); + + /// Release the exclusive access to the bus + spiReleaseBus((SPIDriver*)p->reg_addr); + + + /// Report the transaction as success + t->status = SPITransSuccess; + + /// 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); + } + return TRUE; +} + + + +/** + * spi_slave_select() function + * + * Empty, for paparazzi compatibility only + */ +void spi_slave_select(uint8_t slave) { + (void) slave; +} + +/** + * spi_slave_unselect() function + * + * Empty, for paparazzi compatibility only + */ +void spi_slave_unselect(uint8_t slave) { + (void) slave; +} + +/** + * spi_lock() function + * + * Empty, for paparazzi compatibility only + */ +bool_t spi_lock(struct spi_periph* p, uint8_t slave) { + (void) slave; + (void) p; + return TRUE; +} + +/** + * spi_resume() function + * + * Empty, for paparazzi compatibility only + */ +bool_t spi_resume(struct spi_periph* p, uint8_t slave) { + (void) slave; + (void) p; + return TRUE; +} + +/** + * spi_init_slaves() function + * + * Empty, for paparazzi compatibility only + */ +void spi_init_slaves(void) {} diff --git a/sw/airborne/arch/chibios/mcu_periph/spi_arch.h b/sw/airborne/arch/chibios/mcu_periph/spi_arch.h new file mode 100644 index 00000000000..395a3f9032e --- /dev/null +++ b/sw/airborne/arch/chibios/mcu_periph/spi_arch.h @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2013 AggieAir, A Remote Sensing Unmanned Aerial System for Scientific Applications + * Utah State University, http://aggieair.usu.edu/ + * + * Michal Podhradsky (michal.podhradsky@aggiemail.usu.edu) + * Calvin Coopmans (c.r.coopmans@ieee.org) + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ +/** + * @brief chibios arch dependant implementation of SPI interface + */ +#ifndef SPI_ARCH_H +#define SPI_ARCH_H + + +#endif // SPI_ARCH_H 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 9dc8f228187..9e35d63ca49 100644 --- a/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.h @@ -40,6 +40,8 @@ extern uint32_t cpu_counter; extern uint32_t idle_counter; extern uint8_t cpu_frequency; +#define SysTimeTimerStart(_t) {} + /* * FIXME: Not implemented */ diff --git a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c index 79ba6e223df..5032bffe78b 100644 --- a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c @@ -130,7 +130,7 @@ void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_en * Uart transmit implementation */ void uart_transmit(struct uart_periph* p, uint8_t data ) { - chnWrite((SerialDriver*)p->reg_addr, &data, sizeof(data)); + sdWrite((SerialDriver*)p->reg_addr, &data, sizeof(data)); } /* @@ -140,5 +140,5 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) { * uart_transmit_buffer(&uart2, tx_switch, sizeof(tx_switch)); */ void uart_transmit_buffer(struct uart_periph* p, uint8_t* data_buffer, size_t length) { - chnWrite((SerialDriver*)p->reg_addr, data_buffer, length); + sdWrite((SerialDriver*)p->reg_addr, data_buffer, length); } diff --git a/sw/airborne/arch/chibios/mcu_periph/uart_arch.h b/sw/airborne/arch/chibios/mcu_periph/uart_arch.h index abe30f9736c..118d0579634 100644 --- a/sw/airborne/arch/chibios/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/chibios/mcu_periph/uart_arch.h @@ -40,12 +40,37 @@ #define B19200 19200 #define B38400 38400 #define B57600 57600 -#define B100000 100000 #define B115200 115200 #define B230400 230400 #define B921600 921600 +#define B100000 100000 + +#define ch_uart_receive_downlink(_port, _flag, _callback, _arg) { \ + 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(_arg, charbuf); \ + } \ + } \ + while (charbuf != Q_TIMEOUT); \ + } \ +} -#define UART_GETCH(_port, _flag, _callback) { \ +#define ch_uart_receive(_port, _flag, _callback) { \ if ((_flag & (SD_FRAMING_ERROR | SD_OVERRUN_ERROR | \ SD_NOISE_ERROR)) != 0) { \ if (_flag & SD_OVERRUN_ERROR) { \ @@ -70,4 +95,4 @@ } \ } -#endif /* STM32_UART_ARCH_H */ +#endif /* STM32_UART_ARCH_H */ \ No newline at end of file diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c index 545bb243755..558e5351971 100644 --- a/sw/airborne/boards/baro_board_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -60,6 +60,10 @@ void baro_periodic(void) { /* call the convenience periodic that initializes the sensor and starts reading*/ ms5611_spi_periodic(&bb_ms5611); +#ifdef USE_CHIBIOS_RTOS + baro_event(); +#endif + #if DEBUG if (bb_ms5611.initialized) RunOnceEvery((50*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, @@ -77,7 +81,9 @@ void baro_periodic(void) { 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/boards/lia/baro_board.h b/sw/airborne/boards/lia/baro_board.h new file mode 100644 index 00000000000..bb080094aba --- /dev/null +++ b/sw/airborne/boards/lia/baro_board.h @@ -0,0 +1,15 @@ + +/* + * board specific functions for the lisa_m board + * + */ + +#ifndef BOARDS_LISA_M_BARO_H +#define BOARDS_LISA_M_BARO_H + +// for right now we abuse this file for the ms5611 baro on aspirin as well + +extern void baro_event(void); +#define BaroEvent baro_event + +#endif /* BOARDS_LISA_M_BARO_H */ diff --git a/sw/airborne/boards/lia/chibios/board.h b/sw/airborne/boards/lia/chibios/board.h index d4721850e88..91bdd7628f4 100644 --- a/sw/airborne/boards/lia/chibios/board.h +++ b/sw/airborne/boards/lia/chibios/board.h @@ -69,7 +69,8 @@ /* * Port A setup. - * PA0 - B - Alternate Push Pull output 50MHz (SERVO5) + * PA0 - 4 - Digital input (PPM_IN) + * - B - Alternate Push Pull output 50MHz (SERVO5) * PA1 - B - Alternate Push Pull output 50MHz (SERVO6) * PA2 - B - Alternate Push Pull output 50MHz (UART2_TX) * PA3 - 4 - Digital input (UART2_RX) @@ -104,15 +105,15 @@ * - 7 - Open Drain output 50MHz. (I2C1_SDA) * PB8 - 4 - Digital input. (CAN_RX) * PB9 - 7 - Open Drain output 50MHz. (CAN_TX) - * PB10 - 7 - Open Drain output 50MHz. (I2C2_SCL) - * PB11 - 7 - Open Drain output 50MHz. (I2C2_SDA) + * PB10 - E - Alternate Open Drain output 2MHz.(I2C2_SCL) + * PB11 - E - Alternate Open Drain output 2MHz.(I2C2_SDA) * PB12 - 3 - Push Pull output 50MHz. (IMU_ACC_CS) * PB13 - B - Alternate Push Pull output 50MHz (IMU_SPI_SCK) * PB14 - 4 - Digital input (IMU_SPI_MISO) * PB15 - B - Alternate Push Pull output 50MHz (IMU_SPI_MOSI) */ #define VAL_GPIOBCRL 0xBB474444 /* PB7...PB0 */ -#define VAL_GPIOBCRH 0xB4B37774 /* PB15...PB8 */ +#define VAL_GPIOBCRH 0xB4B3EE74 /* PB15...PB8 */ #define VAL_GPIOBODR 0xFFFFFFFF /* @@ -193,14 +194,6 @@ #define LED_5_GPIO GPIOC #define LED_5_GPIO_PIN 15 -/* - * PPM radio defines - */ -#define RC_PPM_TICKS_PER_USEC 6 -#define PPM_TIMER_FREQUENCY 1000000 -#define PPM_CHANNEL ICU_CHANNEL_3 -#define PPM_TIMER ICUD1 - /* * ADCs */ @@ -211,7 +204,7 @@ #define BOARD_ADC_CHANNEL_4 14 /* - * provide defines that can be used to access the ADC_x in the code or airframe file + * provide defines that can be used to access the ADC_x in the code or airframe file * these directly map to the index number of the 4 adc channels defined above * 4th (index 3) is used for bat monitoring by default */ @@ -236,10 +229,12 @@ #define USE_AD1_4 1 #define DefaultVoltageOfAdc(adc) (0.0047*adc) + +// Read the electrical characteristics for STM32F105 chip #define CpuTempOfAdc(adc) ((1430 - adc)/4.3+25) /* - * PWM + * PWM defines */ #define PWM_FREQUENCY_1MHZ 1000000 #define PWM_CMD_TO_US(_t) _t @@ -268,18 +263,19 @@ #define PWM_SERVO_6_DRIVER PWMD5 #define PWM_SERVO_6_CHANNEL 1 -#define PWM_SERVO_7 6 -#define PWM_SERVO_7_DRIVER PWMD4 -#define PWM_SERVO_7_CHANNEL 0 - -#define PWM_SERVO_8 7 -#define PWM_SERVO_8_DRIVER PWMD4 -#define PWM_SERVO_8_CHANNEL 1 #if USE_SERVOS_7AND8 #if USE_I2C1 #error "You cannot USE_SERVOS_7AND8 and USE_I2C1 at the same time" #else + #define PWM_SERVO_7 6 + #define PWM_SERVO_7_DRIVER PWMD4 + #define PWM_SERVO_7_CHANNEL 0 + + #define PWM_SERVO_8 7 + #define PWM_SERVO_8_DRIVER PWMD4 + #define PWM_SERVO_8_CHANNEL 1 + #define ACTUATORS_PWM_NB 8 #define PWM_CONF_TIM3 1 #define PWM_CONF_TIM4 1 @@ -356,6 +352,53 @@ } #endif + +/** + * PPM radio defines + */ +#define RC_PPM_TICKS_PER_USEC 6 +#define PPM_TIMER_FREQUENCY 6000000 +#define PPM_CHANNEL ICU_CHANNEL_3 +#define PPM_TIMER ICUD1 + +/** + * I2C2 defines + */ +#define I2C2_CLOCK_SPEED 300000 +#define I2C2_CFG_DEF { \ + OPMODE_I2C, \ + I2C2_CLOCK_SPEED, \ + FAST_DUTY_CYCLE_2, \ + } + +/** + * SPI Config + * + * Just defines which make sense for Lia board + */ +#define SPI_SELECT_SLAVE1_PORT GPIOA +#define SPI_SELECT_SLAVE1_PIN 4 + +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN 12 + +#define SPI_SELECT_SLAVE3_PORT GPIOC +#define SPI_SELECT_SLAVE3_PIN 13 + +#define SPI_SELECT_SLAVE4_PORT GPIOC +#define SPI_SELECT_SLAVE4_PIN 12 + +/** + * Baro + * + * Apparently needed for backwards compatibility + * with the ancient onboard baro boards + */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif + + #if !defined(_FROM_ASM_) #ifdef __cplusplus extern "C" { diff --git a/sw/airborne/boards/lia/chibios/halconf.h b/sw/airborne/boards/lia/chibios/halconf.h index 73dc07aaf50..81fccc4aa61 100644 --- a/sw/airborne/boards/lia/chibios/halconf.h +++ b/sw/airborne/boards/lia/chibios/halconf.h @@ -94,7 +94,7 @@ * @brief Enables the ICU subsystem. */ #if !defined(HAL_USE_ICU) || defined(__DOXYGEN__) -#define HAL_USE_ICU FALSE +#define HAL_USE_ICU TRUE #endif /** @@ -150,7 +150,7 @@ * @brief Enables the SPI subsystem. */ #if !defined(HAL_USE_SPI) || defined(__DOXYGEN__) -#define HAL_USE_SPI FALSE +#define HAL_USE_SPI TRUE #endif /** diff --git a/sw/airborne/boards/lia/chibios/mcuconf.h b/sw/airborne/boards/lia/chibios/mcuconf.h index 2243179b6ce..4ef594f335e 100644 --- a/sw/airborne/boards/lia/chibios/mcuconf.h +++ b/sw/airborne/boards/lia/chibios/mcuconf.h @@ -40,7 +40,7 @@ * HAL driver system settings. */ #define STM32_NO_INIT FALSE -#define STM32_HSI_ENABLED TRUE//was true +#define STM32_HSI_ENABLED TRUE #define STM32_LSI_ENABLED FALSE #define STM32_HSE_ENABLED TRUE #define STM32_LSE_ENABLED FALSE @@ -49,7 +49,7 @@ #define STM32_PREDIV1SRC STM32_PREDIV1SRC_HSE #define STM32_PREDIV1_VALUE 1 #define STM32_PLLMUL_VALUE 6 -#define STM32_PREDIV2_VALUE 3//we are not using PLL2/3 +#define STM32_PREDIV2_VALUE 3 #define STM32_PLL2MUL_VALUE 10 #define STM32_PLL3MUL_VALUE 12 #define STM32_HPRE STM32_HPRE_DIV1 @@ -125,7 +125,7 @@ /* * ICU driver system settings. */ -#define STM32_ICU_USE_TIM1 FALSE +#define STM32_ICU_USE_TIM1 TRUE #define STM32_ICU_USE_TIM2 FALSE #define STM32_ICU_USE_TIM3 FALSE #define STM32_ICU_USE_TIM4 FALSE @@ -163,22 +163,22 @@ /* * SERIAL driver system settings. */ -#define STM32_SERIAL_USE_USART1 TRUE +#define STM32_SERIAL_USE_USART1 FALSE #define STM32_SERIAL_USE_USART2 TRUE -#define STM32_SERIAL_USE_USART3 TRUE +#define STM32_SERIAL_USE_USART3 FALSE #define STM32_SERIAL_USE_UART4 FALSE #define STM32_SERIAL_USE_UART5 TRUE #define STM32_SERIAL_USART1_PRIORITY 12 #define STM32_SERIAL_USART2_PRIORITY 10 #define STM32_SERIAL_USART3_PRIORITY 9 -#define STM32_SERIAL_UART4_PRIORITY 13 +#define STM32_SERIAL_UART4_PRIORITY 15 #define STM32_SERIAL_UART5_PRIORITY 11 /* * SPI driver system settings. */ #define STM32_SPI_USE_SPI1 FALSE -#define STM32_SPI_USE_SPI2 FALSE +#define STM32_SPI_USE_SPI2 TRUE #define STM32_SPI_USE_SPI3 FALSE #define STM32_SPI_SPI1_DMA_PRIORITY 1 #define STM32_SPI_SPI2_DMA_PRIORITY 1 @@ -196,7 +196,7 @@ #define STM32_UART_USE_USART3 FALSE #define STM32_UART_USART1_IRQ_PRIORITY 12 #define STM32_UART_USART2_IRQ_PRIORITY 12 -#define STM32_UART_USART3_IRQ_PRIORITY 12 +#define STM32_UART_USART3_IRQ_PRIORITY 8 #define STM32_UART_USART1_DMA_PRIORITY 0 #define STM32_UART_USART2_DMA_PRIORITY 0 #define STM32_UART_USART3_DMA_PRIORITY 0 diff --git a/sw/airborne/firmwares/rotorcraft/main_chibios.c b/sw/airborne/firmwares/rotorcraft/main_chibios.c index f944c3ccb33..0a79639f413 100644 --- a/sw/airborne/firmwares/rotorcraft/main_chibios.c +++ b/sw/airborne/firmwares/rotorcraft/main_chibios.c @@ -35,15 +35,11 @@ * @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" @@ -89,25 +85,104 @@ #include "generated/modules.h" #include "subsystems/abi.h" -/* - * Thread definitions - */ + +/// Thread 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); + 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 #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); + +/** + * GPS callback + * + * The same functionality as on_gps_event() in + * standard paparazzi + */ +void on_gps_event(void) { + //chMtxLock(&ahrs_states_mutex_flag); + ahrs_update_gps(); + //chMtxUnlock(); + + //chMtxLock(&ins_data_flag); + ins_update_gps(); + //chMtxUnlock(); +} #endif #ifdef USE_IMU -static WORKING_AREA(wa_thd_imu_rx, CH_THREAD_AREA_IMU_RX); -static WORKING_AREA(wa_thd_imu_tx, CH_THREAD_AREA_IMU_TX); +#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 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 Mag callback + */ + void on_mag_event(void) { + ImuScaleMag(imu); + #if USE_MAGNETOMETER + if (ahrs.status == AHRS_RUNNING) { + ahrs_update_mag(); + } + #endif + } + #endif #ifdef MODULES_C __attribute__((noreturn)) msg_t thd_modules_periodic(void *arg); @@ -116,23 +191,42 @@ __attribute__((noreturn)) msg_t thd_modules_periodic(void *arg); /* if PRINT_CONFIG is defined, print some config options */ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) -/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h +/** + * 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 +/** + * MODULES_FREQUENCY is defined in generated/modules.h * according to main_freq parameter set for modules in airframe file */ PRINT_CONFIG_VAR(MODULES_FREQUENCY) #ifndef BARO_PERIODIC_FREQUENCY -#define BARO_PERIODIC_FREQUENCY 50 +#define BARO_PERIODIC_FREQUENCY 100 #endif PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY) -/* +#ifndef FAILSAFE_FREQUENCY +#define FAILSAFE_FREQUENCY 20 +#endif +PRINT_CONFIG_VAR(FAILSAFE_FREQUENCY) + +#ifndef ELECTRICAL_PERIODIC_FREQ +#define ELECTRICAL_PERIODIC_FREQ 10 +#endif +PRINT_CONFIG_VAR(ELECTRICAL_PERIODIC_FREQ) + +#ifndef RADIO_CONTROL_FREQ +#define RADIO_CONTROL_FREQ 60 +#endif +PRINT_CONFIG_VAR(RADIO_CONTROL_FREQ) + +/** * HeartBeat & System Info + * + * 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) @@ -148,6 +242,11 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) LED_TOGGLE(SYS_TIME_LED); sys_time.nb_sec++; + if (autopilot_in_flight) { + autopilot_flight_time++; + datalink_time++; + } + core_free_memory = chCoreStatus(); thread_counter = 0; @@ -156,7 +255,7 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) do { thread_counter++; if (tp ==chSysGetIdleThread()) { - idle_counter = (uint32_t)tp->p_time; + idle_counter = (uint32_t)tp->p_time; } tp = chRegNextThread(tp); } while (tp != NULL); @@ -171,9 +270,114 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) } } -#ifdef DOWNLINK /* - * Telemetry TX + * Failsafe Thread + * @note: Replaces failsafe_periodic(), eventually + * it will check also other threads (~hypervisor) + */ +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) { + time += US2ST(1000000/FAILSAFE_FREQUENCY); + failsafe_check(); + //TODO: ChibiOS/RT failsafe check + chThdSleepUntil(time); + } +} + +/* + * Electrical Periodic Thread + * @note: 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) + { + 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. + */ + 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); + } + } + } + } + +#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); + } + } + +#endif + + +#ifdef DOWNLINK +/** + * Telemetry TX thread + * * Replaces telemetryPeriodic() */ static WORKING_AREA(wa_thd_telemetry_tx, 1024); @@ -189,9 +393,11 @@ __attribute__((noreturn)) msg_t thd_telemetry_tx(void *arg) } } -/* - * Telemetry RX +/** + * Telemetry RX thread + * * Replaces DatalinkEvent() + * * @note: assumes PprziDwonlink for now * @todo General definition for different links */ @@ -206,37 +412,11 @@ __attribute__((noreturn)) msg_t thd_telemetry_rx(void *arg) while (TRUE) { chEvtWaitOneTimeout(EVENT_MASK(1), S2ST(1)); - chSysLock(); flags = chEvtGetAndClearFlags(&elTelemetryRx); - chSysUnlock(); - if ((flags & (SD_FRAMING_ERROR | SD_OVERRUN_ERROR | - SD_NOISE_ERROR)) != 0) { - if (flags & SD_OVERRUN_ERROR) { - DOWNLINK_PORT.ore++; - } - if (flags & SD_NOISE_ERROR) { - DOWNLINK_PORT.ne_err++; - } - if (flags & SD_FRAMING_ERROR) { - DOWNLINK_PORT.fe_err++; - } - } - if (flags & CHN_INPUT_AVAILABLE) - { - msg_t charbuf; - do - { - charbuf = chnGetTimeout((SerialDriver*)DOWNLINK_PORT.reg_addr, TIME_IMMEDIATE); - if ( charbuf != Q_TIMEOUT ) - { - parse_pprz(&pprz_tp, charbuf); - } - } - while (charbuf != Q_TIMEOUT); - } + ch_uart_receive_downlink(DOWNLINK_PORT, flags, parse_pprz, &pprz_tp); if (pprz_tp.trans.msg_received) { - pprz_parse_payload(&(pprz_tp)); \ - pprz_tp.trans.msg_received = FALSE; \ + pprz_parse_payload(&(pprz_tp)); + pprz_tp.trans.msg_received = FALSE; dl_parse_msg(); dl_msg_available = FALSE; } @@ -245,7 +425,7 @@ __attribute__((noreturn)) msg_t thd_telemetry_rx(void *arg) #endif #ifdef MODULES_C -/* +/** * Modules periodic tasks */ static WORKING_AREA(wa_thd_modules_periodic, 1024); @@ -262,20 +442,71 @@ __attribute__((noreturn)) msg_t thd_modules_periodic(void *arg) } #endif + +/** + * Paparazzi failsafe thread + */ +static inline void failsafe_check(void) { + if (radio_control.status != RC_OK && + 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) + { + autopilot_set_mode(AP_MODE_FAILSAFE); + } +#endif + +#if USE_GPS + if (autopilot_mode == AP_MODE_NAV && + autopilot_motors_on && +#if NO_GPS_LOST_WITH_RC_VALID + radio_control.status != RC_OK && +#endif + GpsIsLost()) + { + autopilot_set_mode(AP_MODE_FAILSAFE); + } +#endif + + autopilot_check_in_flight(autopilot_motors_on); +} + + /* * Thread initialization + * @note: Done here, not in the submodules, so we don't + * have to include ChibiOs headers to each submodule */ -void thread_init(void) { +static void thread_init(void) { chThdCreateStatic(wa_thd_heartbeat, sizeof(wa_thd_heartbeat), IDLEPRIO, thd_heartbeat, NULL); +chThdCreateStatic(wa_thd_electrical, sizeof(wa_thd_electrical), LOWPRIO, thd_electrical, NULL); +chThdCreateStatic(wa_thd_radio_control, sizeof(wa_thd_radio_control), NORMALPRIO, thd_radio_control, NULL); +chThdCreateStatic(wa_thd_radio_event, sizeof(wa_thd_radio_event), NORMALPRIO, thd_radio_event, NULL); + +#if USE_BARO_BOARD + chThdCreateStatic(wa_thd_baro, sizeof(wa_thd_baro),LOWPRIO, thd_baro, NULL); +#endif #ifdef USE_IMU - chThdCreateStatic(wa_thd_imu_rx, sizeof(wa_thd_imu_rx),NORMALPRIO, thd_imu_rx, NULL); - chThdCreateStatic(wa_thd_imu_tx, sizeof(wa_thd_imu_tx),NORMALPRIO, thd_imu_tx, NULL); +#ifdef INIT_IMU_THREAD + chThdCreateStatic(wa_thd_imu_rx, sizeof(wa_thd_imu_rx), HIGHPRIO, thd_imu_rx, NULL); +#else + imu_init(); + #if USE_IMU_FLOAT + imu_float_init(); + #endif +#endif #endif #ifdef DOWNLINK - chThdCreateStatic(wa_thd_telemetry_rx, sizeof(wa_thd_telemetry_rx),LOWPRIO, thd_telemetry_rx, NULL); - chThdCreateStatic(wa_thd_telemetry_tx, sizeof(wa_thd_telemetry_tx),LOWPRIO, thd_telemetry_tx, 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 @@ -283,17 +514,20 @@ chThdCreateStatic(wa_thd_heartbeat, sizeof(wa_thd_heartbeat), IDLEPRIO, thd_hear #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 goes to sleep. - * Eventually Main thread will be turned into Idle thread. + * + * Initializes system (both chibios and paparazzi), + * then turns into main thread - main_periodic() */ int main(void) { - /* + /** * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. @@ -303,35 +537,36 @@ int main(void) { halInit(); chSysInit(); - /* - * Paparazzi initialization - */ + /// Paparazzi initialization mcu_init(); - thread_init(); electrical_init(); + stateInit(); -#ifdef USE_ACTUATORS + actuators_init(); -#endif + #if USE_MOTOR_MIXING motor_mixing_init(); #endif -#ifdef USE_RADIO_CONTROL + radio_control_init(); -#endif + air_data_init(); -#if USE_BARO_BOARD - baro_init(); -#endif - imu_init(); -#if USE_IMU_FLOAT - imu_float_init(); -#endif - ahrs_aligner_init(); - ahrs_init(); + #if USE_BARO_BOARD + baro_init(); + #endif + + + imu_init(); + #if USE_IMU_FLOAT + imu_float_init(); + #endif + + ahrs_aligner_init(); + ahrs_init(); - ins_init(); + ins_init(); #if USE_GPS gps_init(); @@ -352,13 +587,19 @@ int main(void) { udp_init(); #endif + thread_init(); - /* - * Normal main() thread activity, in this demo it does nothing except - * sleeping in a loop. Eventually we want to modify main to an idle thread. - */ + chThdSetPriority (HIGHPRIO); + + chThdSleep(MS2ST(1500)); + systime_t main_time = chTimeNow(); while (TRUE) { - chThdSleepMilliseconds(500); + main_time += US2ST(1000000/PERIODIC_FREQUENCY); + imu_periodic(); + autopilot_periodic(); + SetActuatorsFromCommands(commands, autopilot_mode); + chThdSleepUntil(main_time); } - return 1; + + return TRUE; } diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 9a513bec2df..36ecd404d23 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -49,9 +49,8 @@ #include "subsystems/ins.h" #include "subsystems/ahrs.h" // I2C Error counters -#ifndef USE_CHIBIOS_RTOS #include "mcu_periph/i2c.h" -#endif + #define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM) diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h index 60769eaeb4b..693c3f7c37e 100644 --- a/sw/airborne/mcu_periph/spi.h +++ b/sw/airborne/mcu_periph/spi.h @@ -35,6 +35,10 @@ #include "mcu_periph/spi_arch.h" +#ifdef USE_CHIBIOS_RTOS +#include "hal.h" +#endif + /** * @addtogroup mcu_periph * @{ @@ -140,8 +144,13 @@ 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 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 diff --git a/sw/airborne/mcu_periph/spi_pprzi.c b/sw/airborne/mcu_periph/spi_pprzi.c new file mode 100644 index 00000000000..3434711c33c --- /dev/null +++ b/sw/airborne/mcu_periph/spi_pprzi.c @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2005-2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** + * @file mcu_periph/spi.c + * + * Architecture independent SPI (Serial Peripheral Interface) API. + */ + +#include "std.h" +#include "mcu_periph/spi.h" + +#if SPI_MASTER + +#if USE_SPI0 +struct spi_periph spi0; + +void spi0_init(void) { + spi_init(&spi0); + spi0_arch_init(); +} +#endif // USE_SPI0 + + +#if USE_SPI1 +struct spi_periph spi1; + +void spi1_init(void) { + spi_init(&spi1); + spi1_arch_init(); +} +#endif // USE_SPI1 + + +#if USE_SPI2 +struct spi_periph spi2; + +void spi2_init(void) { + spi_init(&spi2); + spi2_arch_init(); +} +#endif // USE_SPI2 + + +#if USE_SPI3 +struct spi_periph spi3; + +void spi3_init(void) { + spi_init(&spi3); + spi3_arch_init(); +} +#endif // USE_SPI3 + + +void spi_init(struct spi_periph* p) { + p->trans_insert_idx = 0; + p->trans_extract_idx = 0; + p->status = SPIIdle; + p->mode = SPIMaster; + p->suspend = FALSE; +} + +#endif /* SPI_MASTER */ + + +#if SPI_SLAVE + +#if USE_SPI0_SLAVE +struct spi_periph spi0; + +void spi0_slave_init(void) { + spi_slave_init(&spi0); + spi0_slave_arch_init(); +} +#endif // USE_SPI0_SLAVE + + +#if USE_SPI1_SLAVE +struct spi_periph spi1; + +void spi1_slave_init(void) { + spi_slave_init(&spi1); + spi1_slave_arch_init(); +} +#endif // USE_SPI1_SLAVE + + +#if USE_SPI2_SLAVE +struct spi_periph spi2; + +void spi2_slave_init(void) { + spi_slave_init(&spi2); + spi2_slave_arch_init(); +} +#endif // USE_SPI2_SLAVE + + +#if USE_SPI3_SLAVE +struct spi_periph spi3; + +void spi3_slave_init(void) { + spi_slave_init(&spi3); + spi3_slave_arch_init(); +} +#endif // USE_SPI3_SLAVE + + +extern void spi_slave_init(struct spi_periph* p) { + p->trans_insert_idx = 0; + p->trans_extract_idx = 0; + p->status = SPIIdle; + p->mode = SPISlave; + p->suspend = FALSE; +} + +#endif /* SPI_SLAVE */ diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h index 75975f851b1..3d0435a4f2a 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.h +++ b/sw/airborne/peripherals/mpu60x0_spi.h @@ -51,8 +51,13 @@ 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 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 d8e92931858..0fcca58b1df 100644 --- a/sw/airborne/peripherals/ms5611_spi.c +++ b/sw/airborne/peripherals/ms5611_spi.c @@ -89,6 +89,108 @@ void ms5611_spi_start_conversion(struct Ms5611_Spi *ms) */ void ms5611_spi_periodic_check(struct Ms5611_Spi *ms) { +#ifdef USE_CHIBIOS_RTOS + if (ms->initialized) { + if (ms->spi_trans.status == SPITransFailed) { + ms->status = MS5611_STATUS_IDLE; + ms->spi_trans.status = SPITransDone; + } + switch (ms->status) { + case MS5611_STATUS_IDLE: + ms->tx_buf[0] = MS5611_START_CONV_D1; + spi_submit(ms->spi_p, &(ms->spi_trans)); + ms->status = MS5611_STATUS_CONV_D1; + break; + case MS5611_STATUS_CONV_D1: + /* 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_ADC_D1: + /* read D1 (pressure) */ + ms->data.d1 = (ms->rx_buf[1] << 16) | + (ms->rx_buf[2] << 8) | + ms->rx_buf[3]; + if (ms->data.d1 == 0) { + /* if value is zero, it was read to soon and is invalid, back to idle */ + ms->status = MS5611_STATUS_IDLE; + } + else { + /* start D2 conversion */ + ms->tx_buf[0] = MS5611_START_CONV_D2; + spi_submit(ms->spi_p, &(ms->spi_trans)); + ms->status = MS5611_STATUS_CONV_D2; + } + break; + case MS5611_STATUS_CONV_D2: + /* 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; + case MS5611_STATUS_ADC_D2: + /* read D2 (temperature) */ + ms->data.d2 = (ms->rx_buf[1] << 16) | + (ms->rx_buf[2] << 8) | + ms->rx_buf[3]; + if (ms->data.d2 == 0) { + /* if value is zero, it was read to soon and is invalid, back to idle */ + ms->status = MS5611_STATUS_IDLE; + } + else { + /* calculate temp and pressure from measurements */ + ms5611_calc(&(ms->data)); + ms->status = MS5611_STATUS_IDLE; + ms->data_available = TRUE; + } + break; + + default: + ms->status = MS5611_STATUS_IDLE; + break; + } + } + else { + switch (ms->status) { + case MS5611_STATUS_UNINIT: + ms->initialized = FALSE; + ms->prom_cnt = 0; + ms->tx_buf[0] = MS5611_SOFT_RESET; + spi_submit(ms->spi_p, &(ms->spi_trans)); + ms->status = MS5611_STATUS_PROM; + break; + case MS5611_STATUS_PROM: + if (ms->spi_trans.status != SPITransFailed) { + ///No need to wait between EEPROM reads + /// TODO: SPI status check after each transaction + do { + /* get prom data */ + ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); + spi_submit(ms->spi_p, &(ms->spi_trans)); + /* read prom data */ + ms->data.c[ms->prom_cnt++] = (ms->rx_buf[1] << 8) | ms->rx_buf[2]; + } + while ((ms->prom_cnt < PROM_NB) && (ms->spi_trans.status != SPITransFailed)); + /* done reading prom, check prom crc */ + if (ms5611_prom_crc_ok(ms->data.c)) { + ms->initialized = TRUE; + ms->status = MS5611_STATUS_IDLE; + } + else { + ms->status = MS5611_STATUS_UNINIT; + } + } + else { + ms->status = MS5611_STATUS_UNINIT; + } + break; + default: + ms->status = MS5611_STATUS_UNINIT; + break; + } + } +#else switch (ms->status) { case MS5611_STATUS_RESET: ms->status = MS5611_STATUS_RESET_OK; @@ -98,7 +200,13 @@ void ms5611_spi_periodic_check(struct Ms5611_Spi *ms) /* start getting prom data */ ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); spi_submit(ms->spi_p, &(ms->spi_trans)); - ms->status = MS5611_STATUS_PROM; + } + 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: @@ -126,6 +234,7 @@ void ms5611_spi_periodic_check(struct Ms5611_Spi *ms) default: break; } +#endif } void ms5611_spi_event(struct Ms5611_Spi *ms) { @@ -150,7 +259,7 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) { else { /* start D2 conversion */ ms->tx_buf[0] = MS5611_START_CONV_D2; - spi_submit(ms->spi_p, &(ms->spi_trans)); + spi_submit(ms->spi_p, &(ms->spi_trans)); ms->status = MS5611_STATUS_CONV_D2; } break; @@ -192,21 +301,16 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) { /* read prom data */ ms->data.c[ms->prom_cnt++] = (ms->rx_buf[1] << 8) | ms->rx_buf[2]; - 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)); - } - else { - /* done reading prom, check prom crc */ - if (ms5611_prom_crc_ok(ms->data.c)) { - ms->initialized = TRUE; - ms->status = MS5611_STATUS_IDLE; - } - else { - /* checksum error, try again */ - ms->status = MS5611_STATUS_UNINIT; - } + if (ms->prom_cnt == PROM_NB) { + /* done reading prom, check prom crc */ + if (ms5611_prom_crc_ok(ms->data.c)) { + ms->initialized = TRUE; + ms->status = MS5611_STATUS_IDLE; + } + else { + /* checksum error, try again */ + ms->status = MS5611_STATUS_UNINIT; + } } } ms->spi_trans.status = SPITransDone; diff --git a/sw/airborne/peripherals/ms5611_spi.h b/sw/airborne/peripherals/ms5611_spi.h index 59f8f6e8e88..88435d6d4ed 100644 --- a/sw/airborne/peripherals/ms5611_spi.h +++ b/sw/airborne/peripherals/ms5611_spi.h @@ -36,8 +36,14 @@ 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 + enum Ms5611Status status; bool_t initialized; ///< config done flag volatile bool_t data_available; ///< data ready flag @@ -65,8 +71,11 @@ 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/ahrs/ahrs_aggienav.c b/sw/airborne/subsystems/ahrs/ahrs_aggienav.c index 1877825f04e..93f147fb648 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aggienav.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aggienav.c @@ -62,7 +62,9 @@ void ahrs_init(void) { #else ahrs_impl.mag_offset = 0.0; #endif - ahrs_aligner.status = AHRS_ALIGNER_LOCKED; + ///ahrs_aligner.status = AHRS_ALIGNER_LOCKED; + /// FIXME: Aligner not needed for GX3 + ahrs.status = AHRS_RUNNING; } void ahrs_aligner_run(void) { diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 67b98072b0f..c90494615eb 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -34,6 +34,10 @@ struct GpsState gps; struct GpsTimeSync gps_time_sync; +#ifdef USE_CHIBIOS_RTOS +Mutex gps_mutex_flag; +#endif + void gps_init(void) { gps.fix = GPS_FIX_NONE; gps.cacc = 0; @@ -43,6 +47,9 @@ void gps_init(void) { #ifdef GPS_TYPE_H gps_impl_init(); #endif +#ifdef USE_CHIBIOS_RTOS + chMtxInit(&gps_mutex_flag); +#endif } uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 2254f03d3c9..ccad965926e 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -44,6 +44,12 @@ #define GpsFixValid() (gps.fix == GPS_FIX_3D) +#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 #ifndef GPS_NB_CHANNELS #define GPS_NB_CHANNELS 1 diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 94180289416..755bc09f353 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -77,10 +77,6 @@ UbxSend2(len); \ } -#ifdef USE_CHIBIOS_RTOS -Mutex gps_mutex_flag; -#endif - struct GpsUbx gps_ubx; @@ -296,14 +292,13 @@ __attribute__((noreturn)) msg_t thd_gps_rx(void *arg) { chRegSetThreadName("pprz_gps_rx"); (void) arg; - chMtxInit(&gps_mutex_flag); 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); - UART_GETCH(GPS_PORT, flags, gps_ubx_parse); + ch_uart_receive(GPS_PORT, flags, gps_ubx_parse); if (gps_ubx.msg_available) { chMtxLock(&gps_mutex_flag); gps_ubx_read_message(); @@ -316,8 +311,7 @@ __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; } - ahrs_update_gps(); - ins_update_gps(); + on_gps_event(); } chMtxUnlock(); gps_ubx.msg_available = FALSE; diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index fbe163a48d7..eafb42dc231 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -32,15 +32,7 @@ #endif #ifdef USE_CHIBIOS_RTOS -//Chibios includes -#include "ch.h" -#include "hal.h" -//Callbacks includes -#include "subsystems/ahrs.h" -#include "subsystems/ins.h" #define CH_THREAD_AREA_GPS_RX 1024 -extern __attribute__((noreturn)) msg_t thd_gps_rx(void *arg); -extern Mutex gps_mutex_flag; #endif #include "mcu_periph/uart.h" diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index ef708b53ee5..bf5967193a0 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -151,6 +151,10 @@ 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 9ab98631cab..de391e1b428 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h @@ -57,8 +57,14 @@ 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 + volatile bool_t slave4_ready; };