rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Rewrote uavobjectutilmanager.cpp, such that upon UAVO changes it returns com

Browse Source 
Updated homelocationutil to remove references to ECEF and RNE.
This commit is contained in:
Laura Sebesta 2012-09-27 11:45:59 +02:00
parent 43f85d9617
commit f532f3cda5
5 changed files with 187 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,40 +38,40 @@
namespace Utils {
HomeLocationUtil::HomeLocationUtil()
HomeLocationUtil::HomeLocationUtil()
{
// Initialize();
}
/*
/*Get world magnetic model
* input params: LLA
* output params: Be
*/
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 ECEF[3], double RNE[9], 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);
CoordinateConversions().LLA2ECEF(LLA, ECEF);
CoordinateConversions().RneFromLLA(LLA, (double (*)[3])RNE);
if (WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be) < 0)
return -6;
return 0; // OK
return 0; // OK
}
}

2
ground/openpilotgcs/src/libs/utils/homelocationutil.h
View File

@ -40,7 +40,7 @@ namespace Utils {
public:
HomeLocationUtil();
int getDetails(double LLA[3], double ECEF[3], double RNE[9], double Be[3]);
int getDetails(double LLA[3], double Be[3]);
private:

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

@ -46,7 +46,6 @@
#include "uavtalk/telemetrymanager.h"
#include "uavobject.h"
#include "uavobjectmanager.h"
#include "positionactual.h"
#include "homelocation.h"
@ -590,6 +589,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);

