Added groundtruth simulation support.

This new UAVO serves principally to log data for future comparison with
onboard estimation techniques.

Conflicts:

	ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp
	ground/openpilotgcs/src/plugins/hitl/simulator.h

ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp

@@ -202,6 +202,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
 
     /**********************************************************************************************/
     out.altitude = posZ;
+    out.agl = posZ;
     out.heading = yaw * RAD2DEG;
     out.latitude = lat * 10e6;
     out.longitude = lon * 10e6;

ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp

@@ -268,9 +268,9 @@ void FGSimulator::processUpdate(const QByteArray& inp)
     // Get heading (deg)
     float heading = fields[14].toFloat();
     // Get altitude (m)
-    float altitude = fields[15].toFloat() * FT2M;
+    float altitude_msl = fields[15].toFloat() * FT2M;
     // Get altitudeAGL (m)
-    float altitudeAGL = fields[16].toFloat() * FT2M;
+    float altitude_agl = fields[16].toFloat() * FT2M;
     // Get groundspeed (m/s)
     float groundspeed = fields[17].toFloat() * KT2MPS;
     // Get airspeed (m/s)
@@ -299,7 +299,7 @@ void FGSimulator::processUpdate(const QByteArray& inp)
     float NED[3];
     // convert from cm back to meters
 
-    double LLA[3] = {latitude, longitude, altitude};
+    double LLA[3] = {latitude, longitude, altitude_msl};
     double ECEF[3];
     double RNE[9];
     Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
@@ -310,7 +310,8 @@ void FGSimulator::processUpdate(const QByteArray& inp)
     // Update GPS Position objects
     out.latitude = latitude * 1e7;
     out.longitude = longitude * 1e7;
-    out.altitude = altitude;
+    out.altitude = altitude_msl;
+    out.agl = altitude_agl;
     out.groundspeed = groundspeed;
 
     out.calibratedAirspeed = airspeed;

ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp

@@ -277,8 +277,9 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
 
     // Update BaroAltitude object
     out.altitude = current.Z;
-    out.temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
+    out.agl = current.Z;
-    out.pressure = PRESSURE(current.Z)/1000.0; // kpa
+    out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0;
+    out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity) ; // kpa
 
 
     // Update attActual object

ground/openpilotgcs/src/plugins/hitl/simulator.cpp

@@ -138,7 +138,7 @@ void Simulator::onStart()
     actDesired = ActuatorDesired::GetInstance(objManager);
     actCommand = ActuatorCommand::GetInstance(objManager);
     manCtrlCommand = ManualControlCommand::GetInstance(objManager);
-    gcsReceiver= GCSReceiver::GetInstance(objManager);
+    gcsReceiver = GCSReceiver::GetInstance(objManager);
     flightStatus = FlightStatus::GetInstance(objManager);
     posHome = HomeLocation::GetInstance(objManager);
     velActual = VelocityActual::GetInstance(objManager);
@@ -152,6 +152,7 @@ void Simulator::onStart()
     gpsPos = GPSPosition::GetInstance(objManager);
     gpsVel = GPSVelocity::GetInstance(objManager);
     telStats = GCSTelemetryStats::GetInstance(objManager);
+    groundTruth = GroundTruth::GetInstance(objManager);
 
     // Listen to autopilot connection events
     TelemetryManager* telMngr = pm->getObject<TelemetryManager>();
@@ -392,6 +393,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
         memset(&noise, 0, sizeof(Noise));
     }
 
