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

Uses the new home location utility

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2826 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
pip 2011-02-21 11:09:20 +00:00 committed by pip
parent 7a34e25956
commit 32ae936df5
1 changed files with 50 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -41,7 +41,7 @@
#include <math.h> #include <math.h>
#include "utils/stylehelper.h" #include "utils/stylehelper.h"
#include "utils/homelocationutil.h"
#include "utils/worldmagmodel.h" #include "utils/worldmagmodel.h"
#include "uavtalk/telemetrymanager.h" #include "uavtalk/telemetrymanager.h"
@ -2390,77 +2390,75 @@ void OPMapGadgetWidget::setMapFollowingMode()
void OPMapGadgetWidget::setHomeLocationObject() void OPMapGadgetWidget::setHomeLocationObject()
{ {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); double LLA[3];
if (!pm) return; double ECEF[3];
double RNE[9];
double Be[3];
UAVObjectManager *obm = pm->getObject<UAVObjectManager>(); LLA[0] = home_position.coord.Lat();
if (!obm) return; LLA[1] = home_position.coord.Lng();
LLA[2] = home_position.altitude;
UAVDataObject *obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("HomeLocation"))); Utils::HomeLocationUtil().getDetails(LLA, ECEF, RNE, Be);
if (!obj) return;
UAVObjectField *ECEF_field = obj->getField(QString("ECEF")); // ******************
if (!ECEF_field) return; // save the new home location details
UAVObjectField *RNE_field = obj->getField(QString("RNE")); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!RNE_field) return; if (!pm) return;
UAVObjectField *Be_field = obj->getField(QString("Be")); UAVObjectManager *obm = pm->getObject<UAVObjectManager>();
if (!Be_field) return; if (!obm) return;
// double current_altitude = obj->getField("Altitude")->getDouble(); UAVDataObject *obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("HomeLocation")));
if (!obj) return;
double LLA[3]; UAVObjectField *ECEF_field = obj->getField(QString("ECEF"));
double ECEF[3]; if (!ECEF_field) return;
double RNE[9];
double Be[3];
QDateTime dt = QDateTime::currentDateTime().toUTC(); UAVObjectField *RNE_field = obj->getField(QString("RNE"));
if (!RNE_field) return;
LLA[0] = home_position.coord.Lat(); UAVObjectField *Be_field = obj->getField(QString("Be"));
LLA[1] = home_position.coord.Lng(); if (!Be_field) return;
LLA[2] = home_position.altitude;
Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF); obj->getField("Set")->setValue("TRUE");
Utils::CoordinateConversions().RneFromLLA(LLA, (double (*)[3])RNE);
if (Utils::WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be) < 0)
return;
// debug obj->getField("Latitude")->setValue(LLA[0] * 10e6);
qDebug() << "opmap setting HomeLocation UAV Object .. " << endl; obj->getField("Longitude")->setValue(LLA[1] * 10e6);
QString s; obj->getField("Altitude")->setValue(LLA[2]);
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;
// send the new position to the OP board for (int i = 0; i < 3; i++)
ECEF_field->setValue(ECEF[i] * 100, i);
obj->getField("Set")->setValue("TRUE"); for (int i = 0; i < 9; i++)
RNE_field->setDouble(RNE[i], i);
obj->getField("Latitude")->setValue(LLA[0] * 10e6); for (int i = 0; i < 3; i++)
obj->getField("Longitude")->setValue(LLA[1] * 10e6); Be_field->setDouble(Be[i], i);
obj->getField("Altitude")->setValue(LLA[2]);
for (int i = 0; i < 3; i++) obj->updated();
ECEF_field->setValue(ECEF[i] * 100, i);
for (int i = 0; i < 9; i++) // ******************
RNE_field->setDouble(RNE[i], i); // save the new location to SD card
for (int i = 0; i < 3; i++) // saveObjectToSD(obj);
Be_field->setDouble(Be[i], i);
obj->updated(); // ******************
// debug
// save the new location to SD card .. don't use this yet qDebug() << "opmap setting HomeLocation UAV Object .. " << endl;
// saveObjectToSD(obj); 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;
// *************** // ******************
} }
// ************************************************************************************* // *************************************************************************************