diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp index e1cbb6b78..aedf3e909 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp +++ b/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; diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index 99cbeb82e..24e119855 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/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; diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp index 6bec9fbf3..1e118e144 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp +++ b/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.pressure = PRESSURE(current.Z)/1000.0; // kpa + out.agl = current.Z; + out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0; + out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity) ; // kpa // Update attActual object diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index 9eb5b6eec..b26acc97f 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/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(); @@ -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) { diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index 41de0937d..359de04e3 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/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 calibratedAirspeed; //[m/s] + float groundspeed; //[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 pitchRate; //[deg/s] - float yawRate; //[deg/s] - float delT; + float rollRate; //[deg/s] + float pitchRate; //[deg/s] + float yawRate; //[deg/s] + 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; diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index 448e6f45a..c940e7add 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/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] diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index a39eb243e..b3bda1d2e 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/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 \ diff --git a/shared/uavobjectdefinition/groundtruth.xml b/shared/uavobjectdefinition/groundtruth.xml new file mode 100644 index 000000000..70bc6c6e3 --- /dev/null +++ b/shared/uavobjectdefinition/groundtruth.xml @@ -0,0 +1,17 @@ + + + The updated Attitude estimation from @ref AHRSCommsModule. + + + + + + + + + + + + + +