rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Added groundtruth simulation support.

Browse Source 
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 2012-10-23 11:47:58 +02:00
parent 1ab1ea5899
commit 433d723251
8 changed files with 86 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
View File

@ -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;

9
ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp
View File

@ -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;

5
ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp
View File

@ -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

42
ground/openpilotgcs/src/plugins/hitl/simulator.cpp
View File

@ -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) {

18
ground/openpilotgcs/src/plugins/hitl/simulator.h
View File

@ -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;

9
ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp
View File

@ -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]

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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
View 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>