mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-13 20:48:42 +01:00
317 lines
10 KiB
C++
317 lines
10 KiB
C++
/**
|
|
******************************************************************************
|
|
*
|
|
* @file il2simulator.cpp
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief
|
|
* @see The GNU Public License (GPL) Version 3
|
|
* @defgroup hitlplugin
|
|
* @{
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
/*
|
|
* Description of DeviceLink Protocol:
|
|
* A Request is initiated with R/ followed by id's of to be requested settings
|
|
* even id's indicate read only values, odd are write only
|
|
* (usually id =get value id+1= set - for same setting)
|
|
* id's are separated by /
|
|
* requests can contain values to set, or to select a subsystem
|
|
* values are separated by \
|
|
* example: R/30/48/64\0/64\1/
|
|
* request read only settings 30,48 and 64 with parameters 0 and 1
|
|
* the answer consists of an A followed by id value pairs in the same format
|
|
* example: A/30\0/48\0/64\0\22/64\1\102/
|
|
*
|
|
* A full protocol description as well as a list of ID's and their meanings
|
|
* can be found shipped with IL2 in the file DeviceLink.txt
|
|
*
|
|
* id's used in this file:
|
|
* 30: CAS in km/h (float)
|
|
* 32: vario in m/s (float)
|
|
* 38: angular speed °/s (float) (which direction? azimuth?)
|
|
* 40: barometric alt in m (float)
|
|
* 42: flight course in ° (0-360) (float)
|
|
* 46: roll angle in ° (-180 - 180) (floatniguration)
|
|
* 48: pitch angle in ° (-90 - 90) (float)
|
|
* 80/81: engine power (-1.0 (0%) - 1.0 (100%)) (float)
|
|
* 84/85: aileron servo (-1.0 - 1.0) (float)
|
|
* 86/87: elevator servo (-1.0 - 1.0) (float)
|
|
* 88/89: rudder servo (-1.0 - 1.0) (float)
|
|
*
|
|
* IL2 currently offers no useful way of providing GPS data
|
|
* therefore fake GPS data will be calculated using IMS
|
|
*
|
|
* unfortunately angular acceleration provided is very limited, too
|
|
*/
|
|
|
|
|
|
#include "il2simulator.h"
|
|
#include "extensionsystem/pluginmanager.h"
|
|
#include <coreplugin/icore.h>
|
|
#include <coreplugin/threadmanager.h>
|
|
#include <math.h>
|
|
|
|
const float IL2Simulator::FT2M = 12 * .254;
|
|
const float IL2Simulator::KT2MPS = 0.514444444;
|
|
const float IL2Simulator::MPS2KMH = 3.6;
|
|
const float IL2Simulator::KMH2MPS = (1.0 / 3.6);
|
|
const float IL2Simulator::INHG2KPA = 3.386;
|
|
const float IL2Simulator::RAD2DEG = (180.0 / M_PI);
|
|
const float IL2Simulator::DEG2RAD = (M_PI / 180.0);
|
|
const float IL2Simulator::NM2DEG = 60. * 1852.; // 60 miles per degree times 1852 meters per mile
|
|
const float IL2Simulator::DEG2NM = (1.0 / (60. * 1852.));
|
|
|
|
IL2Simulator::IL2Simulator(const SimulatorSettings & params) :
|
|
Simulator(params)
|
|
{
|
|
airParameters = getAirParameters();
|
|
}
|
|
|
|
|
|
IL2Simulator::~IL2Simulator()
|
|
{}
|
|
|
|
void IL2Simulator::setupUdpPorts(const QString & host, int inPort, int outPort)
|
|
{
|
|
Q_UNUSED(outPort);
|
|
|
|
inSocket->connectToHost(host, inPort); // IL2
|
|
if (!inSocket->waitForConnected()) {
|
|
qCritical() << 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.Thrust * 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.toLatin1();
|
|
// outSocket->write(data);
|
|
inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!!
|
|
}
|
|
|
|
|
|
/**
|
|
* process data string from flight simulator
|
|
*/
|
|
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("/");
|
|
|
|
// 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.cas = 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 CAS
|
|
float gravity = 9.805;
|
|
current.tas = cas2tas(current.cas, current.Z, airParameters, gravity);
|
|
|
|
// 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)
|
|
if (isnan(old.ddX) || isinf(old.ddX)) {
|
|
old.ddX = 0;
|
|
}
|
|
if (isnan(old.ddY) || isinf(old.ddY)) {
|
|
old.ddY = 0;
|
|
}
|
|
if (isnan(old.ddZ) || isinf(old.ddZ)) {
|
|
old.ddZ = 0;
|
|
}
|
|
#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)
|
|
if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) {
|
|
old.dAzimuth = 0;
|
|
}
|
|
if (isnan(old.dPitch) || isinf(old.dPitch)) {
|
|
old.dPitch = 0;
|
|
}
|
|
if (isnan(old.dRoll) || isinf(old.dRoll)) {
|
|
old.dRoll = 0;
|
|
}
|
|
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);
|
|
|
|
///////
|
|
// Output formatting
|
|
///////
|
|
Output2Hardware out;
|
|
memset(&out, 0, sizeof(Output2Hardware));
|
|
|
|
// Compute rotation matrix, for later calculations
|
|
float Rbe[3][3];
|
|
float rpy[3];
|
|
float quat[4];
|
|
rpy[0] = current.roll;
|
|
rpy[1] = current.pitch;
|
|
rpy[2] = current.azimuth;
|
|
Utils::CoordinateConversions().RPY2Quaternion(rpy, quat);
|
|
Utils::CoordinateConversions().Quaternion2R(quat, Rbe);
|
|
|
|
// Update GPS Position objects
|
|
double HomeLLA[3];
|
|
double LLA[3];
|
|
double NED[3];
|
|
HomeLLA[0] = settings.latitude.toFloat();
|
|
HomeLLA[1] = settings.longitude.toFloat();
|
|
HomeLLA[2] = 0;
|
|
NED[0] = current.Y;
|
|
NED[1] = current.X;
|
|
NED[2] = -current.Z;
|
|
Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA, NED, LLA);
|
|
out.latitude = LLA[0] * 1e7;
|
|
out.longitude = LLA[1] * 1e7;
|
|
out.groundspeed = current.groundspeed;
|
|
|
|
out.calibratedAirspeed = current.cas;
|
|
out.trueAirspeed = cas2tas(current.cas, current.Z, airParameters, gravity);
|
|
|
|
out.dstN = current.Y;
|
|
out.dstE = current.X;
|
|
out.dstD = -current.Z;
|
|
|
|
// Update BaroSensor object
|
|
out.altitude = current.Z;
|
|
out.agl = current.Z;
|
|
out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0;
|
|
out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity); // kpa
|
|
|
|
|
|
// Update attState object
|
|
out.roll = current.roll; // roll;
|
|
out.pitch = current.pitch; // pitch
|
|
out.heading = current.azimuth; // yaw
|
|
|
|
|
|
// Update VelocityState.{North,East,Down}
|
|
out.velNorth = current.dY;
|
|
out.velEast = current.dX;
|
|
out.velDown = -current.dZ;
|
|
|
|
// rotate turn rates and accelerations into body frame
|
|
// (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
|
|
out.rollRate = current.dRoll;
|
|
out.pitchRate = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
|
|
out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
|
|
|
|
// Update accelerometer sensor data
|
|
out.accX = current.ddX * Rbe[0][0]
|
|
+ current.ddY * Rbe[0][1]
|
|
+ (current.ddZ + GEE) * Rbe[0][2];
|
|
out.accY = current.ddX * Rbe[1][0]
|
|
+ current.ddY * Rbe[1][1]
|
|
+ (current.ddZ + GEE) * Rbe[1][2];
|
|
out.accZ = -(current.ddX * Rbe[2][0]
|
|
+ current.ddY * Rbe[2][1]
|
|
+ (current.ddZ + GEE) * Rbe[2][2]);
|
|
|
|
updateUAVOs(out);
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|