379
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;
}
@ -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,121 +310,48 @@ QByteArray UAVObjectUtilManager::getBoardDescription()
int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard)
{
double ECEF[3];
double RNE[9];
double Be[3];
UAVObjectField *field;
double Be[3];
QMutexLocker locker(mutex);
Q_ASSERT (Utils::HomeLocationUtil().getDetails(LLA, Be) >= 0);
if (Utils::HomeLocationUtil().getDetails(LLA, ECEF, RNE, Be) < 0)
return -1; // error
// ******************
// 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] * 1e7;
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -3;
homeLocationData.Be[0] = Be[0];
homeLocationData.Be[1] = Be[1];
homeLocationData.Be[2] = Be[2];
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("HomeLocation")));
if (!obj) return -4;
homeLocationData.Set = HomeLocation::SET_TRUE;
UAVObjectField *ECEF_field = obj->getField(QString("ECEF"));
if (!ECEF_field) return -5;
homeLocation->setData(homeLocationData);
homeLocation->updated();
UAVObjectField *RNE_field = obj->getField(QString("RNE"));
if (!RNE_field) return -6;
if (save_to_sdcard)
saveObjectToSD(homeLocation);
UAVObjectField *Be_field = obj->getField(QString("Be"));
if (!Be_field) return -7;
field = obj->getField("Latitude");
if (!field) return -8;
field->setDouble(LLA[0] * 10e6);
field = obj->getField("Longitude");
if (!field) return -9;
field->setDouble(LLA[1] * 10e6);
field = obj->getField("Altitude");
if (!field) return -10;
field->setDouble(LLA[2]);
for (int i = 0; i < 3; i++)
ECEF_field->setDouble(ECEF[i] * 100, i);
for (int i = 0; i < 9; i++)
RNE_field->setDouble(RNE[i], i);
for (int i = 0; i < 3; i++)
Be_field->setDouble(Be[i], i);
field = obj->getField("Set");
if (!field) return -11;
field->setValue("TRUE");
obj->updated();
// ******************
// save the new setting to SD card
if (save_to_sdcard)
saveObjectToSD(obj);
// ******************
// debug
/*
qDebug() << "setting HomeLocation UAV Object .. " << endl;
QString s;
s = " LAT:" + QString::number(LLA[0], 'f', 7) + " LON:" + QString::number(LLA[1], 'f', 7) + " ALT:" + QString::number(LLA[2], 'f', 1);
qDebug() << s << endl;
s = " ECEF "; for (int i = 0; i < 3; i++) s += " " + QString::number((int)(ECEF[i] * 100));
qDebug() << s << endl;
s = " RNE "; for (int i = 0; i < 9; i++) s += " " + QString::number(RNE[i], 'f', 7);
qDebug() << s << endl;
s = " Be "; for (int i = 0; i < 3; i++) s += " " + QString::number(Be[i], 'f', 2);
qDebug() << s << endl;
*/
// ******************
return 0; // OK
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;
LLA[1] = homeLocationData.Longitude;
LLA[2] = homeLocationData.Altitude;
if (LLA[0] != LLA[0]) LLA[0] = 0; // nan detection
else
@ -439,88 +370,70 @@ 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;
//int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3], double ECEF[3], double RNE[9], double Be[3])
//{
// UAVObjectField *field;
QMutexLocker locker(mutex);
// QMutexLocker locker(mutex);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -1;
//// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
//// if (!pm) return -1;
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -2;
// UAVObjectManager *om = pm->getObject<UAVObjectManager>();
// if (!om) return -2;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("HomeLocation")));
if (!obj) return -3;
// UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("HomeLocation")));
// if (!obj) return -3;
// obj->requestUpdate();
//// obj->requestUpdate();
field = obj->getField("Set");
if (!field) return -4;
set = field->getValue().toBool();
// 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("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("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("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("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("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);
// field = obj->getField(QString("Be"));
// if (!field) return -10;
// for (int i = 0; i < 3; i++)
// Be[i] = field->getDouble(i);
return 0; // OK
}
// 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
@ -539,93 +452,93 @@ int UAVObjectUtilManager::getGPSPosition(double LLA[3])
return 0; // OK
}
// ******************************
// telemetry port
//// ******************************
//// telemetry port
int UAVObjectUtilManager::setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard)
{
UAVObjectField *field;
//int UAVObjectUtilManager::setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard)
//{
// UAVObjectField *field;
QMutexLocker locker(mutex);
// QMutexLocker locker(mutex);
// ******************
// save the new settings
// // ******************
// // save the new settings
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -1;
//// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
//// if (!pm) return -1;
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -2;
// UAVObjectManager *om = pm->getObject<UAVObjectManager>();
// if (!om) return -2;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("/*TelemetrySettings*/")));
if (!obj) return -3;
// 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);
// field = obj->getField(QString("Speed"));
// if (!field) return -4;
// field->setValue(speed);
obj->updated();
// obj->updated();
// ******************
// save the new setting to SD card
// // ******************
// // save the new setting to SD card
if (save_to_sdcard)
saveObjectToSD(obj);
// if (save_to_sdcard)
// saveObjectToSD(obj);
// ******************
// // ******************
return 0; // OK
}
// return 0; // OK
//}
int UAVObjectUtilManager::getTelemetrySerialPortSpeed(QString &speed)
{
UAVObjectField *field;
//int UAVObjectUtilManager::getTelemetrySerialPortSpeed(QString &speed)
//{
// UAVObjectField *field;
QMutexLocker locker(mutex);
// QMutexLocker locker(mutex);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -1;
//// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
//// if (!pm) return -1;
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -2;
// UAVObjectManager *om = pm->getObject<UAVObjectManager>();
// if (!om) return -2;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings")));
if (!obj) return -3;
// UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings")));
// if (!obj) return -3;
// obj->requestUpdate();
//// obj->requestUpdate();
field = obj->getField(QString("Speed"));
if (!field) return -4;
speed = field->getValue().toString();
// field = obj->getField(QString("Speed"));
// if (!field) return -4;
// speed = field->getValue().toString();
return 0; // OK
}
// return 0; // OK
//}
int UAVObjectUtilManager::getTelemetrySerialPortSpeeds(QComboBox *comboBox)
{
UAVObjectField *field;
//int UAVObjectUtilManager::getTelemetrySerialPortSpeeds(QComboBox *comboBox)
//{
// UAVObjectField *field;
QMutexLocker locker(mutex);
// QMutexLocker locker(mutex);
if (!comboBox) return -1;
// if (!comboBox) return -1;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -2;
//// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
//// if (!pm) return -2;
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -3;
// UAVObjectManager *om = pm->getObject<UAVObjectManager>();
// if (!om) return -3;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings")));
if (!obj) return -4;
// UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings")));
// if (!obj) return -4;
// obj->requestUpdate();
//// obj->requestUpdate();
field = obj->getField(QString("Speed"));
if (!field) return -5;
comboBox->addItems(field->getOptions());
// field = obj->getField(QString("Speed"));
// if (!field) return -5;
// comboBox->addItems(field->getOptions());
return 0; // OK
}
// return 0; // OK
//}
deviceDescriptorStruct UAVObjectUtilManager::getBoardDescriptionStruct()
{

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

@ -55,13 +55,13 @@ 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 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 setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard);
// int getTelemetrySerialPortSpeed(QString &speed);
// int getTelemetrySerialPortSpeeds(QComboBox *comboBox);
int getBoardModel();
QByteArray getBoardCPUSerial();
@ -78,11 +78,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);