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") }); } }