/**
* 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 * KM_PER_HOUR2METERS_PER_SECOND;
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
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, convertin from the current NED position
// to LLA
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];
out.longitude = LLA[1];
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 BaroAltitude 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 attActual object
out.roll = current.roll; //roll;
out.pitch = current.pitch; // pitch
out.heading = current.azimuth; // yaw
// Update VelocityActual.{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+GRAVITY)*Rbe[0][2];
out.accY = current.ddX*Rbe[1][0]
+current.ddY*Rbe[1][1]
+(current.ddZ+GRAVITY)*Rbe[1][2];
out.accZ = - (current.ddX*Rbe[2][0]
+current.ddY*Rbe[2][1]
+(current.ddZ+GRAVITY)*Rbe[2][2]);
updateUAVOs(out);
}
/**
* Module thread, should not return.
*/
static void airspeedTask(void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
BaroAirspeedData airspeedData;
AirspeedActualData airspeedActualData;
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation
portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop
float airspeedErrInt=0;
#endif
//GPS airspeed calculation variables
#ifdef GPS_AIRSPEED_PRESENT
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
gps_airspeedInitialize();
#endif
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1)
{
// Update the airspeed object
BaroAirspeedGet(&airspeedData);
#ifdef BARO_AIRSPEED_PRESENT
float airspeed_tas_baro=0;
if(airspeedSensorType != AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) {
//Fetch calibrated airspeed from sensors
baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin);
//Calculate true airspeed, not taking into account compressibility effects
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
#ifdef GPS_AIRSPEED_PRESENT
//GPS present, so use baro sensor to filter TAS
airspeed_tas_baro = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#else
//No GPS, so TAS comes only from baro sensor
airspeedData.TrueAirspeed = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#endif
}
else
#endif
{ //Have to catch the fallthrough, or else this loop will monopolize the processor!
airspeedData.BaroConnected=BAROAIRSPEED_BAROCONNECTED_FALSE;
airspeedData.SensorValue=12345;
//Likely, we have a GPS, so let's configure the fallthrough at close to GPS refresh rates
vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
}
#ifdef GPS_AIRSPEED_PRESENT
float v_air_GPS=-1.0f;
//Check if sufficient time has passed. This will depend on whether we have a pitot tube
//sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS
//as quickly as possible.
#ifdef BARO_AIRSPEED_PRESENT
float delT = TICKS2MS(lastSysTime - lastLoopTime) / 1000.0f;
lastLoopTime=lastSysTime;
if ( (TICKS2MS(lastSysTime - lastGPSTime) > 1000 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY)
&& gpsNew) {
lastGPSTime=lastSysTime;
#else
if (gpsNew) {
#endif
gpsNew=false; //Do this first
//Calculate airspeed as a function of GPS groundspeed and vehicle attitude. From "IMU Wind Estimation (Theory)", by William Premerlani
gps_airspeedGet(&v_air_GPS);
}
//Use the GPS error to correct the airspeed estimate.
if (v_air_GPS > 0) //We have valid GPS estimate...
{
airspeedData.GPSAirspeed=v_air_GPS;
#ifdef BARO_AIRSPEED_PRESENT
if(airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ //Check if there is an airspeed sensors present...
//Calculate error and error integral
float airspeedErr=v_air_GPS - airspeed_tas_baro;
airspeedErrInt+=airspeedErr * delT;
//Saturate integral component at 5 m/s
airspeedErrInt = airspeedErrInt > (5.0f / GPS_AIRSPEED_BIAS_KI) ? (5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
airspeedErrInt = airspeedErrInt < -(5.0f / GPS_AIRSPEED_BIAS_KI) ? -(5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
//There's already an airspeed sensor, so instead correct it for bias with P correction. The I correction happened earlier in the function.
airspeedData.TrueAirspeed = airspeed_tas_baro + airspeedErr * GPS_AIRSPEED_BIAS_KP;
/* Note:
This would be a good place to change the airspeed calibration, so that it matches the GPS computed values. However,
this might be a bad idea, as their are two degrees of freedom here: temperature and sensor calibration. I don't
know how to control for temperature bias.
*/
}
else
#endif
{
//...there's no airspeed sensor, so everything comes from GPS. In this
//case, filter the airspeed for smoother output
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedData.TrueAirspeed=v_air_GPS*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
//Calculate calibrated airspeed from GPS, since we're not getting it from a discrete airspeed sensor
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
airspeedData.CalibratedAirspeed = tas2cas(airspeedData.TrueAirspeed, -positionActual_Down, &air_STP);
}
}
#ifdef BARO_AIRSPEED_PRESENT
else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){
//No GPS velocity estimate this loop, so filter true airspeed data with baro airspeed
float alpha=delT/(delT + BARO_TRUEAIRSPEED_TIME_CONSTANT_S); //Low pass filter.
airspeedData.TrueAirspeed=airspeed_tas_baro*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
}
#endif
#endif
//Set the UAVO
airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed;
airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
BaroAirspeedSet(&airspeedData);
AirspeedActualSet(&airspeedActualData);
}
}
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
{
gpsNew=true;
}
#endif
#ifdef BARO_AIRSPEED_PRESENT
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin_dummy)
{
//Find out which sensor we're using.
switch (airspeedSensorType) {
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
//MPXV5004 and MPXV7002 sensors
baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
default:
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
vTaskDelayUntil(lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
break;
}
}
#endif
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSensorType=airspeedSettingsData.AirspeedSensorType;
gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms;
#if defined(PIOS_INCLUDE_MPXV7002)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
}