public float[] Update(float VaSetpoint, float Va, float altitudeSetpoint, float altitude, float T)
{
float Kerror = 0.5f * mass * (VaSetpoint * VaSetpoint - Va * Va);
float Uerror = mass * g * (altitudeSetpoint - altitude);
if (Uerror > mass * g * ALTITUDE_SATURATION)
{
Uerror = mass * g * ALTITUDE_SATURATION;
}
else if (Uerror < -mass * g * ALTITUDE_SATURATION)
{
Uerror = -mass * g * ALTITUDE_SATURATION;
}
float E = Uerror + Kerror;
float B = Uerror - Kerror;
piAirspeed.Update(E, 0.0f, T);
piAltitude.Update(B, 0.0f, T);
return(new float[] { piAirspeed.output, piAltitude.output });
}
void FixedUpdate()
{
/* Get current sample time */
float T = Time.deltaTime;
totalTime += T;
if (totalTime - timerSensors >= SAMPLE_TIME_SENSORS)
{
/* Read sensors and update state estimator */
ac.GetSensorMeasurements();
/* Filter barometer measurements */
altFilter.Update(ac.baroAltitude);
timerSensors = totalTime;
}
if (totalTime - timerStateEstimation >= SAMPLE_TIME_KALMAN)
{
float T_KF = totalTime - timerStateEstimation;
/* Update state estimate */
kf.Update(ac.gyroQ, ac.accX, ac.accZ, ac.pitotVa, T_KF);
timerStateEstimation = totalTime;
}
/* Get controller outputs */
if (totalTime - timerCtrlAirspeed >= SAMPLE_TIME_CTRL_AIRSPEED)
{
float T_CTRL_AIRSPEED = totalTime - timerCtrlAirspeed;
throttleSetting = piAirspeed.Update(airspeedSetpoint, kf.VaFilt, T_CTRL_AIRSPEED);
timerCtrlAirspeed = totalTime;
}
if (totalTime - timerCtrlAltitude >= SAMPLE_TIME_CTRL_ALTITUDE)
{
float T_CTRL_ALTITUDE = totalTime - timerCtrlAltitude;
pitchAngleSetpoint = piAltitude.Update(altitudeSetpoint, altFilter.output, T_CTRL_ALTITUDE);
timerCtrlAltitude = totalTime;
}
if (totalTime - timerCtrlPitch >= SAMPLE_TIME_CTRL_PITCH)
{
float T_CTRL_PITCH = totalTime - timerCtrlPitch;
piPitch.Update(pitchAngleSetpoint, kf.theta, T_CTRL_PITCH);
timerCtrlPitch = totalTime;
}
/* Calculate elevator deflection */
elevatorDeflection = -actElevator.Update(piPitch.output, T);
/* Calculate engine thrust */
engineThrust = engine.GetThrust(actEngine.Update(throttleSetting, T));
/* Update aircraft state vector */
ac.Update(engineThrust, elevatorDeflection, T);
/* Update 3D position and view */
trans.eulerAngles = new Vector3(-ac.acState[3] * RADTODEG, 0.0f, 0.0f);
trans.position = new Vector3(0.0f, ac.acState[5], ac.acState[4]);
camTrans.position = trans.position + camOffset;
/* Update GUI */
UpdateGUI();
/* Log data */
if (csvLogging)
{
csv.AddLine(new string[] { totalTime.ToString("F6"),
ac.acState[0].ToString("F6"), ac.acState[1].ToString("F6"), ac.acState[2].ToString("F6"), ac.acState[3].ToString("F6"), ac.acState[4].ToString("F6"), ac.acState[5].ToString("F6"),
ac.GetAirspeed(ac.acState[0], ac.acState[1]).ToString("F6"), ac.GetAngleOfAttack(ac.acState[0], ac.acState[1]).ToString("F6"),
kf.qFilt.ToString("F6"), kf.axFilt.ToString("F6"), kf.azFilt.ToString("F6"), kf.VaFilt.ToString("F6"), altFilter.output.ToString("F6"), kf.theta.ToString("F6"),
elevatorDeflection.ToString("F6"), throttleSetting.ToString("F6"),
airspeedSetpoint.ToString("F6"), pitchAngleSetpoint.ToString("F6"), altitudeSetpoint.ToString("F6") });
}
}