Merge branch 'next' into revo

Conflicts:
	ground/openpilotgcs/src/libs/utils/homelocationutil.cpp
	ground/openpilotgcs/src/libs/utils/homelocationutil.h
	ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
	ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp

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 */

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 */

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 */

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;		
+			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)];
 		
 		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

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 */
+#ifdef PIOS_INCLUDE_FREERTOS
-
+#define PIOS_EXTI_HANDLE_LINE(line, woken)			\
-#define PIOS_EXTI_HANDLE_LINE(line)				\
+	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);
+#ifdef PIOS_INCLUDE_FREERTOS
-	PIOS_EXTI_HANDLE_LINE(6);
+	portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-	PIOS_EXTI_HANDLE_LINE(7);
+#else
-	PIOS_EXTI_HANDLE_LINE(8);
+	bool xHigherPriorityTaskWoken;  // dummy variable
-	PIOS_EXTI_HANDLE_LINE(9);
+#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);
+#ifdef PIOS_INCLUDE_FREERTOS
-	PIOS_EXTI_HANDLE_LINE(11);
+	portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-	PIOS_EXTI_HANDLE_LINE(12);
+#else
-	PIOS_EXTI_HANDLE_LINE(13);
+	bool xHigherPriorityTaskWoken;  // dummy variable
-	PIOS_EXTI_HANDLE_LINE(14);
+#endif
-	PIOS_EXTI_HANDLE_LINE(15);
+	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")));
 

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);
+#ifdef PIOS_INCLUDE_FREERTOS
-	PIOS_EXTI_HANDLE_LINE(6);
+	portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-	PIOS_EXTI_HANDLE_LINE(7);
+#else
-	PIOS_EXTI_HANDLE_LINE(8);
+	bool xHigherPriorityTaskWoken;
-	PIOS_EXTI_HANDLE_LINE(9);
+#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);
+#ifdef PIOS_INCLUDE_FREERTOS
-	PIOS_EXTI_HANDLE_LINE(11);
+	portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-	PIOS_EXTI_HANDLE_LINE(12);
+#else
-	PIOS_EXTI_HANDLE_LINE(13);
+	bool xHigherPriorityTaskWoken;
-	PIOS_EXTI_HANDLE_LINE(14);
+#endif
-	PIOS_EXTI_HANDLE_LINE(15);
+	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")));
 

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 */
 

flight/PiOS/inc/pios_exti.h

@@ -36,7 +36,7 @@
 #include <pios_stm32.h>
 
 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;

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 */
 
 /** 

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 */
 

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 */
 

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
+        double latitude = LLA[0];
-	//
+        double longitude = LLA[1];
-	// output params: ECEF, RNE and Be
+        double altitude = LLA[2];
-        int HomeLocationUtil::getDetails(double LLA[3], double Be[3])
-    {
-		// *************
-		// check input parms
 
-		double latitude = LLA[0];
+        if (latitude != latitude) return -1;				// prevent nan error
-		double longitude = LLA[1];
+        if (longitude != longitude) return -2;				// prevent nan error
-		double altitude = LLA[2];
+        if (altitude != altitude) return -3;				// prevent nan error
 
-		if (latitude != latitude) return -1;				// prevent nan error
+        if (latitude < -90 || latitude > 90) return -4;		// range checking
-		if (longitude != longitude) return -2;				// prevent nan error
+        if (longitude < -180 || longitude > 180) return -5;	// range checking
-		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
 
-		// *************
+        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
     }
 
 }

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
 }

ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp

@@ -35,7 +35,10 @@
 #include <QEventLoop>
 #include <QTimer>
 #include <objectpersistence.h>
-#include <firmwareiapobj.h>
+
+#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<UAVObjectManager>();
+        obum = pm->getObject<UAVObjectUtilManager>();
+    }
+
 }
 
 UAVObjectUtilManager::~UAVObjectUtilManager()
@@ -67,10 +82,8 @@ UAVObjectUtilManager::~UAVObjectUtilManager()
 
 
 UAVObjectManager* UAVObjectUtilManager::getObjectManager() {
-    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    Q_ASSERT(obm);
-    UAVObjectManager * objMngr = pm->getObject<UAVObjectManager>();
+    return obm;
-    Q_ASSERT(objMngr);
-    return objMngr;
 }
 
 
