rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/kenz/uavo_util_fixes' into next

Browse Source
This commit is contained in:
James Cotton 2012-09-27 14:07:47 -05:00
commit 29a970564f
5 changed files with 102 additions and 320 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,40 +38,41 @@
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 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:

20
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);
@ -919,7 +919,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;
@ -941,13 +941,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.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();
@ -1659,7 +1659,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
}

328
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,121 +310,47 @@ 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];
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);
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*1e-7;
LLA[1] = homeLocationData.Longitude*1e-7;
LLA[2] = homeLocationData.Altitude;
if (LLA[0] != LLA[0]) LLA[0] = 0; // nan detection
else
@ -439,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
@ -539,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);