mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-16 08:29:15 +01:00
Merge branch 'pt/MapEnhancements' of ssh://git.openpilot.org/OpenPilot into pt/MapEnhancements
This commit is contained in:
commit
f4bd4c3120
@ -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];
|
||||
|
@ -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]);
|
||||
|
@ -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>
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
|
@ -49,7 +49,7 @@
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>16777215</height>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="contextMenuPolicy">
|
||||
|
@ -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
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user