From 8c94e931d27ce0685ff7394ef92ffc09fda6e6e8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 26 May 2012 17:27:22 +0200 Subject: [PATCH] Bugfix: Fixed and reenabled HITL (backport of working code from revo repo) --- .../src/plugins/hitlnew/fgsimulator.cpp | 47 +++++++++------- .../src/plugins/hitlnew/il2simulator.cpp | 56 ++++++++++--------- .../src/plugins/hitlnew/simulator.cpp | 8 ++- .../src/plugins/hitlnew/simulator.h | 6 +- .../src/plugins/hitlnew/xplanesimulator.cpp | 50 +++++++++-------- ground/openpilotgcs/src/plugins/plugins.pro | 11 ++-- 6 files changed, 100 insertions(+), 78 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp index abfb9f75f..fec61c88d 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp @@ -308,13 +308,7 @@ void FGSimulator::processUpdate(const QByteArray& inp) double ECEF[3]; double RNE[9]; Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - for (int t=0;t<9;t++) { - homeData.RNE[t]=RNE[t]; - } Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - homeData.ECEF[0]=ECEF[0]*100; - homeData.ECEF[1]=ECEF[1]*100; - homeData.ECEF[2]=ECEF[2]*100; homeData.Be[0]=0; homeData.Be[1]=0; homeData.Be[2]=0; @@ -335,7 +329,7 @@ void FGSimulator::processUpdate(const QByteArray& inp) memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); positionActualData.North = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance positionActualData.East = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance - positionActualData.Down = (altitude * 100); //Multiply by 100 because positionActual expects input in Centimeters. + positionActualData.Down = altitude ; //Multiply by 1 because positionActual expects input in meters. posActual->setData(positionActualData); // Update AltitudeActual object @@ -371,26 +365,39 @@ void FGSimulator::processUpdate(const QByteArray& inp) gpsPos->setData(gpsData); float NED[3]; - double LLA[3] = {(double) gpsData.Latitude / 1e7, (double) gpsData.Longitude / 1e7, (double) (gpsData.GeoidSeparation + gpsData.Altitude)}; // convert from cm back to meters - double ECEF[3] = {(double) (homeData.ECEF[0] / 100), (double) (homeData.ECEF[1] / 100), (double) (homeData.ECEF[2] / 100)}; - Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) homeData.RNE, NED); - positionActualData.North = NED[0]*100; //Currently hardcoded as there is no way of setting up a reference point to calculate distance - positionActualData.East = NED[1]*100; //Currently hardcoded as there is no way of setting up a reference point to calculate distance - positionActualData.Down = NED[2]*100; //Multiply by 100 because positionActual expects input in Centimeters. + double hLLA[3] = {(double) homeData.Latitude / 1e7, (double) homeData.Longitude / 1e7, (double) (homeData.Altitude)}; + double ECEF[3]; + double RNE[9]; + Utils::CoordinateConversions().RneFromLLA(hLLA,(double (*)[3])RNE); + Utils::CoordinateConversions().LLA2ECEF(hLLA,ECEF); + Utils::CoordinateConversions().LLA2Base(hLLA, ECEF, (float (*)[3]) RNE, NED); + + positionActualData.North = NED[0]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance + positionActualData.East = NED[1]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance + positionActualData.Down = NED[2]; //Multiply by 1 because positionActual expects input in meters. posActual->setData(positionActualData); // Update AttitudeRaw object (filtered gyros only for now) - AttitudeRaw::DataFields rawData; - memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); - rawData = attRaw->getData(); - rawData.gyros[0] = rollRate; + //AttitudeRaw::DataFields rawData; + //AttitudeRaw::DataFields rawData; + Gyros::DataFields gyroData; + Accels::DataFields accelData; + memset(&gyroData, 0, sizeof(Gyros::DataFields)); + memset(&accelData, 0, sizeof(Accels::DataFields)); + gyroData = gyros->getData(); + accelData = accels->getData(); + //rawData.gyros[0] = rollRate; //rawData.gyros[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate; //rawData.gyros[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate; - rawData.gyros[1] = pitchRate; - rawData.gyros[2] = yawRate; - attRaw->setData(rawData); + //rawData.gyros[1] = pitchRate; + //rawData.gyros[2] = yawRate; + gyroData.x = rollRate; + gyroData.y = pitchRate; + gyroData.z = yawRate; + // TODO: Accels are still missing!!!! + gyros->setData(gyroData); // attRaw->updated(); } diff --git a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp index 0d957afe4..78f78ffac 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp @@ -263,35 +263,45 @@ void IL2Simulator::processUpdate(const QByteArray& inp) // Update positionActual objects PositionActual::DataFields posData; memset(&posData, 0, sizeof(PositionActual::DataFields)); - posData.North = current.Y*100; - posData.East = current.X*100; - posData.Down = current.Z*-100; + posData.North = current.Y; + posData.East = current.X; + posData.Down = current.Z*-1.; // Update velocityActual objects VelocityActual::DataFields velData; memset(&velData, 0, sizeof(VelocityActual::DataFields)); - velData.North = current.dY*100; - velData.East = current.dX*100; - velData.Down = current.dZ*-100; + velData.North = current.dY; + velData.East = current.dX; + velData.Down = current.dZ*-1.; // Update AttitudeRaw object (filtered gyros and accels only for now) - AttitudeRaw::DataFields rawData; - memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); - rawData = attRaw->getData(); + //AttitudeRaw::DataFields rawData; + //memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); + //rawData = attRaw->getData(); + Gyros::DataFields gyroData; + Accels::DataFields accelData; + memset(&gyroData, 0, sizeof(Gyros::DataFields)); + memset(&accelData, 0, sizeof(Accels::DataFields)); + gyroData = gyros->getData(); + accelData = accels->getData(); + // rotate turn rates and accelerations into body frame // (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!) - rawData.gyros[0] = current.dRoll; - rawData.gyros[1] = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth; - rawData.gyros[2] = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; + gyroData.x = current.dRoll; + gyroData.y = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth; + gyroData.z = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; // accels are in NED and can be converted using standard ned->local rotation matrix float Rbe[3][3]; Utils::CoordinateConversions().Quaternion2R(quat,Rbe); - for (int t=0;t<3;t++) { - rawData.accels[t]=current.ddX*Rbe[t][0] - +current.ddY*Rbe[t][1] - +(current.ddZ+GEE)*Rbe[t][2]; - } - rawData.accels[2]=-rawData.accels[2]; + accelData.x = current.ddX*Rbe[0][0] + +current.ddY*Rbe[0][1] + +(current.ddZ+GEE)*Rbe[0][2]; + accelData.y = current.ddX*Rbe[1][0] + +current.ddY*Rbe[1][1] + +(current.ddZ+GEE)*Rbe[1][2]; + accelData.z = - (current.ddX*Rbe[2][0] + +current.ddY*Rbe[2][1] + +(current.ddZ+GEE)*Rbe[2][2]); // Update homelocation HomeLocation::DataFields homeData; @@ -307,13 +317,7 @@ void IL2Simulator::processUpdate(const QByteArray& inp) double ECEF[3]; double RNE[9]; Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - for (int t=0;t<9;t++) { - homeData.RNE[t]=RNE[t]; - } Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - homeData.ECEF[0]=ECEF[0]*100; - homeData.ECEF[1]=ECEF[1]*100; - homeData.ECEF[2]=ECEF[2]*100; homeData.Be[0]=0; homeData.Be[1]=0; homeData.Be[2]=0; @@ -339,7 +343,9 @@ void IL2Simulator::processUpdate(const QByteArray& inp) // update every time (50ms) attActual->setData(attActualData); //attActual->updated(); - attRaw->setData(rawData); + //attRaw->setData(rawData); + gyros->setData(gyroData); + accels->setData(accelData); //attRaw->updated(); velActual->setData(velData); //velActual->updated(); diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp index e701f0485..2cb7f9ae8 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp @@ -49,7 +49,7 @@ Simulator::Simulator(const SimulatorSettings& params) : outSocket(NULL), settings(params), updatePeriod(50), - simTimeout(2000), + simTimeout(8000), autopilotConnectionStatus(false), simConnectionStatus(false), txTimer(NULL), @@ -131,7 +131,8 @@ void Simulator::onStart() posActual = PositionActual::GetInstance(objManager); altActual = BaroAltitude::GetInstance(objManager); attActual = AttitudeActual::GetInstance(objManager); - attRaw = AttitudeRaw::GetInstance(objManager); + accels = Accels::GetInstance(objManager); + gyros = Gyros::GetInstance(objManager); gpsPos = GPSPosition::GetInstance(objManager); telStats = GCSTelemetryStats::GetInstance(objManager); @@ -225,7 +226,8 @@ void Simulator::setupObjects() setupOutputObject(posActual, 250); setupOutputObject(velActual, 250); setupOutputObject(posHome, 1000); - setupOutputObject(attRaw, 10); + setupOutputObject(accels, 10); + setupOutputObject(gyros, 10); //setupOutputObject(attRaw, 100); diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h index 3f5f65355..b345f0a5e 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h @@ -44,7 +44,8 @@ #include "attitudeactual.h" #include "gpsposition.h" #include "homelocation.h" -#include "attituderaw.h" +#include "accels.h" +#include "gyros.h" #include "gcstelemetrystats.h" #include "flightstatus.h" @@ -177,7 +178,8 @@ protected: VelocityActual* velActual; PositionActual* posActual; HomeLocation* posHome; - AttitudeRaw* attRaw; + Accels* accels; + Gyros* gyros; GPSPosition* gpsPos; GCSTelemetryStats* telStats; diff --git a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp index ab8e485f4..66dbfd1d6 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp @@ -306,13 +306,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) double ECEF[3]; double RNE[9]; Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - for (int t=0;t<9;t++) { - homeData.RNE[t]=RNE[t]; - } Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - homeData.ECEF[0]=ECEF[0]*100; - homeData.ECEF[1]=ECEF[1]*100; - homeData.ECEF[2]=ECEF[2]*100; homeData.Be[0]=0; homeData.Be[1]=0; homeData.Be[2]=0; @@ -368,33 +362,45 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) // Update VelocityActual.{Nort,East,Down} VelocityActual::DataFields velocityActualData; memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); - velocityActualData.North = velY*100; - velocityActualData.East = velX*100; - velocityActualData.Down = -velZ*100; + velocityActualData.North = velY; + velocityActualData.East = velX; + velocityActualData.Down = -velZ; velActual->setData(velocityActualData); // Update PositionActual.{Nort,East,Down} PositionActual::DataFields positionActualData; memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); - positionActualData.North = (dstY-initY)*100; - positionActualData.East = (dstX-initX)*100; - positionActualData.Down = -(dstZ-initZ)*100; + positionActualData.North = (dstY-initY); + positionActualData.East = (dstX-initX); + positionActualData.Down = -(dstZ-initZ); posActual->setData(positionActualData); // Update AttitudeRaw object (filtered gyros only for now) - AttitudeRaw::DataFields rawData; - memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); - rawData = attRaw->getData(); - rawData.gyros[0] = rollRate; + //AttitudeRaw::DataFields rawData; + //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[1] = pitchRate; - rawData.gyros[2] = yawRate; - rawData.accels[0] = accX; - rawData.accels[1] = accY; - rawData.accels[2] = -accZ; - attRaw->setData(rawData); + //rawData.gyros[1] = pitchRate; + //rawData.gyros[2] = yawRate; + //rawData.accels[0] = accX; + //rawData.accels[1] = accY; + //rawData.accels[2] = -accZ; + //attRaw->setData(rawData); + Gyros::DataFields gyroData; + memset(&gyroData, 0, sizeof(Gyros::DataFields)); + gyroData.x = rollRate; + gyroData.y = pitchRate; + gyroData.z = yawRate; + gyros->setData(gyroData); + Accels::DataFields accelData; + memset(&accelData, 0, sizeof(Accels::DataFields)); + accelData.x = accX; + accelData.y = accY; + accelData.z = -accZ; + accels->setData(accelData); } // issue manual update diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index b6a9a309d..fad1b0c3a 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -140,13 +140,12 @@ plugin_ipconnection.subdir = ipconnection plugin_ipconnection.depends = plugin_coreplugin SUBDIRS += plugin_ipconnection -# Disable until updated to the new sensor objects #HITLNEW Simulation gadget -#plugin_hitlnew.subdir = hitlnew -#plugin_hitlnew.depends = plugin_coreplugin -#plugin_hitlnew.depends += plugin_uavobjects -#plugin_hitlnew.depends += plugin_uavtalk -#SUBDIRS += plugin_hitlnew +plugin_hitlnew.subdir = hitlnew +plugin_hitlnew.depends = plugin_coreplugin +plugin_hitlnew.depends += plugin_uavobjects +plugin_hitlnew.depends += plugin_uavtalk +SUBDIRS += plugin_hitlnew # Export and Import GCS Configuration plugin_importexport.subdir = importexport