rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Bugfix: Fixed and reenabled HITL (backport of working code from revo repo)

Browse Source
This commit is contained in:
Corvus Corax 2012-05-26 17:27:22 +02:00
parent f1f01f2b4b
commit 8c94e931d2
6 changed files with 100 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp
View File

@ -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();
}

56
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
View File

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

8
ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp
View File

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

6
ground/openpilotgcs/src/plugins/hitlnew/simulator.h
View File

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

50
ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp
View File

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

11
ground/openpilotgcs/src/plugins/plugins.pro
View File

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