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

OP-169: Hitl: IL2simulator - updating new UAVObjects

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1978 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2010-10-17 16:55:18 +00:00 committed by corvus
parent 7a37dc1f32
commit 8aecc1ec0a
4 changed files with 210 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

377
ground/src/plugins/hitlnew/il2simulator.cpp
View File

@ -84,7 +84,7 @@ const float IL2Simulator::TEMP_LAPSE_RATE = -0.0065; //degrees per meter
const float IL2Simulator::AIR_CONST_FACTOR = -0.0341631947363104; //several nature constants calculated into one
IL2Simulator::IL2Simulator(const SimulatorSettings& params) :
Simulator(params)
Simulator(params)
{
}
@ -96,31 +96,31 @@ IL2Simulator::~IL2Simulator()
void IL2Simulator::setupUdpPorts(const QString& host, int inPort, int outPort)
{
inSocket->connectToHost(host,inPort); // IL2
if(!inSocket->waitForConnected())
qxtLog->error(Name() + " cann't connect to UDP Port: " + QString::number(inPort));
inSocket->connectToHost(host,inPort); // IL2
if(!inSocket->waitForConnected())
qxtLog->error(Name() + " cann't connect to UDP Port: " + QString::number(inPort));
}
void IL2Simulator::transmitUpdate()
{
// Read ActuatorDesired from autopilot
ActuatorDesired::DataFields actData = actDesired->getData();
float ailerons = actData.Roll;
float elevator = actData.Pitch;
float rudder = actData.Yaw;
float throttle = actData.Throttle*2-1.0;
// Read ActuatorDesired from autopilot
ActuatorDesired::DataFields actData = actDesired->getData();
float ailerons = actData.Roll;
float elevator = actData.Pitch;
float rudder = actData.Yaw;
float throttle = actData.Throttle*2-1.0;
// Send update to Il2
QString cmd;
cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/")
.arg(throttle)
.arg(ailerons)
.arg(elevator)
.arg(rudder);
QByteArray data = cmd.toAscii();
//outSocket->write(data);
inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!!
// Send update to Il2
QString cmd;
cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/")
.arg(throttle)
.arg(ailerons)
.arg(elevator)
.arg(rudder);
QByteArray data = cmd.toAscii();
//outSocket->write(data);
inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!!
}
@ -128,17 +128,17 @@ void IL2Simulator::transmitUpdate()
* calculate air density from altitude
*/
float IL2Simulator::DENSITY(float alt) {
return (GROUNDDENSITY * pow(
((TEMP_GROUND+(TEMP_LAPSE_RATE*alt))/TEMP_GROUND),
((AIR_CONST_FACTOR/TEMP_LAPSE_RATE)-1) )
);
return (GROUNDDENSITY * pow(
((TEMP_GROUND+(TEMP_LAPSE_RATE*alt))/TEMP_GROUND),
((AIR_CONST_FACTOR/TEMP_LAPSE_RATE)-1) )
);
}
/**
* calculate air pressure from altitude
*/
float IL2Simulator::PRESSURE(float alt) {
return DENSITY(alt)*(TEMP_GROUND+(alt*TEMP_LAPSE_RATE))*AIR_CONST;
return DENSITY(alt)*(TEMP_GROUND+(alt*TEMP_LAPSE_RATE))*AIR_CONST;
}
@ -146,7 +146,7 @@ float IL2Simulator::PRESSURE(float alt) {
* calculate TAS from IAS and altitude
*/
float IL2Simulator::TAS(float IAS, float alt) {
return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt)));
return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt)));
}
/**
@ -154,158 +154,195 @@ float IL2Simulator::TAS(float IAS, float alt) {
*/
void IL2Simulator::processUpdate(const QByteArray& inp)
{
// save old flight data to calculate delta's later
old=current;
QString data(inp);
// Split
QStringList fields = data.split("/");
// save old flight data to calculate delta's later
old=current;
QString data(inp);
// Split
QStringList fields = data.split("/");
// split up response string
int t;
for (t=0; t<fields.length(); t++) {
QStringList values = fields[t].split("\\");
// parse values
if (values.length()>=2) {
int id = values[0].toInt();
float value = values[1].toFloat();
switch (id) {
case 30:
current.ias=value * KMH2MPS;
break;
case 32:
current.dZ=value;
break;
case 40:
current.Z=value;
break;
case 42:
current.azimuth=value;
break;
case 46:
current.roll=-value;
break;
case 48:
current.pitch=value;
break;
}
}
// split up response string
int t;
for (t=0; t<fields.length(); t++) {
QStringList values = fields[t].split("\\");
// parse values
if (values.length()>=2) {
int id = values[0].toInt();
float value = values[1].toFloat();
switch (id) {
case 30:
current.ias=value * KMH2MPS;
break;
case 32:
current.dZ=value;
break;
case 40:
current.Z=value;
break;
case 42:
current.azimuth=value;
break;
case 46:
current.roll=-value;
break;
case 48:
current.pitch=value;
break;
}
}
}
// measure time
current.dT = ((float)time->restart()) / 1000.0;
if (current.dT<0.001) current.dT=0.001;
current.T = old.T+current.dT;
current.i = old.i+1;
if (current.i==1) {
old.dRoll=0;
old.dPitch=0;
old.dAzimuth=0;
old.ddX=0;
old.ddX=0;
old.ddX=0;
}
// calculate TAS from alt and IAS
current.tas = TAS(current.ias,current.Z);
// assume the plane actually flies straight and no wind
// groundspeed is horizontal vector of TAS
current.groundspeed = current.tas*cos(current.pitch*DEG2RAD);
// x and y vector components
current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD);
current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD);
// simple IMS - integration over time the easy way...
current.X = old.X + (current.dX*current.dT);
current.Y = old.Y + (current.dY*current.dT);
// accelerations (filtered)
#define SPEED_FILTER 10
current.ddX = ((current.dX-old.dX)/current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1);
current.ddY = ((current.dY-old.dY)/current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1);
current.ddZ = ((current.dZ-old.dZ)/current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1);
#define TURN_FILTER 10
// turn speeds (filtered)
current.dAzimuth = (angleDifference(current.azimuth,old.azimuth)/current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1);
current.dPitch = (angleDifference(current.pitch,old.pitch)/current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1);
current.dRoll = (angleDifference(current.roll,old.roll)/current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1);
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = current.Z;
altActualData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
altActualData.Pressure = PRESSURE(current.Z)/1000.0; // kpa
// Update attActual object
AttitudeActual::DataFields attActualData;
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
attActualData.Roll = current.roll;
attActualData.Pitch = current.pitch;
attActualData.Yaw = current.azimuth;
attActualData.q1 = 0;
attActualData.q2 = 0;
attActualData.q3 = 0;
attActualData.q4 = 0;
// 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;
// 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;
// Update AttitudeRaw object (filtered gyros only for now)
AttitudeRaw::DataFields rawData;
memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
rawData = attRaw->getData();
rawData.gyros_filtered[0] = current.dRoll;
rawData.gyros_filtered[1] = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
rawData.gyros_filtered[2] = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
// Update homelocation
HomeLocation::DataFields homeData;
memset(&homeData, 0, sizeof(HomeLocation::DataFields));
homeData = posHome->getData();
homeData.Latitude = settings.latitude.toFloat() * 10e6;
homeData.Longitude = settings.longitude.toFloat() * 10e6;
homeData.Altitude = 0;
double LLA[3];
LLA[0]=settings.latitude.toFloat();
LLA[1]=settings.longitude.toFloat();
LLA[2]=0;
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;
homeData.Set=1;
// measure time
current.dT = ((float)time->restart()) / 1000.0;
current.T = old.T+current.dT;
// calculate TAS from alt and IAS
current.tas = TAS(current.ias,current.Z);
// assume the plane actually flies straight and no wind
// groundspeed is horizontal vector of TAS
current.groundspeed = current.tas*cos(current.pitch*DEG2RAD);
// x and y vector components
current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD);
current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD);
// simple IMS - integration over time the easy way...
current.X = old.X + (current.dX*current.dT);
current.Y = old.Y + (current.dY*current.dT);
// accelerations (filtered)
#define SPEED_FILTER 2
current.ddX = ((current.dX-old.dX) + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1);
current.ddY = ((current.dY-old.dY) + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1);
current.ddZ = ((current.dZ-old.dZ) + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1);
#define TURN_FILTER 2
// turn speeds (filtered)
current.dAzimuth = ((current.azimuth-old.azimuth) + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1);
current.dPitch = ((current.pitch-old.pitch) + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1);
current.dRoll = ((current.roll-old.roll) + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1);
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = current.Z;
altActualData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
altActualData.Pressure = PRESSURE(current.Z)/1000.0; // kpa
altActual->setData(altActualData);
// Update attActual object
AttitudeActual::DataFields attActualData;
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
attActualData.Roll = current.roll;
attActualData.Pitch = current.pitch;
attActualData.Yaw = current.azimuth;
attActualData.q1 = 0;
attActualData.q2 = 0;
attActualData.q3 = 0;
attActualData.q4 = 0;
attActual->setData(attActualData);
// Update positionActual objects
PositionActual::DataFields posData;
posData.North = current.Y*100;
posData.East = current.X*100;
posData.Down = current.Z*-100;
posActual->setData(posData);
// Update velocityActual objects
VelocityActual::DataFields velData;
velData.North = current.dY*100;
velData.East = current.dX*100;
velData.Down = current.dZ*100;
velActual->setData(velData);
// Update AttitudeRaw object (filtered gyros only for now)
AttitudeRaw::DataFields rawData;
rawData.gyros_filtered[0] = current.dRoll;
rawData.gyros_filtered[1] = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
rawData.gyros_filtered[2] = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
attRaw->setData(rawData);
// Update homelocation
HomeLocation::DataFields homeData;
homeData.Latitude = settings.latitude.toFloat() * 10e6;
homeData.Longitude = settings.longitude.toFloat() * 10e6;
homeData.Altitude = 0;
double LLA[3];
LLA[0]=settings.latitude.toFloat();
LLA[1]=settings.longitude.toFloat();
LLA[2]=0;
double ECEF[3];
Utils::CoordinateConversions().RneFromLLA(LLA,(double(*)[3])homeData.RNE);
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;
posHome->setData(homeData);
// Update gps objects
// Update gps objects
GPSPosition::DataFields gpsData;
memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
gpsData.Altitude = current.Z;
gpsData.Heading = current.azimuth;
gpsData.Groundspeed = current.groundspeed;
double NED[3];
NED[0] = current.Y;
NED[1] = current.X;
NED[2] = -current.Z;
Utils::CoordinateConversions().GetLLA(ECEF,NED,LLA);
gpsData.Latitude = LLA[0];
gpsData.Longitude = LLA[1];
gpsData.Satellites = 7;
gpsData.Heading = current.azimuth;
gpsData.Groundspeed = current.groundspeed;
double NED[3];
NED[0] = current.Y;
NED[1] = current.X;
NED[2] = -current.Z;
Utils::CoordinateConversions().GetLLA(ECEF,NED,LLA);
gpsData.Latitude = LLA[0] * 10e6;
gpsData.Longitude = LLA[1] * 10e6;
gpsData.Satellites = 7;
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData);
// issue manual update
attActual->updated();
altActual->updated();
posActual->updated();
velActual->updated();
posHome->updated();
gpsPos->updated();
// issue manual update
// update every time (50ms)
attActual->setData(attActualData);
attActual->updated();
attRaw->setData(rawData);
attRaw->updated();
// update every 5th time (250 ms)
if ( ! ((old.i-1) % 5) ) {
velActual->setData(velData);
velActual->updated();
posActual->setData(posData);
posActual->updated();
altActual->setData(altActualData);
altActual->updated();
gpsPos->setData(gpsData);
gpsPos->updated();
}
// update every 20th time (1000ms)
if ( ! ((old.i-1) % 20) ) {
posHome->setData(homeData);
posHome->updated();
}
}
/**
* process data string from flight simulator
*/
float IL2Simulator::angleDifference(float a, float b)
{
float d=a-b;
if (d>180) d-=360;
if (d<-180)d+=360;
return d;
}

1
ground/src/plugins/hitlnew/il2simulator.h
View File

@ -65,6 +65,7 @@ private:
float TAS(float ias,float alt);
void processUpdate(const QByteArray& data);
float angleDifference(float a,float b);
};

1
ground/src/plugins/hitlnew/simulator.cpp
View File

@ -172,6 +172,7 @@ void Simulator::onStart()
time = new QTime();
time->start();
current.T=0;
current.i=0;
}
void Simulator::receiveUpdate()

1
ground/src/plugins/hitlnew/simulator.h
View File

@ -56,6 +56,7 @@
// time
float T;
float dT;
unsigned int i;
// speed (relative)
float ias;