+    /*******************************/
     HomeLocation::DataFields homeData = posHome->getData();
     if(!once)
     {
@@ -401,7 +403,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
         // Update homelocation
         homeData.Latitude = out.latitude;   //Already in *10^7 integer format
         homeData.Longitude = out.longitude; //Already in *10^7 integer format
-        homeData.Altitude = out.altitude;
+        homeData.Altitude = out.agl;
         double LLA[3];
         LLA[0]=out.latitude;
         LLA[1]=out.longitude;
@@ -424,7 +426,36 @@ void Simulator::updateUAVOs(Output2Hardware out){
         once=true;
     }
 
+    /*******************************/
+    //Copy everything to the ground truth object. GroundTruth is Noise-free.
+    GroundTruth::DataFields groundTruthData;
+    groundTruthData = groundTruth->getData();
 
+    groundTruthData.AngularRates[0]=out.rollRate;
+    groundTruthData.AngularRates[1]=out.pitchRate;
+    groundTruthData.AngularRates[2]=out.yawRate;
+
+    groundTruthData.CalibratedAirspeed=out.calibratedAirspeed;
+    groundTruthData.TrueAirspeed=out.trueAirspeed;
+    groundTruthData.AngleOfAttack=out.angleOfAttack;
+    groundTruthData.AngleOfSlip=out.angleOfSlip;
+
+    groundTruthData.PositionNED[0]=out.dstN-initN;
+    groundTruthData.PositionNED[1]=out.dstE-initD;
+    groundTruthData.PositionNED[2]=out.dstD-initD;
+
+    groundTruthData.VelocityNED[0]=out.velNorth;
+    groundTruthData.VelocityNED[1]=out.velEast;
+    groundTruthData.VelocityNED[2]=out.velDown;
+
+    groundTruthData.RPY[0]=out.roll;
+    groundTruthData.RPY[0]=out.pitch;
+    groundTruthData.RPY[0]=out.heading;
+
+    //Set UAVO
+    groundTruth->setData(groundTruthData);
+
+/*******************************/
     // Update attActual object
     AttitudeActual::DataFields attActualData;
     attActualData = attActual->getData();
@@ -574,6 +605,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
         /*****************************************/
     }
 
+    /*******************************/
     if (settings.gcsReceiverEnabled) {
         if (gcsRcvrTime.msecsTo(currentTime) >= settings.minOutputPeriod) {
             GCSReceiver::DataFields gcsRcvrData;
@@ -591,6 +623,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
     }
 
 
+    /*******************************/
     if (settings.gpsPositionEnabled) {
         if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
             qDebug()<< " GPS time:" << gpsPosTime << ", currentTime: " << currentTime  << ", difference: "  << gpsPosTime.msecsTo(currentTime);
@@ -623,6 +656,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
         }
     }
 
+    /*******************************/
     // Update VelocityActual.{North,East,Down}
     if (settings.groundTruthEnabled) {
         if (groundTruthTime.msecsTo(currentTime) >= settings.groundTruthRate) {
@@ -645,6 +679,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
         }
     }
 
+//    /*******************************/
 //    if (settings.sonarAltitude) {
 //        static QTime sonarAltTime = currentTime;
 //        if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) {
@@ -666,6 +701,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
 //        }
 //    }
 
+    /*******************************/
     // Update BaroAltitude object
     if (settings.baroAltitudeEnabled){
         if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) {
@@ -680,6 +716,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
         }
     }
 
+    /*******************************/
     // Update AirspeedActual object
     if (settings.airspeedActualEnabled){
         if (airspeedActualTime.msecsTo(currentTime) >= settings.airspeedActualRate) {
@@ -692,6 +729,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
         }
     }
 
