rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +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 { 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 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 ECEF[3], double RNE[9], 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);
CoordinateConversions().LLA2ECEF(LLA, ECEF); return 0; // OK
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
} }
} }

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

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

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

@ -46,7 +46,6 @@
#include "uavtalk/telemetrymanager.h" #include "uavtalk/telemetrymanager.h"
#include "uavobject.h" #include "uavobject.h"
#include "uavobjectmanager.h"
#include "positionactual.h" #include "positionactual.h"
#include "homelocation.h" #include "homelocation.h"
@ -590,6 +589,7 @@ void OPMapGadgetWidget::updatePosition()
VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm); VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm);
Gyros *gyrosObj = Gyros::GetInstance(obm); Gyros *gyrosObj = Gyros::GetInstance(obm);
Q_ASSERT(attitudeActualObj);
Q_ASSERT(positionActualObj); Q_ASSERT(positionActualObj);
Q_ASSERT(velocityActualObj); Q_ASSERT(velocityActualObj);
Q_ASSERT(gyrosObj); Q_ASSERT(gyrosObj);

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

@ -35,7 +35,10 @@
#include <QEventLoop> #include <QEventLoop>
#include <QTimer> #include <QTimer>
#include <objectpersistence.h> #include <objectpersistence.h>
#include <firmwareiapobj.h>
#include "firmwareiapobj.h"
#include "homelocation.h"
#include "gpsposition.h"
// ****************************** // ******************************
// constructor/destructor // constructor/destructor
@ -48,6 +51,18 @@ UAVObjectUtilManager::UAVObjectUtilManager()
failureTimer.setSingleShot(true); failureTimer.setSingleShot(true);
failureTimer.setInterval(1000); failureTimer.setInterval(1000);
connect(&failureTimer, SIGNAL(timeout()),this,SLOT(objectPersistenceOperationFailed())); 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() UAVObjectUtilManager::~UAVObjectUtilManager()
@ -67,10 +82,8 @@ UAVObjectUtilManager::~UAVObjectUtilManager()
UAVObjectManager* UAVObjectUtilManager::getObjectManager() { UAVObjectManager* UAVObjectUtilManager::getObjectManager() {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(obm);
UAVObjectManager * objMngr = pm->getObject<UAVObjectManager>(); return obm;
Q_ASSERT(objMngr);
return objMngr;
} }
@ -233,16 +246,7 @@ FirmwareIAPObj::DataFields UAVObjectUtilManager::getFirmwareIap()
{ {
FirmwareIAPObj::DataFields dummy; 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); Q_ASSERT(firmwareIap);
if (!firmwareIap) if (!firmwareIap)
return dummy; return dummy;
@ -306,121 +310,48 @@ QByteArray UAVObjectUtilManager::getBoardDescription()
int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard) int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard)
{ {
double ECEF[3]; double Be[3];
double RNE[9];
double Be[3];
UAVObjectField *field;
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
// ****************** 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] * 1e7;
UAVObjectManager *om = pm->getObject<UAVObjectManager>(); homeLocationData.Be[0] = Be[0];
if (!om) return -3; homeLocationData.Be[1] = Be[1];
homeLocationData.Be[2] = Be[2];
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("HomeLocation"))); homeLocationData.Set = HomeLocation::SET_TRUE;
if (!obj) return -4;
UAVObjectField *ECEF_field = obj->getField(QString("ECEF")); homeLocation->setData(homeLocationData);
if (!ECEF_field) return -5; homeLocation->updated();
UAVObjectField *RNE_field = obj->getField(QString("RNE")); if (save_to_sdcard)
if (!RNE_field) return -6; saveObjectToSD(homeLocation);
UAVObjectField *Be_field = obj->getField(QString("Be")); return 0;
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
} }
int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3]) 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;
if (!om) return -2; LLA[1] = homeLocationData.Longitude;
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 if (LLA[0] != LLA[0]) LLA[0] = 0; // nan detection
else else
@ -439,88 +370,70 @@ int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3])
return 0; // OK return 0; // OK
} }
int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3], double ECEF[3], double RNE[9], double Be[3]) //int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3], double ECEF[3], double RNE[9], double Be[3])
{ //{
UAVObjectField *field; // UAVObjectField *field;
QMutexLocker locker(mutex); // QMutexLocker locker(mutex);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); //// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -1; //// if (!pm) return -1;
UAVObjectManager *om = pm->getObject<UAVObjectManager>(); // UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -2; // if (!om) return -2;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("HomeLocation"))); // UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("HomeLocation")));
if (!obj) return -3; // if (!obj) return -3;
// obj->requestUpdate(); //// obj->requestUpdate();
field = obj->getField("Set"); // field = obj->getField("Set");
if (!field) return -4; // if (!field) return -4;
set = field->getValue().toBool(); // set = field->getValue().toBool();
field = obj->getField("Latitude"); // field = obj->getField("Latitude");
if (!field) return -5; // if (!field) return -5;
LLA[0] = field->getDouble() * 1e-7; // LLA[0] = field->getDouble() * 1e-7;
field = obj->getField("Longitude"); // field = obj->getField("Longitude");
if (!field) return -6; // if (!field) return -6;
LLA[1] = field->getDouble() * 1e-7; // LLA[1] = field->getDouble() * 1e-7;
field = obj->getField("Altitude"); // field = obj->getField("Altitude");
if (!field) return -7; // if (!field) return -7;
LLA[2] = field->getDouble(); // LLA[2] = field->getDouble();
field = obj->getField(QString("ECEF")); // field = obj->getField(QString("ECEF"));
if (!field) return -8; // if (!field) return -8;
for (int i = 0; i < 3; i++) // for (int i = 0; i < 3; i++)
ECEF[i] = field->getDouble(i); // ECEF[i] = field->getDouble(i);
field = obj->getField(QString("RNE")); // field = obj->getField(QString("RNE"));
if (!field) return -9; // if (!field) return -9;
for (int i = 0; i < 9; i++) // for (int i = 0; i < 9; i++)
RNE[i] = field->getDouble(i); // RNE[i] = field->getDouble(i);
field = obj->getField(QString("Be")); // field = obj->getField(QString("Be"));
if (!field) return -10; // if (!field) return -10;
for (int i = 0; i < 3; i++) // for (int i = 0; i < 3; i++)
Be[i] = field->getDouble(i); // Be[i] = field->getDouble(i);
return 0; // OK // return 0; // OK
} //}
// ****************************** // ******************************
// GPS // GPS
int UAVObjectUtilManager::getGPSPosition(double LLA[3]) 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 if (LLA[0] != LLA[0]) LLA[0] = 0; // nan detection
else else
@ -539,93 +452,93 @@ int UAVObjectUtilManager::getGPSPosition(double LLA[3])
return 0; // OK return 0; // OK
} }
// ****************************** //// ******************************
// telemetry port //// telemetry port
int UAVObjectUtilManager::setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard) //int UAVObjectUtilManager::setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard)
{ //{
UAVObjectField *field; // UAVObjectField *field;
QMutexLocker locker(mutex); // QMutexLocker locker(mutex);
// ****************** // // ******************
// save the new settings // // save the new settings
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); //// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -1; //// if (!pm) return -1;
UAVObjectManager *om = pm->getObject<UAVObjectManager>(); // UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -2; // if (!om) return -2;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("/*TelemetrySettings*/"))); // UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("/*TelemetrySettings*/")));
if (!obj) return -3; // if (!obj) return -3;
field = obj->getField(QString("Speed")); // field = obj->getField(QString("Speed"));
if (!field) return -4; // if (!field) return -4;
field->setValue(speed); // 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) // if (save_to_sdcard)
saveObjectToSD(obj); // saveObjectToSD(obj);
// ****************** // // ******************
return 0; // OK // return 0; // OK
} //}
int UAVObjectUtilManager::getTelemetrySerialPortSpeed(QString &speed) //int UAVObjectUtilManager::getTelemetrySerialPortSpeed(QString &speed)
{ //{
UAVObjectField *field; // UAVObjectField *field;
QMutexLocker locker(mutex); // QMutexLocker locker(mutex);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); //// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -1; //// if (!pm) return -1;
UAVObjectManager *om = pm->getObject<UAVObjectManager>(); // UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -2; // if (!om) return -2;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings"))); // UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings")));
if (!obj) return -3; // if (!obj) return -3;
// obj->requestUpdate(); //// obj->requestUpdate();
field = obj->getField(QString("Speed")); // field = obj->getField(QString("Speed"));
if (!field) return -4; // if (!field) return -4;
speed = field->getValue().toString(); // speed = field->getValue().toString();
return 0; // OK // return 0; // OK
} //}
int UAVObjectUtilManager::getTelemetrySerialPortSpeeds(QComboBox *comboBox) //int UAVObjectUtilManager::getTelemetrySerialPortSpeeds(QComboBox *comboBox)
{ //{
UAVObjectField *field; // UAVObjectField *field;
QMutexLocker locker(mutex); // QMutexLocker locker(mutex);
if (!comboBox) return -1; // if (!comboBox) return -1;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); //// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return -2; //// if (!pm) return -2;
UAVObjectManager *om = pm->getObject<UAVObjectManager>(); // UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -3; // if (!om) return -3;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings"))); // UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("TelemetrySettings")));
if (!obj) return -4; // if (!obj) return -4;
// obj->requestUpdate(); //// obj->requestUpdate();
field = obj->getField(QString("Speed")); // field = obj->getField(QString("Speed"));
if (!field) return -5; // if (!field) return -5;
comboBox->addItems(field->getOptions()); // comboBox->addItems(field->getOptions());
return 0; // OK // return 0; // OK
} //}
deviceDescriptorStruct UAVObjectUtilManager::getBoardDescriptionStruct() 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 setHomeLocation(double LLA[3], bool save_to_sdcard);
int getHomeLocation(bool &set, double LLA[3]); 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 getGPSPosition(double LLA[3]);
int setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard); // int setTelemetrySerialPortSpeed(QString speed, bool save_to_sdcard);
int getTelemetrySerialPortSpeed(QString &speed); // int getTelemetrySerialPortSpeed(QString &speed);
int getTelemetrySerialPortSpeeds(QComboBox *comboBox); // int getTelemetrySerialPortSpeeds(QComboBox *comboBox);
int getBoardModel(); int getBoardModel();
QByteArray getBoardCPUSerial(); QByteArray getBoardCPUSerial();
@ -78,11 +78,15 @@ signals:
void saveCompleted(int objectID, bool status); void saveCompleted(int objectID, bool status);
private: 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: private slots:
//void transactionCompleted(UAVObject *obj, bool success); //void transactionCompleted(UAVObject *obj, bool success);