rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge branch 'next' into revo

Browse Source 
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
This commit is contained in:
James Cotton 2012-09-28 01:00:10 -05:00
commit 5c13c31c08
15 changed files with 298 additions and 335 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/PiOS/Common/pios_bma180.c
View File

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

6
flight/PiOS/Common/pios_hmc5883.c
View File

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

7
flight/PiOS/Common/pios_l3gd20.c
View File

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

24
flight/PiOS/Common/pios_mpu6000.c
View File

@ -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

108
flight/PiOS/STM32F10x/pios_exti.c
View File

@ -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")));

104
flight/PiOS/STM32F4xx/pios_exti.c
View File

@ -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")));

2
flight/PiOS/inc/pios_bma180.h
View File

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

2
flight/PiOS/inc/pios_exti.h
View File

@ -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;

2
flight/PiOS/inc/pios_hmc5883.h
View File

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

2
flight/PiOS/inc/pios_l3gd20.h
View File

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

2
flight/PiOS/inc/pios_mpu6000.h
View File

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

49
ground/openpilotgcs/src/libs/utils/homelocationutil.cpp
View File

@ -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
}
}

18
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -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
}

283
ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
View File

@ -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();
UAVObjectManager * objMngr = pm->getObject<UAVObjectManager>();
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<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();
Q_ASSERT(pm);
if (!pm)
return dummy;
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
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<UAVObjectManager>();
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<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();
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<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();
if (!pm) return -1;
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();
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<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;

19
ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h
View File

@ -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<UAVObject *> queue;
enum {IDLE, AWAITING_ACK, AWAITING_COMPLETED} saveState;
void saveNextObject();
QTimer failureTimer;
QMutex *mutex;
QQueue<UAVObject *> 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);