diff --git a/WHATSNEW.txt b/WHATSNEW.txt index 236577e43..9f6e282ea 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -1,3 +1,12 @@ +--- RELEASE-13.06.03 --- + +This maintenance release addresses the following issues: +- FIxed CC3D attitude estimation failure after multiple settings changes and reboots. +- Fixed OPLink crashes when erasing settings + +JIRA issues addressed in this maintenance release: +OP-1049, OP-1050 + --- RELEASE-13.06.02 --- Refactoring of OPLink radio driver. Auto-configuration was removed, and a diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index 371068e0c..5ea27f0c8 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -64,7 +64,13 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg); static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU6000_GetReg(uint8_t address); -#define GRAV 9.81f +#define GRAV 9.81f + +#ifdef PIOS_MPU6000_ACCEL +#define PIOS_MPU6000_SAMPLES_BYTES 14 +#else +#define PIOS_MPU6000_SAMPLES_BYTES 8 +#endif /** * @brief Allocate a new device @@ -489,6 +495,62 @@ int32_t PIOS_MPU6000_Test(void) return 0; } +/** + * @brief Reads the contents of the MPU6000 Interrupt Status register from an ISR + * @return The register value or -1 on failure to claim the bus + */ +static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken) +{ + /* Interrupt Status register can be read at high SPI clock speed */ + uint8_t data; + + if (PIOS_MPU6000_ClaimBusISR(woken) != 0) { + return -1; + } + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG)); + data = PIOS_SPI_TransferByte(dev->spi_id, 0); + PIOS_MPU6000_ReleaseBusISR(woken); + return data; +} + +/** + * @brief Resets the MPU6000 FIFO from an ISR + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged + * @return 0 if operation was successful + * @return -1 if unable to claim SPI bus + * @return -2 if write to the device failed + */ +static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken) +{ + int32_t result = 0; + + /* Register writes must be at < 1MHz SPI clock. + * Speed can only be changed when SPI bus semaphore is held, but + * device chip select must not be enabled, so we use the direct + * SPI bus claim call here */ + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -1; + } + /* Reduce SPI clock speed. */ + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); + /* Enable chip select */ + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + /* Reset FIFO. */ + if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) { + result = -2; + } else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) { + result = -2; + } + /* Disable chip select. */ + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + /* Increase SPI clock speed. */ + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); + /* Release the SPI bus semaphore. */ + PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + return result; +} + /** * @brief Obtains the number of bytes in the FIFO. Call from ISR only. * @return the number of bytes in the FIFO @@ -542,8 +604,29 @@ bool PIOS_MPU6000_IRQHandler(void) return false; } + /* Temporary fix for OP-1049. Expected to be superceded for next major release + * by code changes for OP-1039. + * Read interrupt status register to check for FIFO overflow. Must be the + * first read after interrupt, in case the device is configured so that + * any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in + * interrupt config register) */ + int32_t result; + if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(&woken)) < 0) { + return woken; + } + if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) { + /* The FIFO has overflowed, so reset it, + * to enable sample sync to be recovered. + * If the reset fails, we are in trouble, but + * we keep trying on subsequent interrupts. */ + PIOS_MPU6000_ResetFifoISR(&woken); + /* Return and wait for the next new sample. */ + return woken; + } + + /* Usual case - FIFO has not overflowed. */ mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); - if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) { + if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) { return woken; } @@ -551,8 +634,8 @@ bool PIOS_MPU6000_IRQHandler(void) return woken; } - static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data)] = { PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0 }; - static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data)]; + static uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 }; + static uint8_t mpu6000_rec_buf[1 + PIOS_MPU6000_SAMPLES_BYTES]; if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBusISR(&woken); @@ -565,7 +648,7 @@ bool PIOS_MPU6000_IRQHandler(void) static struct pios_mpu6000_data data; // In the case where extras samples backed up grabbed an extra - if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) { + if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) { mpu6000_fifo_backup++; if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { return woken; diff --git a/flight/pios/inc/pios_mpu6000.h b/flight/pios/inc/pios_mpu6000.h index 91ea89d3e..cda019edf 100644 --- a/flight/pios/inc/pios_mpu6000.h +++ b/flight/pios/inc/pios_mpu6000.h @@ -33,71 +33,72 @@ #define PIOS_MPU6000_H /* MPU6000 Addresses */ -#define PIOS_MPU6000_SMPLRT_DIV_REG 0X19 -#define PIOS_MPU6000_DLPF_CFG_REG 0X1A -#define PIOS_MPU6000_GYRO_CFG_REG 0X1B -#define PIOS_MPU6000_ACCEL_CFG_REG 0X1C -#define PIOS_MPU6000_FIFO_EN_REG 0x23 -#define PIOS_MPU6000_INT_CFG_REG 0x37 -#define PIOS_MPU6000_INT_EN_REG 0x38 -#define PIOS_MPU6000_INT_STATUS_REG 0x3A -#define PIOS_MPU6000_ACCEL_X_OUT_MSB 0x3B -#define PIOS_MPU6000_ACCEL_X_OUT_LSB 0x3C -#define PIOS_MPU6000_ACCEL_Y_OUT_MSB 0x3D -#define PIOS_MPU6000_ACCEL_Y_OUT_LSB 0x3E -#define PIOS_MPU6000_ACCEL_Z_OUT_MSB 0x3F -#define PIOS_MPU6000_ACCEL_Z_OUT_LSB 0x40 -#define PIOS_MPU6000_TEMP_OUT_MSB 0x41 -#define PIOS_MPU6000_TEMP_OUT_LSB 0x42 -#define PIOS_MPU6000_GYRO_X_OUT_MSB 0x43 -#define PIOS_MPU6000_GYRO_X_OUT_LSB 0x44 -#define PIOS_MPU6000_GYRO_Y_OUT_MSB 0x45 -#define PIOS_MPU6000_GYRO_Y_OUT_LSB 0x46 -#define PIOS_MPU6000_GYRO_Z_OUT_MSB 0x47 -#define PIOS_MPU6000_GYRO_Z_OUT_LSB 0x48 -#define PIOS_MPU6000_USER_CTRL_REG 0x6A -#define PIOS_MPU6000_PWR_MGMT_REG 0x6B -#define PIOS_MPU6000_FIFO_CNT_MSB 0x72 -#define PIOS_MPU6000_FIFO_CNT_LSB 0x73 -#define PIOS_MPU6000_FIFO_REG 0x74 -#define PIOS_MPU6000_WHOAMI 0x75 +#define PIOS_MPU6000_SMPLRT_DIV_REG 0X19 +#define PIOS_MPU6000_DLPF_CFG_REG 0X1A +#define PIOS_MPU6000_GYRO_CFG_REG 0X1B +#define PIOS_MPU6000_ACCEL_CFG_REG 0X1C +#define PIOS_MPU6000_FIFO_EN_REG 0x23 +#define PIOS_MPU6000_INT_CFG_REG 0x37 +#define PIOS_MPU6000_INT_EN_REG 0x38 +#define PIOS_MPU6000_INT_STATUS_REG 0x3A +#define PIOS_MPU6000_ACCEL_X_OUT_MSB 0x3B +#define PIOS_MPU6000_ACCEL_X_OUT_LSB 0x3C +#define PIOS_MPU6000_ACCEL_Y_OUT_MSB 0x3D +#define PIOS_MPU6000_ACCEL_Y_OUT_LSB 0x3E +#define PIOS_MPU6000_ACCEL_Z_OUT_MSB 0x3F +#define PIOS_MPU6000_ACCEL_Z_OUT_LSB 0x40 +#define PIOS_MPU6000_TEMP_OUT_MSB 0x41 +#define PIOS_MPU6000_TEMP_OUT_LSB 0x42 +#define PIOS_MPU6000_GYRO_X_OUT_MSB 0x43 +#define PIOS_MPU6000_GYRO_X_OUT_LSB 0x44 +#define PIOS_MPU6000_GYRO_Y_OUT_MSB 0x45 +#define PIOS_MPU6000_GYRO_Y_OUT_LSB 0x46 +#define PIOS_MPU6000_GYRO_Z_OUT_MSB 0x47 +#define PIOS_MPU6000_GYRO_Z_OUT_LSB 0x48 +#define PIOS_MPU6000_USER_CTRL_REG 0x6A +#define PIOS_MPU6000_PWR_MGMT_REG 0x6B +#define PIOS_MPU6000_FIFO_CNT_MSB 0x72 +#define PIOS_MPU6000_FIFO_CNT_LSB 0x73 +#define PIOS_MPU6000_FIFO_REG 0x74 +#define PIOS_MPU6000_WHOAMI 0x75 /* FIFO enable for storing different values */ -#define PIOS_MPU6000_FIFO_TEMP_OUT 0x80 -#define PIOS_MPU6000_FIFO_GYRO_X_OUT 0x40 -#define PIOS_MPU6000_FIFO_GYRO_Y_OUT 0x20 -#define PIOS_MPU6000_FIFO_GYRO_Z_OUT 0x10 -#define PIOS_MPU6000_ACCEL_OUT 0x08 +#define PIOS_MPU6000_FIFO_TEMP_OUT 0x80 +#define PIOS_MPU6000_FIFO_GYRO_X_OUT 0x40 +#define PIOS_MPU6000_FIFO_GYRO_Y_OUT 0x20 +#define PIOS_MPU6000_FIFO_GYRO_Z_OUT 0x10 +#define PIOS_MPU6000_ACCEL_OUT 0x08 /* Interrupt Configuration */ -#define PIOS_MPU6000_INT_ACTL 0x80 -#define PIOS_MPU6000_INT_OPEN 0x40 -#define PIOS_MPU6000_INT_LATCH_EN 0x20 -#define PIOS_MPU6000_INT_CLR_ANYRD 0x10 +#define PIOS_MPU6000_INT_ACTL 0x80 +#define PIOS_MPU6000_INT_OPEN 0x40 +#define PIOS_MPU6000_INT_LATCH_EN 0x20 +#define PIOS_MPU6000_INT_CLR_ANYRD 0x10 -#define PIOS_MPU6000_INTEN_OVERFLOW 0x10 -#define PIOS_MPU6000_INTEN_DATA_RDY 0x01 +#define PIOS_MPU6000_INTEN_OVERFLOW 0x10 +#define PIOS_MPU6000_INTEN_DATA_RDY 0x01 /* Interrupt status */ -#define PIOS_MPU6000_INT_STATUS_FIFO_FULL 0x80 -#define PIOS_MPU6000_INT_STATUS_IMU_RDY 0X04 -#define PIOS_MPU6000_INT_STATUS_DATA_RDY 0X01 +#define PIOS_MPU6000_INT_STATUS_FIFO_FULL 0x80 +#define PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW 0x10 +#define PIOS_MPU6000_INT_STATUS_IMU_RDY 0X04 +#define PIOS_MPU6000_INT_STATUS_DATA_RDY 0X01 /* User control functionality */ -#define PIOS_MPU6000_USERCTL_FIFO_EN 0X40 -#define PIOS_MPU6000_USERCTL_I2C_MST_EN 0x20 -#define PIOS_MPU6000_USERCTL_DIS_I2C 0X10 -#define PIOS_MPU6000_USERCTL_FIFO_RST 0X04 -#define PIOS_MPU6000_USERCTL_SIG_COND 0X02 -#define PIOS_MPU6000_USERCTL_GYRO_RST 0X01 +#define PIOS_MPU6000_USERCTL_FIFO_EN 0X40 +#define PIOS_MPU6000_USERCTL_I2C_MST_EN 0x20 +#define PIOS_MPU6000_USERCTL_DIS_I2C 0X10 +#define PIOS_MPU6000_USERCTL_FIFO_RST 0X04 +#define PIOS_MPU6000_USERCTL_SIG_COND 0X02 +#define PIOS_MPU6000_USERCTL_GYRO_RST 0X01 /* Power management and clock selection */ -#define PIOS_MPU6000_PWRMGMT_IMU_RST 0X80 -#define PIOS_MPU6000_PWRMGMT_INTERN_CLK 0X00 -#define PIOS_MPU6000_PWRMGMT_PLL_X_CLK 0X01 -#define PIOS_MPU6000_PWRMGMT_PLL_Y_CLK 0X02 -#define PIOS_MPU6000_PWRMGMT_PLL_Z_CLK 0X03 -#define PIOS_MPU6000_PWRMGMT_STOP_CLK 0X07 +#define PIOS_MPU6000_PWRMGMT_IMU_RST 0X80 +#define PIOS_MPU6000_PWRMGMT_INTERN_CLK 0X00 +#define PIOS_MPU6000_PWRMGMT_PLL_X_CLK 0X01 +#define PIOS_MPU6000_PWRMGMT_PLL_Y_CLK 0X02 +#define PIOS_MPU6000_PWRMGMT_PLL_Z_CLK 0X03 +#define PIOS_MPU6000_PWRMGMT_STOP_CLK 0X07 enum pios_mpu6000_range { PIOS_MPU6000_SCALE_250_DEG = 0x00, diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 5c429d57e..7192b0a4f 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -124,6 +124,10 @@ void PIOS_Board_Init(void) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* PIOS_INCLUDE_RTC */ +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + /* IAP System Setup */ PIOS_IAP_Init(); // check for safe mode commands from gcs @@ -141,10 +145,6 @@ void PIOS_Board_Init(void) OPLinkStatusInitialize(); #endif /* PIOS_INCLUDE_RFM22B */ -#if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ - /* Initialize the delayed callback library */ CallbackSchedulerInitialize();