@@ -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<ObjectPersistence*>( 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();
+    FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(obm);
-    Q_ASSERT(pm);
-    if (!pm)
-        return dummy;
-    UAVObjectManager *om = pm->getObject<UAVObjectManager>();
-    Q_ASSERT(om);
-    if (!om)
-        return dummy;
-
-    FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(om);
     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
 
-        // ******************
+    HomeLocation *homeLocation = HomeLocation::GetInstance(obm);
-	// save the new settings
+    Q_ASSERT(homeLocation != NULL);
 
-	ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    HomeLocation::DataFields homeLocationData = homeLocation->getData();
-	if (!pm) return -2;
+    homeLocationData.Latitude = LLA[0] * 1e7;
+    homeLocationData.Longitude = LLA[1] * 1e7;
+    homeLocationData.Altitude = LLA[2];
 
-	UAVObjectManager *om = pm->getObject<UAVObjectManager>();
+    homeLocationData.Be[0] = Be[0];
-	if (!om) return -3;
+    homeLocationData.Be[1] = Be[1];
+    homeLocationData.Be[2] = Be[2];
 
-        HomeLocation *homeLocation = HomeLocation::GetInstance(om);
+    homeLocationData.Set = HomeLocation::SET_TRUE;
-        Q_ASSERT(homeLocation != NULL);
 
-        HomeLocation::DataFields homeLocationData = homeLocation->getData();
+    homeLocation->setData(homeLocationData);
-        homeLocationData.Latitude = LLA[0] * 10e6;
-        homeLocationData.Longitude = LLA[1] * 10e6;
-        homeLocationData.Altitude = LLA[2] * 10e6;
 
-        homeLocationData.Be[0] = Be[0];
+    if (save_to_sdcard)
-        homeLocationData.Be[1] = Be[1];
+        saveObjectToSD(homeLocation);
-        homeLocationData.Be[2] = Be[2];
 
-        homeLocationData.Set = HomeLocation::SET_TRUE;
+    return 0;
-
-        homeLocation->setData(homeLocationData);
-        homeLocation->updated();
-
-	if (save_to_sdcard)
-                saveObjectToSD(homeLocation);
-
-        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();
+    set = homeLocationData.Set;
-	if (!pm) return -1;
 
-	UAVObjectManager *om = pm->getObject<UAVObjectManager>();
+    LLA[0] = homeLocationData.Latitude*1e-7;
-	if (!om) return -2;
+    LLA[1] = homeLocationData.Longitude*1e-7;
-
+    LLA[2] = homeLocationData.Altitude;
-	UAVDataObject *obj = dynamic_cast<UAVDataObject *>(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();
 
 	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<UAVObjectManager>();
-	if (!om) return -2;
-
-	UAVDataObject *obj = dynamic_cast<UAVDataObject *>(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();
+    LLA[0] = gpsPositionData.Latitude;
-	if (!pm) return -1;
+    LLA[1] = gpsPositionData.Longitude;
-
+    LLA[2] = gpsPositionData.Altitude;
-	UAVObjectManager *om = pm->getObject<UAVObjectManager>();
-	if (!om) return -2;
-
-	UAVDataObject *obj = dynamic_cast<UAVDataObject *>(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();
 
 	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<UAVObjectManager>();
-	if (!om) return -2;
-
-	UAVDataObject *obj = dynamic_cast<UAVDataObject *>(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<UAVObjectManager>();
-	if (!om) return -2;
-
-	UAVDataObject *obj = dynamic_cast<UAVDataObject *>(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<UAVObjectManager>();
-	if (!om) return -3;
-
-	UAVDataObject *obj = dynamic_cast<UAVDataObject *>(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;

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;
+    QMutex *mutex;
-	QQueue<UAVObject *> queue;
+    QQueue<UAVObject *> queue;
-        enum {IDLE, AWAITING_ACK, AWAITING_COMPLETED} saveState;
+    enum {IDLE, AWAITING_ACK, AWAITING_COMPLETED} saveState;
-	void saveNextObject();
+    void saveNextObject();
-        QTimer failureTimer;
+    QTimer failureTimer;
+
+    ExtensionSystem::PluginManager *pm;
+    UAVObjectManager *obm;
+    UAVObjectUtilManager *obum;
 
 private slots:
         //void transactionCompleted(UAVObject *obj, bool success);