rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

Merge branch 'pt/MapEnhancements' of ssh://git.openpilot.org/OpenPilot into pt/MapEnhancements

Browse Source
This commit is contained in:
PT_Dreamer 2012-08-11 22:03:09 +01:00
commit f4bd4c3120
7 changed files with 65 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
View File

@ -121,19 +121,18 @@ int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3])
}
/**
* Get the current location in Longitude, Latitude Altitude (above WSG-48 ellipsoid)
* @param[in] BaseECEF the ECEF of the home location (in cm)
* Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid)
* @param[in] BaseECEF the ECEF of the home location (in m)
* @param[in] NED the offset from the home location (in m)
* @param[out] position three element double for position in degrees and meters
* @param[out] position three element double for position in decimal degrees and altitude in meters
* @returns
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double position[3])
int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3], double position[3])
{
int i;
// stored value is in cm, convert to m
double BaseECEFm[3] = {BaseECEFcm[0], BaseECEFcm[1], BaseECEFcm[2]};
double BaseLLA[3];
double ECEF[3];
double Rne [3][3];
@ -151,6 +150,29 @@ int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double po
return 0;
}
/**
* Get the current location in Longitude, Latitude, Altitude (above WSG-84 ellipsoid)
* @param[in] homeLLA the latitude, longitude, and altitude of the home location (in [m])
* @param[in] NED the offset from the home location (in [m])
* @param[out] position three element double for position in decimal degrees and altitude in meters
* @returns
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::NED2LLA_HomeLLA(double homeLLA[3], double NED[3], double position[3])
{
double T[3];
T[0] = homeLLA[2]+6.378137E6f * M_PI / 180.0;
T[1] = cosf(homeLLA[0] * M_PI / 180.0)*(homeLLA[2]+6.378137E6f) * M_PI / 180.0;
T[2] = -1.0f;
position[0] = homeLLA[0] + NED[0] / T[0];
position[1] = homeLLA[1] + NED[1] / T[1];
position[2] = homeLLA[2] + NED[2] / T[2];
return 0;
}
void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
double ECEF[3];

3
ground/openpilotgcs/src/libs/utils/coordinateconversions.h
View File

@ -41,7 +41,8 @@ class QTCREATOR_UTILS_EXPORT CoordinateConversions
{
public:
CoordinateConversions();
int GetLLA(double LLA[3], double NED[3], double position[3]);
int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]);
int NED2LLA_HomeLLA(double LLA[3], double NED[3], double position[3]);
void RneFromLLA(double LLA[3], double Rne[3][3]);
void LLA2ECEF(double LLA[3], double ECEF[3]);
int ECEF2LLA(double ECEF[3], double LLA[3]);

2
ground/openpilotgcs/src/plugins/config/input.ui
View File

@ -385,7 +385,7 @@ margin:1px;</string>
</widget>
</item>
<item row="0" column="1">
<spacer name="verticalSpacer_3">
<spacer name="verticalSpacer_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>

2
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
View File

@ -333,7 +333,7 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
NED[0] = current.Y;
NED[1] = current.X;
NED[2] = -current.Z;
Utils::CoordinateConversions().GetLLA(ECEF,NED,LLA);
Utils::CoordinateConversions().NED2LLA_HomeECEF(ECEF,NED,LLA);
gpsData.Latitude = LLA[0] * 10e6;
gpsData.Longitude = LLA[1] * 10e6;
gpsData.Satellites = 7;

24
ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp
View File

@ -210,9 +210,9 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
float accX = 0;
float accY = 0;
float accZ = 0;
float rollRate=0;
float pitchRate=0;
float yawRate=0;
float rollRate_rad=0;
float pitchRate_rad=0;
float yawRate_rad=0;
QString str;
QByteArray& buf = const_cast<QByteArray&>(dataBuf);
@ -269,10 +269,10 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
velZ = *((float*)(buf.data()+4*5));
break;
case XplaneSimulator::AngularVelocities:
pitchRate = *((float*)(buf.data()+4*1));
rollRate = *((float*)(buf.data()+4*2));
yawRate = *((float*)(buf.data()+4*3));
case XplaneSimulator::AngularVelocities: //In [rad/s]
pitchRate_rad = *((float*)(buf.data()+4*1));
rollRate_rad = *((float*)(buf.data()+4*2));
yawRate_rad = *((float*)(buf.data()+4*3));
break;
case XplaneSimulator::Gload:
@ -380,8 +380,8 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
//memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
//rawData = attRaw->getData();
//rawData.gyros[0] = rollRate;
//rawData.gyros_filtered[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate;
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate;
//rawData.gyros_filtered[1] = cos(DEG2RAD * roll) * pitchRate_rad + sin(DEG2RAD * roll) * yawRate_rad;
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate_rad - sin(DEG2RAD * roll) * pitchRate_rad;
//rawData.gyros[1] = pitchRate;
//rawData.gyros[2] = yawRate;
//rawData.accels[0] = accX;
@ -391,9 +391,9 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
Gyros::DataFields gyroData;
memset(&gyroData, 0, sizeof(Gyros::DataFields));
#define Pi 3.141529654
gyroData.x = rollRate*180/Pi;
gyroData.y = pitchRate*180/Pi;
gyroData.z = yawRate*180/Pi;
gyroData.x = rollRate_rad*180/Pi;
gyroData.y = pitchRate_rad*180/Pi;
gyroData.z = yawRate_rad*180/Pi;
gyros->setData(gyroData);
Accels::DataFields accelData;

2
ground/openpilotgcs/src/plugins/opmap/opmap_widget.ui
View File

@ -49,7 +49,7 @@
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
<height>40</height>
</size>
</property>
<property name="contextMenuPolicy">

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

@ -2017,54 +2017,43 @@ internals::PointLatLng OPMapGadgetWidget::destPoint(internals::PointLatLng sourc
bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, double &altitude)
{
double BaseECEF[3];
double NED[3];
double LLA[3];
UAVObject *obj;
double homeLLA[3];
if (!obm)
return false;
Q_ASSERT(obm != NULL);
obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("HomeLocation")));
if (!obj) return false;
BaseECEF[0] = obj->getField(QString("ECEF"))->getDouble(0) / 100;
BaseECEF[1] = obj->getField(QString("ECEF"))->getDouble(1) / 100;
BaseECEF[2] = obj->getField(QString("ECEF"))->getDouble(2) / 100;
HomeLocation *homeLocation = HomeLocation::GetInstance(obm);
Q_ASSERT(homeLocation != NULL);
HomeLocation::DataFields homeLocationData = homeLocation->getData();
obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("PositionActual")));
if (!obj) return false;
NED[0] = obj->getField(QString("North"))->getDouble() / 100;
NED[1] = obj->getField(QString("East"))->getDouble() / 100;
NED[2] = obj->getField(QString("Down"))->getDouble() / 100;
homeLLA[0] = homeLocationData.Latitude / 1e7;
homeLLA[1] = homeLocationData.Longitude / 1e7;
homeLLA[2] = homeLocationData.Altitude;
// obj = dynamic_cast<UAVDataObject*>(om->getObject(QString("PositionDesired")));
PositionActual *positionActual = PositionActual::GetInstance(obm);
Q_ASSERT(positionActual != NULL);
PositionActual::DataFields positionActualData = positionActual->getData();
// obj = dynamic_cast<UAVDataObject*>(objManager->getObject("VelocityActual")); // air speed
NED[0] = positionActualData.North;
NED[1] = positionActualData.East;
NED[2] = positionActualData.Down;
Utils::CoordinateConversions().GetLLA(BaseECEF, NED, LLA);
Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA);
latitude = LLA[0];
longitude = LLA[1];
altitude = LLA[2];
qDebug()<< " " << latitude << " " << longitude << " " << altitude;
if (latitude != latitude) latitude = 0; // nan detection
// if (isNan(latitude)) latitude = 0; // nan detection
else
// if (!isFinite(latitude)) latitude = 0;
// else
if (latitude > 90) latitude = 90;
else
if (latitude < -90) latitude = -90;
else if (latitude > 90) latitude = 90;
else if (latitude < -90) latitude = -90;
if (longitude != longitude) longitude = 0; // nan detection
else
// if (longitude > std::numeric_limits<double>::max()) longitude = 0; // +infinite
// else
// if (longitude < -std::numeric_limits<double>::max()) longitude = 0; // -infinite
// else
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
else if (longitude > 180) longitude = 180;
else if (longitude < -180) longitude = -180;
if (altitude != altitude) altitude = 0; // nan detection