mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
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
This commit is contained in:
Laura Sebesta
parent
1ab1ea5899
commit
433d723251
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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 trueAirspeed; //[m/s]
|
||||
float angleOfAttack;
|
||||
float angleOfSlip;
|
||||
float roll;
|
||||
float pitch;
|
||||
float pressure;
|
||||
@ -169,7 +174,7 @@ struct Output2Hardware{
|
||||
float rollRate; //[deg/s]
|
||||
float pitchRate; //[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;
|
||||
|
||||
|
||||
@ -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]
|
||||
|
||||
@ -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 \
|
||||
|
||||
17
shared/uavobjectdefinition/groundtruth.xml
Normal file
17
shared/uavobjectdefinition/groundtruth.xml
Normal file
@ -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>
|
||||
Loading…
Reference in New Issue
Block a user