Example #1
0
/**
 * 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);
}
Example #2
0
/**
 * 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
}