diff --git a/flight/PiOS/Common/pios_bma180.c b/flight/PiOS/Common/pios_bma180.c index 2a578dd5d..65b872e2c 100644 --- a/flight/PiOS/Common/pios_bma180.c +++ b/flight/PiOS/Common/pios_bma180.c @@ -434,7 +434,7 @@ int32_t PIOS_BMA180_Test() * @brief IRQ Handler. Read data from the BMA180 FIFO and push onto a local fifo. */ int32_t bma180_irqs = 0; -void PIOS_BMA180_IRQHandler(void) +bool PIOS_BMA180_IRQHandler(void) { bma180_irqs++; @@ -470,7 +470,8 @@ void PIOS_BMA180_IRQHandler(void) data.temperature = pios_bma180_dmabuf[7]; fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); - + + return false; } #endif /* PIOS_INCLUDE_BMA180 */ diff --git a/flight/PiOS/Common/pios_hmc5883.c b/flight/PiOS/Common/pios_hmc5883.c index 8891709fc..14d4f96fe 100644 --- a/flight/PiOS/Common/pios_hmc5883.c +++ b/flight/PiOS/Common/pios_hmc5883.c @@ -391,9 +391,11 @@ int32_t PIOS_HMC5883_Test(void) /** * @brief IRQ Handler */ -void PIOS_HMC5883_IRQHandler(void) +bool PIOS_HMC5883_IRQHandler(void) { - pios_hmc5883_data_ready = true; + pios_hmc5883_data_ready = true + + return false; } #endif /* PIOS_INCLUDE_HMC5883 */ diff --git a/flight/PiOS/Common/pios_l3gd20.c b/flight/PiOS/Common/pios_l3gd20.c index 3151d26e2..e98a66153 100644 --- a/flight/PiOS/Common/pios_l3gd20.c +++ b/flight/PiOS/Common/pios_l3gd20.c @@ -353,7 +353,7 @@ uint8_t PIOS_L3GD20_Test(void) /** * @brief IRQ Handler. Read all the data from onboard buffer */ -void PIOS_L3GD20_IRQHandler(void) +bool PIOS_L3GD20_IRQHandler(void) { l3gd20_irq++; @@ -375,7 +375,10 @@ void PIOS_L3GD20_IRQHandler(void) memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6); data.temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); - xQueueSend(dev->queue, (void *) &data, 0); + portBASE_TYPE xHigherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); + + return xHigherPriorityTaskWoken == pdTRUE; } #endif /* L3GD20 */ diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c index f1e39712f..a5773b91f 100644 --- a/flight/PiOS/Common/pios_mpu6000.c +++ b/flight/PiOS/Common/pios_mpu6000.c @@ -405,21 +405,21 @@ uint32_t mpu6000_interval_us; uint32_t mpu6000_time_us; uint32_t mpu6000_transfer_size; -void PIOS_MPU6000_IRQHandler(void) +bool PIOS_MPU6000_IRQHandler(void) { static uint32_t timeval; mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); if(!mpu6000_configured) - return; + return false; mpu6000_count = PIOS_MPU6000_FifoDepth(); if(mpu6000_count < sizeof(struct pios_mpu6000_data)) - return; + return false; if(PIOS_MPU6000_ClaimBus() != 0) - return; + return false; uint8_t mpu6000_send_buf[1+sizeof(struct pios_mpu6000_data)] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t mpu6000_rec_buf[1+sizeof(struct pios_mpu6000_data)]; @@ -427,7 +427,7 @@ void PIOS_MPU6000_IRQHandler(void) if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBus(); mpu6000_fails++; - return; + return false; } PIOS_MPU6000_ReleaseBus(); @@ -438,15 +438,12 @@ void PIOS_MPU6000_IRQHandler(void) if (mpu6000_count >= (sizeof(data) * 2)) { mpu6000_fifo_backup++; if(PIOS_MPU6000_ClaimBus() != 0) - return; - - uint8_t mpu6000_send_buf[1+sizeof(struct pios_mpu6000_data)] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0}; - uint8_t mpu6000_rec_buf[1+sizeof(struct pios_mpu6000_data)]; + return false; if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBus(); mpu6000_fails++; - return; + return false; } PIOS_MPU6000_ReleaseBus(); @@ -510,12 +507,15 @@ void PIOS_MPU6000_IRQHandler(void) data.gyro_z = -(mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]); data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; #endif - - xQueueSend(dev->queue, (void *) &data, 0); + + portBASE_TYPE xHigherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); mpu6000_irq++; mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); + + return xHigherPriorityTaskWoken == pdTRUE; } #endif diff --git a/flight/PiOS/STM32F10x/pios_exti.c b/flight/PiOS/STM32F10x/pios_exti.c index 65a651f6f..9b8221bbe 100644 --- a/flight/PiOS/STM32F10x/pios_exti.c +++ b/flight/PiOS/STM32F10x/pios_exti.c @@ -149,7 +149,7 @@ out_fail: return -1; } -static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) +static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; @@ -158,69 +158,133 @@ static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || cfg_index == PIOS_EXTI_INVALID) { /* Unconfigured interrupt just fired! */ - return; + return false; } struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - cfg->vector(); + return cfg->vector(); } -/* Bind Interrupt Handlers */ - -#define PIOS_EXTI_HANDLE_LINE(line) \ +#ifdef PIOS_INCLUDE_FREERTOS +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } +#else +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ EXTI_ClearITPendingBit(EXTI_Line##line); \ PIOS_EXTI_generic_irq_handler(line); \ } +#endif + +/* Bind Interrupt Handlers */ static void PIOS_EXTI_0_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(0); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); static void PIOS_EXTI_1_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(1); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); static void PIOS_EXTI_2_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(2); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); static void PIOS_EXTI_3_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(3); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); static void PIOS_EXTI_4_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(4); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); static void PIOS_EXTI_9_5_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(5); - PIOS_EXTI_HANDLE_LINE(6); - PIOS_EXTI_HANDLE_LINE(7); - PIOS_EXTI_HANDLE_LINE(8); - PIOS_EXTI_HANDLE_LINE(9); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); static void PIOS_EXTI_15_10_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(10); - PIOS_EXTI_HANDLE_LINE(11); - PIOS_EXTI_HANDLE_LINE(12); - PIOS_EXTI_HANDLE_LINE(13); - PIOS_EXTI_HANDLE_LINE(14); - PIOS_EXTI_HANDLE_LINE(15); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); diff --git a/flight/PiOS/STM32F4xx/pios_exti.c b/flight/PiOS/STM32F4xx/pios_exti.c index 9f9567d51..45efb2ae5 100644 --- a/flight/PiOS/STM32F4xx/pios_exti.c +++ b/flight/PiOS/STM32F4xx/pios_exti.c @@ -149,7 +149,7 @@ out_fail: return -1; } -static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) +static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; @@ -158,69 +158,133 @@ static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || cfg_index == PIOS_EXTI_INVALID) { /* Unconfigured interrupt just fired! */ - return; + return false; } struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - cfg->vector(); + return cfg->vector(); } /* Bind Interrupt Handlers */ -#define PIOS_EXTI_HANDLE_LINE(line) \ +#ifdef PIOS_INCLUDE_FREERTOS +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } +#else +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ EXTI_ClearITPendingBit(EXTI_Line##line); \ PIOS_EXTI_generic_irq_handler(line); \ } +#endif static void PIOS_EXTI_0_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(0); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); static void PIOS_EXTI_1_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(1); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); static void PIOS_EXTI_2_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(2); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); static void PIOS_EXTI_3_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(3); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); static void PIOS_EXTI_4_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(4); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); static void PIOS_EXTI_9_5_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(5); - PIOS_EXTI_HANDLE_LINE(6); - PIOS_EXTI_HANDLE_LINE(7); - PIOS_EXTI_HANDLE_LINE(8); - PIOS_EXTI_HANDLE_LINE(9); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); static void PIOS_EXTI_15_10_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(10); - PIOS_EXTI_HANDLE_LINE(11); - PIOS_EXTI_HANDLE_LINE(12); - PIOS_EXTI_HANDLE_LINE(13); - PIOS_EXTI_HANDLE_LINE(14); - PIOS_EXTI_HANDLE_LINE(15); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); diff --git a/flight/PiOS/inc/pios_bma180.h b/flight/PiOS/inc/pios_bma180.h index ef9627f00..51e65a6ec 100644 --- a/flight/PiOS/inc/pios_bma180.h +++ b/flight/PiOS/inc/pios_bma180.h @@ -103,7 +103,7 @@ extern float PIOS_BMA180_GetScale(); extern int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer); extern int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data); extern int32_t PIOS_BMA180_Test(); -extern void PIOS_BMA180_IRQHandler(); +extern bool PIOS_BMA180_IRQHandler(); #endif /* PIOS_BMA180_H */ diff --git a/flight/PiOS/inc/pios_exti.h b/flight/PiOS/inc/pios_exti.h index 73a3bb556..4b1203aa2 100644 --- a/flight/PiOS/inc/pios_exti.h +++ b/flight/PiOS/inc/pios_exti.h @@ -36,7 +36,7 @@ #include struct pios_exti_cfg { - void (* vector)(void); + bool (* vector)(void); uint32_t line; /* use EXTI_LineN macros */ struct stm32_gpio pin; struct stm32_irq irq; diff --git a/flight/PiOS/inc/pios_hmc5883.h b/flight/PiOS/inc/pios_hmc5883.h index 037dca788..d9c362e9d 100644 --- a/flight/PiOS/inc/pios_hmc5883.h +++ b/flight/PiOS/inc/pios_hmc5883.h @@ -107,7 +107,7 @@ extern bool PIOS_HMC5883_NewDataAvailable(void); extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]); extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]); extern int32_t PIOS_HMC5883_Test(void); -extern void PIOS_HMC5883_IRQHandler(); +bool void PIOS_HMC5883_IRQHandler(); #endif /* PIOS_HMC5883_H */ /** diff --git a/flight/PiOS/inc/pios_l3gd20.h b/flight/PiOS/inc/pios_l3gd20.h index 83b2070b6..3a2e896fd 100644 --- a/flight/PiOS/inc/pios_l3gd20.h +++ b/flight/PiOS/inc/pios_l3gd20.h @@ -141,7 +141,7 @@ extern int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range); extern float PIOS_L3GD20_GetScale(); extern int32_t PIOS_L3GD20_ReadID(); extern uint8_t PIOS_L3GD20_Test(); -extern void PIOS_L3GD20_IRQHandler(); +bool void PIOS_L3GD20_IRQHandler(); #endif /* PIOS_L3GD20_H */ diff --git a/flight/PiOS/inc/pios_mpu6000.h b/flight/PiOS/inc/pios_mpu6000.h index b8a49a1ad..254f10192 100644 --- a/flight/PiOS/inc/pios_mpu6000.h +++ b/flight/PiOS/inc/pios_mpu6000.h @@ -165,7 +165,7 @@ extern int32_t PIOS_MPU6000_ReadID(); extern int32_t PIOS_MPU6000_Test(); extern float PIOS_MPU6000_GetScale(); extern float PIOS_MPU6000_GetAccelScale(); -extern void PIOS_MPU6000_IRQHandler(void); +extern bool PIOS_MPU6000_IRQHandler(void); #endif /* PIOS_MPU6000_H */ diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp index 70d52597d..f00024d14 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp @@ -38,37 +38,40 @@ namespace Utils { - HomeLocationUtil::HomeLocationUtil() +HomeLocationUtil::HomeLocationUtil() +{ +} + + /** + * @brief Get local magnetic field + * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at + * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) + * @returns 0 if successful, -1 otherwise. + */ + int HomeLocationUtil::getDetails(double LLA[3], double Be[3]) { -// Initialize(); - } + // ************* + // check input parms - // input params: LLA - // - // output params: ECEF, RNE and Be - int HomeLocationUtil::getDetails(double LLA[3], double Be[3]) - { - // ************* - // check input parms + double latitude = LLA[0]; + double longitude = LLA[1]; + double altitude = LLA[2]; - double latitude = LLA[0]; - double longitude = LLA[1]; - double altitude = LLA[2]; + if (latitude != latitude) return -1; // prevent nan error + if (longitude != longitude) return -2; // prevent nan error + if (altitude != altitude) return -3; // prevent nan error - if (latitude != latitude) return -1; // prevent nan error - if (longitude != longitude) return -2; // prevent nan error - if (altitude != altitude) return -3; // prevent nan error + if (latitude < -90 || latitude > 90) return -4; // range checking + if (longitude < -180 || longitude > 180) return -5; // range checking - if (latitude < -90 || latitude > 90) return -4; // range checking - if (longitude < -180 || longitude > 180) return -5; // range checking + // ************* - // ************* + QDateTime dt = QDateTime::currentDateTime().toUTC(); - QDateTime dt = QDateTime::currentDateTime().toUTC(); + //Fetch world magnetic model + Q_ASSERT(WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be) >= 0); - Q_ASSERT(WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be) >= 0); - - return 0; // OK + return 0; // OK } } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 4405d2b2e..cd0e2f32a 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -49,7 +49,6 @@ #include "uavtalk/telemetrymanager.h" #include "uavobject.h" -#include "uavobjectmanager.h" #include "positionactual.h" #include "homelocation.h" @@ -568,6 +567,7 @@ void OPMapGadgetWidget::updatePosition() VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm); Gyros *gyrosObj = Gyros::GetInstance(obm); + Q_ASSERT(attitudeActualObj); Q_ASSERT(positionActualObj); Q_ASSERT(velocityActualObj); Q_ASSERT(gyrosObj); @@ -892,7 +892,7 @@ void OPMapGadgetWidget::setHome(QPointF pos) /** Sets the home position on the map widget */ -void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon,double altitude) +void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon, double altitude) { if (!m_widget || !m_map) return; @@ -914,13 +914,13 @@ void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon,double altitu if (longitude > 180) longitude = 180; else if (longitude < -180) longitude = -180; - else if(altitude != altitude) altitude=0; // ********* m_home_position.coord = internals::PointLatLng(latitude, longitude); + m_home_position.altitude = altitude; - m_map->Home->SetCoord(m_home_position.coord); + m_map->Home->SetCoord(m_home_position.coord); m_map->Home->SetAltitude(altitude); m_map->Home->RefreshPos(); @@ -1602,7 +1602,15 @@ void OPMapGadgetWidget::onSetHomeAct_triggered() if (!m_widget || !m_map) return; - setHome(m_context_menu_lat_lon,0); + float altitude=0; + bool ok; + + //Get desired HomeLocation altitude from dialog box. + //TODO: Populate box with altitude already in HomeLocation UAVO + altitude = QInputDialog::getDouble(this, tr("Set home altitude"), + tr("In [m], referenced to WGS84:"), altitude, -100, 100000, 2, &ok); + + setHome(m_context_menu_lat_lon, altitude); setHomeLocationObject(); // update the HomeLocation UAVObject } diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 6446d450e..3b02a8d0d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -35,7 +35,10 @@ #include #include #include -#include + +#include "firmwareiapobj.h" +#include "homelocation.h" +#include "gpsposition.h" // ****************************** // constructor/destructor @@ -48,6 +51,18 @@ UAVObjectUtilManager::UAVObjectUtilManager() failureTimer.setSingleShot(true); failureTimer.setInterval(1000); connect(&failureTimer, SIGNAL(timeout()),this,SLOT(objectPersistenceOperationFailed())); + + pm = NULL; + obm = NULL; + obum = NULL; + + pm = ExtensionSystem::PluginManager::instance(); + if (pm) + { + obm = pm->getObject(); + obum = pm->getObject(); + } + } UAVObjectUtilManager::~UAVObjectUtilManager() @@ -67,10 +82,8 @@ UAVObjectUtilManager::~UAVObjectUtilManager() UAVObjectManager* UAVObjectUtilManager::getObjectManager() { - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * objMngr = pm->getObject(); - Q_ASSERT(objMngr); - return objMngr; + Q_ASSERT(obm); + return obm; } @@ -107,7 +120,7 @@ void UAVObjectUtilManager::saveNextObject() // Get next object from the queue UAVObject* obj = queue.head(); - qDebug() << "Request board to save object " << obj->getName(); + qDebug() << "Send save object request to board " << obj->getName(); ObjectPersistence* objper = dynamic_cast( getObjectManager()->getObject(ObjectPersistence::NAME) ); connect(objper, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject*,bool))); @@ -233,16 +246,7 @@ FirmwareIAPObj::DataFields UAVObjectUtilManager::getFirmwareIap() { FirmwareIAPObj::DataFields dummy; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - Q_ASSERT(pm); - if (!pm) - return dummy; - UAVObjectManager *om = pm->getObject(); - Q_ASSERT(om); - if (!om) - return dummy; - - FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(om); + FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(obm); Q_ASSERT(firmwareIap); if (!firmwareIap) return dummy; @@ -306,76 +310,47 @@ QByteArray UAVObjectUtilManager::getBoardDescription() int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard) { - double Be[3]; + double Be[3]; - QMutexLocker locker(mutex); + Q_ASSERT (Utils::HomeLocationUtil().getDetails(LLA, Be) >= 0); - Q_ASSERT (Utils::HomeLocationUtil().getDetails(LLA, Be) >= 0); + // ****************** + // save the new settings - // ****************** - // save the new settings + HomeLocation *homeLocation = HomeLocation::GetInstance(obm); + Q_ASSERT(homeLocation != NULL); - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) return -2; + HomeLocation::DataFields homeLocationData = homeLocation->getData(); + homeLocationData.Latitude = LLA[0] * 1e7; + homeLocationData.Longitude = LLA[1] * 1e7; + homeLocationData.Altitude = LLA[2]; - UAVObjectManager *om = pm->getObject(); - if (!om) return -3; + homeLocationData.Be[0] = Be[0]; + homeLocationData.Be[1] = Be[1]; + homeLocationData.Be[2] = Be[2]; - HomeLocation *homeLocation = HomeLocation::GetInstance(om); - Q_ASSERT(homeLocation != NULL); + homeLocationData.Set = HomeLocation::SET_TRUE; - HomeLocation::DataFields homeLocationData = homeLocation->getData(); - homeLocationData.Latitude = LLA[0] * 10e6; - homeLocationData.Longitude = LLA[1] * 10e6; - homeLocationData.Altitude = LLA[2] * 10e6; + homeLocation->setData(homeLocationData); - homeLocationData.Be[0] = Be[0]; - homeLocationData.Be[1] = Be[1]; - homeLocationData.Be[2] = Be[2]; + if (save_to_sdcard) + saveObjectToSD(homeLocation); - homeLocationData.Set = HomeLocation::SET_TRUE; - - homeLocation->setData(homeLocationData); - homeLocation->updated(); - - if (save_to_sdcard) - saveObjectToSD(homeLocation); - - return 0; + return 0; } int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3]) { - UAVObjectField *field; + HomeLocation *homeLocation = HomeLocation::GetInstance(obm); + Q_ASSERT(homeLocation != NULL); - QMutexLocker locker(mutex); + HomeLocation::DataFields homeLocationData = homeLocation->getData(); - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) return -1; + set = homeLocationData.Set; - UAVObjectManager *om = pm->getObject(); - if (!om) return -2; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("HomeLocation"))); - if (!obj) return -3; - -// obj->requestUpdate(); - - field = obj->getField("Set"); - if (!field) return -4; - set = field->getValue().toBool(); - - field = obj->getField("Latitude"); - if (!field) return -5; - LLA[0] = field->getDouble() * 1e-7; - - field = obj->getField("Longitude"); - if (!field) return -6; - LLA[1] = field->getDouble() * 1e-7; - - field = obj->getField("Altitude"); - if (!field) return -7; - LLA[2] = field->getDouble(); + LLA[0] = homeLocationData.Latitude*1e-7; + LLA[1] = homeLocationData.Longitude*1e-7; + LLA[2] = homeLocationData.Altitude; if (LLA[0] != LLA[0]) LLA[0] = 0; // nan detection else @@ -394,88 +369,20 @@ int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3]) return 0; // OK } -int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3], double ECEF[3], double RNE[9], double Be[3]) -{ - UAVObjectField *field; - - QMutexLocker locker(mutex); - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) return -1; - - UAVObjectManager *om = pm->getObject(); - if (!om) return -2; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("HomeLocation"))); - if (!obj) return -3; - -// obj->requestUpdate(); - - field = obj->getField("Set"); - if (!field) return -4; - set = field->getValue().toBool(); - - field = obj->getField("Latitude"); - if (!field) return -5; - LLA[0] = field->getDouble() * 1e-7; - - field = obj->getField("Longitude"); - if (!field) return -6; - LLA[1] = field->getDouble() * 1e-7; - - field = obj->getField("Altitude"); - if (!field) return -7; - LLA[2] = field->getDouble(); - - field = obj->getField(QString("ECEF")); - if (!field) return -8; - for (int i = 0; i < 3; i++) - ECEF[i] = field->getDouble(i); - - field = obj->getField(QString("RNE")); - if (!field) return -9; - for (int i = 0; i < 9; i++) - RNE[i] = field->getDouble(i); - - field = obj->getField(QString("Be")); - if (!field) return -10; - for (int i = 0; i < 3; i++) - Be[i] = field->getDouble(i); - - return 0; // OK -} // ****************************** // GPS int UAVObjectUtilManager::getGPSPosition(double LLA[3]) { - UAVObjectField *field; + GPSPosition *gpsPosition = GPSPosition::GetInstance(obm); + Q_ASSERT(gpsPosition != NULL); - QMutexLocker locker(mutex); + GPSPosition::DataFields gpsPositionData = gpsPosition->getData(); - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) return -1; - - UAVObjectManager *om = pm->getObject(); - if (!om) return -2; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("GPSPosition"))); - if (!obj) return -3; - -// obj->requestUpdate(); - - field = obj->getField(QString("Latitude")); - if (!field) return -4; - LLA[0] = field->getDouble() * 1e-7; - - field = obj->getField(QString("Longitude")); - if (!field) return -5; - LLA[1] = field->getDouble() * 1e-7; - - field = obj->getField(QString("Altitude")); - if (!field) return -6; - LLA[2] = field->getDouble(); + LLA[0] = gpsPositionData.Latitude; + LLA[1] = gpsPositionData.Longitude; + LLA[2] = gpsPositionData.Altitude; if (LLA[0] != LLA[0]) LLA[0] = 0; // nan detection else @@ -494,94 +401,6 @@ int UAVObjectUtilManager::getGPSPosition(double LLA[3]) return 0; // OK } -// ****************************** -// telemetry port - -int UAVObjectUtilManager::setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard) -{ - UAVObjectField *field; - - QMutexLocker locker(mutex); - - // ****************** - // save the new settings - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) return -1; - - UAVObjectManager *om = pm->getObject(); - if (!om) return -2; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("/*TelemetrySettings*/"))); - if (!obj) return -3; - - field = obj->getField(QString("Speed")); - if (!field) return -4; - field->setValue(speed); - - obj->updated(); - - // ****************** - // save the new setting to SD card - - if (save_to_sdcard) - saveObjectToSD(obj); - - // ****************** - - return 0; // OK -} - -int UAVObjectUtilManager::getTelemetrySerialPortSpeed(QString &speed) -{ - UAVObjectField *field; - - QMutexLocker locker(mutex); - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) return -1; - - UAVObjectManager *om = pm->getObject(); - if (!om) return -2; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("TelemetrySettings"))); - if (!obj) return -3; - -// obj->requestUpdate(); - - field = obj->getField(QString("Speed")); - if (!field) return -4; - speed = field->getValue().toString(); - - return 0; // OK -} - -int UAVObjectUtilManager::getTelemetrySerialPortSpeeds(QComboBox *comboBox) -{ - UAVObjectField *field; - - QMutexLocker locker(mutex); - - if (!comboBox) return -1; - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) return -2; - - UAVObjectManager *om = pm->getObject(); - if (!om) return -3; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("TelemetrySettings"))); - if (!obj) return -4; - -// obj->requestUpdate(); - - field = obj->getField(QString("Speed")); - if (!field) return -5; - comboBox->addItems(field->getOptions()); - - return 0; // OK -} - deviceDescriptorStruct UAVObjectUtilManager::getBoardDescriptionStruct() { deviceDescriptorStruct ret; diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 11f5f0979..8ea9dc8f2 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -55,14 +55,9 @@ public: int setHomeLocation(double LLA[3], bool save_to_sdcard); int getHomeLocation(bool &set, double LLA[3]); - int getHomeLocation(bool &set, double LLA[3], double ECEF[3], double RNE[9], double Be[3]); int getGPSPosition(double LLA[3]); - int setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard); - int getTelemetrySerialPortSpeed(QString &speed); - int getTelemetrySerialPortSpeeds(QComboBox *comboBox); - int getBoardModel(); QByteArray getBoardCPUSerial(); quint32 getFirmwareCRC(); @@ -78,11 +73,15 @@ signals: void saveCompleted(int objectID, bool status); private: - QMutex *mutex; - QQueue queue; - enum {IDLE, AWAITING_ACK, AWAITING_COMPLETED} saveState; - void saveNextObject(); - QTimer failureTimer; + QMutex *mutex; + QQueue queue; + enum {IDLE, AWAITING_ACK, AWAITING_COMPLETED} saveState; + void saveNextObject(); + QTimer failureTimer; + + ExtensionSystem::PluginManager *pm; + UAVObjectManager *obm; + UAVObjectUtilManager *obum; private slots: //void transactionCompleted(UAVObject *obj, bool success);