+    /*******************************/
     // Update raw attitude sensors
     if (settings.attRawEnabled) {
         if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) {

ground/openpilotgcs/src/plugins/hitl/simulator.h

@@ -50,6 +50,7 @@
 #include "gcstelemetrystats.h"
 #include "gpsposition.h"
 #include "gpsvelocity.h"
+#include "groundtruth.h"
 #include "gyros.h"
 #include "homelocation.h"
 #include "manualcontrolcommand.h"
@@ -150,9 +151,13 @@ struct Output2Hardware{
     float latitude;
     float longitude;
     float altitude;
+    float agl;                //[m]
     float heading;
-    float groundspeed; //[m/s]
+    float groundspeed;        //[m/s]
-    float calibratedAirspeed;    //[m/s]
+    float calibratedAirspeed; //[m/s]
+    float trueAirspeed;       //[m/s]
+    float angleOfAttack;
+    float angleOfSlip;
     float roll;
     float pitch;
     float pressure;
@@ -166,10 +171,10 @@ struct Output2Hardware{
     float accX;       //[m/s^2]
     float accY;       //[m/s^2]
     float accZ;       //[m/s^2]
-    float rollRate;     //[deg/s]
+    float rollRate;   //[deg/s]
-    float pitchRate;     //[deg/s]
+    float pitchRate;  //[deg/s]
-    float yawRate;     //[deg/s]
+    float yawRate;    //[deg/s]
-    float delT;
+    float delT;       //[s]
 
     float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; //Elements in rc_channel are between -1 and 1
 
@@ -275,6 +280,7 @@ protected:
     Gyros*  gyros;
     GCSTelemetryStats* telStats;
     GCSReceiver* gcsReceiver;
+    GroundTruth* groundTruth;
 
     SimulatorSettings settings;
 

ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp

@@ -195,7 +195,8 @@ void XplaneSimulator::transmitUpdate()
  */
 void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
 {
-    float altitude = 0;
+    float altitude_msl = 0;
+    float altitude_agl = 0;
     float latitude = 0;
     float longitude = 0;
     float airspeed_keas = 0;
@@ -239,7 +240,8 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
             case XplaneSimulator::LatitudeLongitudeAltitude:
                 latitude = *((float*)(buf.data()+4*1));
                 longitude = *((float*)(buf.data()+4*2));
-                altitude = *((float*)(buf.data()+4*3))* FT2M;
+                altitude_msl = *((float*)(buf.data()+4*3))* FT2M;
+                altitude_agl = *((float*)(buf.data()+4*4))* FT2M;
                 break;
 
             case XplaneSimulator::Speed:
@@ -302,7 +304,8 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
         // Update GPS Position objects
         out.latitude = latitude * 1e7;
         out.longitude = longitude * 1e7;
-        out.altitude = altitude;
+        out.altitude = altitude_msl;
+        out.agl = altitude_agl;
         out.groundspeed = groundspeed_ktgs*1.15*1.6089/3.6; //Convert from [kts] to [m/s]
 
         out.calibratedAirspeed = airspeed_keas*1.15*1.6089/3.6;  //Convert from [kts] to [m/s]

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -61,6 +61,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/mixerstatus.h \
     $$UAVOBJECT_SYNTHETICS/velocitydesired.h \
     $$UAVOBJECT_SYNTHETICS/velocityactual.h \
+    $$UAVOBJECT_SYNTHETICS/groundtruth.h \
     $$UAVOBJECT_SYNTHETICS/guidancesettings.h \
     $$UAVOBJECT_SYNTHETICS/positiondesired.h \
     $$UAVOBJECT_SYNTHETICS/relaytuning.h \
@@ -128,6 +129,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/mixerstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/velocityactual.cpp \
+    $$UAVOBJECT_SYNTHETICS/groundtruth.cpp \
     $$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \
     $$UAVOBJECT_SYNTHETICS/positiondesired.cpp \
     $$UAVOBJECT_SYNTHETICS/relaytuningsettings.cpp \

shared/uavobjectdefinition/groundtruth.xml

@@ -0,0 +1,17 @@
+<xml>
+    <object name="GroundTruth" singleinstance="true" settings="false">
+        <description>The updated Attitude estimation from @ref AHRSCommsModule.</description>
+        <field name="PositionNED" units="m" type="float" elements="3"/>
+        <field name="VelocityNED" units="m" type="float" elements="3"/>
+        <field name="RPY" units="deg" type="float" elements="3"/>
+        <field name="AngularRates" units="deg/s" type="float" elements="3"/>
+        <field name="TrueAirspeed" units="m/s" type="float" elements="1"/>
+        <field name="CalibratedAirspeed" units="m/s" type="float" elements="1"/>
+        <field name="AngleOfAttack" units="deg" type="float" elements="1"/>
+        <field name="AngleOfSlip" units="deg" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readonly"/>
+        <telemetrygcs acked="false" updatemode="periodic" period="50"/>
+        <telemetryflight acked="false" updatemode="manual" period="50000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>