mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Brought out several air parameter functions from IL2 simulator and added them to the global simulator.cpp file.
This commit is contained in:
Laura Sebesta
parent
5e785d4524
commit
b84b8f656c
@ -42,7 +42,7 @@
|
||||
* can be found shipped with IL2 in the file DeviceLink.txt
|
||||
*
|
||||
* id's used in this file:
|
||||
* 30: IAS in km/h (float)
|
||||
* 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)
|
||||
@ -68,25 +68,20 @@
|
||||
#include <math.h>
|
||||
#include <qxtlogger.h>
|
||||
|
||||
const float IL2Simulator::FT2M = 0.3048;
|
||||
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::M2DEG = 60.*1852.; // 60 miles per degree times 1852 meters per mile
|
||||
const float IL2Simulator::DEG2M = (1.0/(60.*1852.));
|
||||
const float IL2Simulator::AIR_CONST = 287.058; // J/(kg*K)
|
||||
const float IL2Simulator::GROUNDDENSITY = 1.225; // kg/m³ ;)
|
||||
const float IL2Simulator::TEMP_GROUND = (15.0 + 273.0); // 15°C in Kelvin
|
||||
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
|
||||
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();
|
||||
}
|
||||
|
||||
|
||||
@ -125,31 +120,6 @@ 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) )
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* calculate air pressure from altitude
|
||||
*/
|
||||
float IL2Simulator::PRESSURE(float alt) {
|
||||
return DENSITY(alt)*(TEMP_GROUND+(alt*TEMP_LAPSE_RATE))*AIR_CONST;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* calculate TAS from IAS and altitude
|
||||
*/
|
||||
float IL2Simulator::TAS(float IAS, float alt) {
|
||||
return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt)));
|
||||
}
|
||||
|
||||
/**
|
||||
* process data string from flight simulator
|
||||
*/
|
||||
@ -171,7 +141,7 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
|
||||
float value = values[1].toFloat();
|
||||
switch (id) {
|
||||
case 30:
|
||||
current.ias=value * KMH2MPS;
|
||||
current.cas=value * KMH2MPS;
|
||||
break;
|
||||
case 32:
|
||||
current.dZ=value;
|
||||
@ -206,8 +176,9 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
|
||||
old.ddX=0;
|
||||
}
|
||||
|
||||
// calculate TAS from alt and IAS
|
||||
current.tas = TAS(current.ias,current.Z);
|
||||
// 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
|
||||
@ -269,7 +240,8 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
|
||||
out.longitude = LLA[1] * 1e7;
|
||||
out.groundspeed = current.groundspeed;
|
||||
|
||||
out.calibratedAirspeed = current.ias;
|
||||
out.calibratedAirspeed = current.cas;
|
||||
out.trueAirspeed=cas2tas(current.cas, current.Z, airParameters, gravity);
|
||||
|
||||
out.dstN=current.Y;
|
||||
out.dstE=current.X;
|
||||
|
||||
@ -44,29 +44,20 @@ private slots:
|
||||
void transmitUpdate();
|
||||
|
||||
private:
|
||||
static const float FT2M;
|
||||
static const float KT2MPS;
|
||||
static const float MPS2KMH;
|
||||
static const float KMH2MPS;
|
||||
static const float INHG2KPA;
|
||||
static const float RAD2DEG;
|
||||
static const float DEG2RAD;
|
||||
static const float M2DEG;
|
||||
static const float DEG2M;
|
||||
static const float AIR_CONST;
|
||||
static const float GROUNDDENSITY;
|
||||
static const float TEMP_GROUND;
|
||||
static const float TEMP_LAPSE_RATE;
|
||||
static const float AIR_CONST_FACTOR;
|
||||
|
||||
|
||||
float DENSITY(float pressure);
|
||||
float PRESSURE(float alt);
|
||||
float TAS(float ias,float alt);
|
||||
static const float FT2M;
|
||||
static const float KT2MPS;
|
||||
static const float MPS2KMH;
|
||||
static const float KMH2MPS;
|
||||
static const float INHG2KPA;
|
||||
static const float RAD2DEG;
|
||||
static const float DEG2RAD;
|
||||
static const float NM2DEG;
|
||||
static const float DEG2NM;
|
||||
|
||||
void processUpdate(const QByteArray& data);
|
||||
float angleDifference(float a,float b);
|
||||
|
||||
AirParameters airParameters;
|
||||
};
|
||||
|
||||
class IL2SimulatorCreator : public SimulatorCreator
|
||||
|
||||
@ -71,6 +71,15 @@ Simulator::Simulator(const SimulatorSettings& params) :
|
||||
baroAltTime = currentTime;
|
||||
airspeedActualTime=currentTime;
|
||||
|
||||
//Define standard atmospheric constants
|
||||
airParameters.univGasConstant=8.31447; //[J/(mol·K)]
|
||||
airParameters.dryAirConstant=287.058; //[J/(kg*K)]
|
||||
airParameters.groundDensity=1.225; //[kg/m^3]
|
||||
airParameters.groundTemp=15+273.15; //[K]
|
||||
airParameters.tempLapseRate=0.0065; //[deg/m]
|
||||
airParameters.M=0.0289644; //[kg/mol]
|
||||
airParameters.relativeHumidity=20; //[%]
|
||||
airParameters.seaLevelPress=101.325; //[kPa]
|
||||
}
|
||||
|
||||
Simulator::~Simulator()
|
||||
@ -408,10 +417,7 @@ void Simulator::updateUAVOs(Output2Hardware out){
|
||||
LLA[0]=out.latitude;
|
||||
LLA[1]=out.longitude;
|
||||
LLA[2]=out.altitude;
|
||||
double ECEF[3];
|
||||
double RNE[9];
|
||||
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
|
||||
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
|
||||
|
||||
homeData.Be[0]=0;
|
||||
homeData.Be[1]=0;
|
||||
homeData.Be[2]=0;
|
||||
@ -727,6 +733,9 @@ void Simulator::updateUAVOs(Output2Hardware out){
|
||||
AirspeedActual::DataFields airspeedActualData;
|
||||
memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields));
|
||||
airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed;
|
||||
airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed;
|
||||
airspeedActualData.alpha=out.angleOfAttack;
|
||||
airspeedActualData.beta=out.angleOfSlip;
|
||||
airspeedActual->setData(airspeedActualData);
|
||||
|
||||
airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate);
|
||||
@ -757,3 +766,68 @@ void Simulator::updateUAVOs(Output2Hardware out){
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* calculate air density from altitude. http://en.wikipedia.org/wiki/Density_of_air
|
||||
*/
|
||||
float Simulator::airDensityFromAltitude(float alt, AirParameters air, float gravity) {
|
||||
float p= airPressureFromAltitude(alt, air, gravity);
|
||||
float rho=p*air.M/(air.univGasConstant*(air.groundTemp-air.tempLapseRate*alt));
|
||||
|
||||
return rho;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Simulator::airPressureFromAltitude Get air pressure from altitude and atmospheric conditions.
|
||||
* @param alt altitude
|
||||
* @param air atmospheric conditions
|
||||
* @param gravity
|
||||
* @return
|
||||
*/
|
||||
float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gravity) {
|
||||
return air.seaLevelPress* pow(1 - air.tempLapseRate * alt /air.groundTemp, gravity * air.M/(air.univGasConstant*air.tempLapseRate));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Simulator::cas2tas calculate TAS from CAS and altitude. http://en.wikipedia.org/wiki/Airspeed
|
||||
* @param CAS Calibrated airspeed
|
||||
* @param alt altitude
|
||||
* @param air atmospheric conditions
|
||||
* @param gravity
|
||||
* @return True airspeed
|
||||
*/
|
||||
float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity) {
|
||||
float rho=airDensityFromAltitude(alt, air, gravity);
|
||||
|
||||
return (CAS * sqrt(air.groundDensity/rho));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Simulator::tas2cas calculate CAS from TAS and altitude. http://en.wikipedia.org/wiki/Airspeed
|
||||
* @param TAS True airspeed
|
||||
* @param alt altitude
|
||||
* @param air atmospheric conditions
|
||||
* @param gravity
|
||||
* @return Calibrated airspeed
|
||||
*/
|
||||
float Simulator::tas2cas(float TAS, float alt, AirParameters air, float gravity) {
|
||||
float rho=airDensityFromAltitude(alt, air, gravity);
|
||||
|
||||
return (TAS / sqrt(air.groundDensity/rho));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Simulator::getAirParameters get air parameters
|
||||
* @return airParameters
|
||||
*/
|
||||
AirParameters Simulator::getAirParameters(){
|
||||
return airParameters;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Simulator::setAirParameters set air parameters
|
||||
* @param airParameters
|
||||
*/
|
||||
void Simulator::setAirParameters(AirParameters airParameters){
|
||||
this->airParameters=airParameters;
|
||||
}
|
||||
|
||||
@ -41,9 +41,9 @@
|
||||
#include "accels.h"
|
||||
#include "actuatorcommand.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "airspeedactual.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudesettings.h"
|
||||
#include "airspeedactual.h"
|
||||
#include "baroaltitude.h"
|
||||
#include "flightstatus.h"
|
||||
#include "gcsreceiver.h"
|
||||
@ -70,10 +70,9 @@ typedef struct _FLIGHT_PARAM {
|
||||
float dT;
|
||||
unsigned int i;
|
||||
|
||||
// speed (relative)
|
||||
float ias;
|
||||
float cas;
|
||||
float tas;
|
||||
// speeds
|
||||
float cas; //Calibrated airspeed
|
||||
float tas; //True airspeed
|
||||
float groundspeed;
|
||||
|
||||
// position (absolute)
|
||||
@ -103,6 +102,18 @@ typedef struct _FLIGHT_PARAM {
|
||||
|
||||
} FLIGHT_PARAM;
|
||||
|
||||
struct AirParameters
|
||||
{
|
||||
float groundDensity;
|
||||
float groundTemp;
|
||||
float seaLevelPress;
|
||||
float tempLapseRate;
|
||||
float univGasConstant;
|
||||
float dryAirConstant;
|
||||
float relativeHumidity; //[%]
|
||||
float M; //Molar mass
|
||||
};
|
||||
|
||||
typedef struct _CONNECTION
|
||||
{
|
||||
QString simulatorId;
|
||||
@ -213,6 +224,10 @@ public:
|
||||
QString SimulatorId() const { return simulatorId; }
|
||||
void setSimulatorId(QString str) { simulatorId = str; }
|
||||
|
||||
float airDensityFromAltitude(float alt, AirParameters air, float gravity);
|
||||
float airPressureFromAltitude(float alt, AirParameters air, float gravity);
|
||||
float cas2tas(float CAS, float alt, AirParameters air, float gravity);
|
||||
float tas2cas(float TAS, float alt, AirParameters air, float gravity);
|
||||
|
||||
|
||||
static bool IsStarted() { return isStarted; }
|
||||
@ -226,6 +241,9 @@ public:
|
||||
void resetInitialHomePosition();
|
||||
void updateUAVOs(Output2Hardware out);
|
||||
|
||||
AirParameters getAirParameters();
|
||||
void setAirParameters(AirParameters airParameters);
|
||||
|
||||
signals:
|
||||
void autopilotConnected();
|
||||
void autopilotDisconnected();
|
||||
@ -317,6 +335,8 @@ private:
|
||||
void setupInputObject(UAVObject* obj, quint32 updatePeriod);
|
||||
void setupWatchedObject(UAVObject *obj, quint32 updatePeriod);
|
||||
void setupObjects();
|
||||
|
||||
AirParameters airParameters;
|
||||
};
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user