diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp index 18b1fd9af..4700bdb5c 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp @@ -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 } } diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.h b/ground/openpilotgcs/src/libs/utils/homelocationutil.h index 6448c3e52..acfc43888 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.h +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.h @@ -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: diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 2d8e01506..9cb7a7cc6 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -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); diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index d68a87d59..70e766823 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/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(); - 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() { diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 11f5f0979..1a4147f94 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -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);