public LinearSystemModel(LinearSystemModel original)
{
state_count = original.state_count;
input_count = original.input_count;
A = original.A.Duplicate();
B = original.B.Duplicate();
C = original.C.Duplicate();
Ax = Bu = AxBu = null;
}
/// <summary>
/// Main control function
/// </summary>
/// <param name="cntrl">Control state to change</param>
/// <param name="target_value">Desired AoA in radians</param>
/// <param name="target_derivative">Desired AoA derivative</param>
public float ApplyControl(FlightCtrlState cntrl, float target_value, float target_derivative)
{
if (imodel.dyn_pressure <= (v_controller.moder_cutoff_ias * v_controller.moder_cutoff_ias) ||
!v_controller.moderate_aoa)
{
v_controller.user_controlled = true;
v_controller.ApplyControl(cntrl, 0.0f);
return(0.0f);
}
cur_aoa = imodel.AoA(axis);
float user_input = Common.Clampf(ControlUtils.get_neutralized_user_input(cntrl, axis), 1.0f);
if (user_controlled || user_input != 0.0f)
{
if (user_input >= 0.0f)
{
desired_aoa = user_input * v_controller.res_max_aoa;
}
else
{
desired_aoa = -user_input * v_controller.res_min_aoa;
}
user_controlled = true;
}
else
{
desired_aoa = (float)Common.Clamp(target_value, v_controller.res_min_aoa, v_controller.res_max_aoa);
}
// Let's find equilibrium angular v on desired_aoa
LinearSystemModel model = lin_model_gen;
eq_A[0, 0] = model.A[0, 1];
eq_A[0, 1] = model.A[0, 2] + model.A[0, 3] + model.B[0, 0];
eq_A[1, 0] = model.A[1, 1];
eq_A[1, 1] = model.A[1, 2] + model.B[1, 0] + model.A[1, 3];
eq_b[0, 0] = target_derivative - (model.A[0, 0] * desired_aoa + model.C[0, 0]);
eq_b[1, 0] = -(model.A[1, 0] * desired_aoa + model.C[1, 0]);
eq_A.old_lu = true;
try
{
eq_x = eq_A.SolveWith(eq_b);
double new_eq_v = eq_x[0, 0];
if (!double.IsInfinity(new_eq_v) && !double.IsNaN(new_eq_v))
{
desired_aoa_equilibr_v = (float)Common.simple_filter(new_eq_v, desired_aoa_equilibr_v, v_filter_k);
}
}
catch (MSingularException) { }
//cur_aoa_equilibr_v += 0.5f * (float)get_roll_aoa_deriv();
// parabolic descend to desired angle of attack
double error = Common.Clampf(desired_aoa - cur_aoa, Mathf.Abs(v_controller.res_max_aoa - v_controller.res_min_aoa));
// special workaround for twitches on out of controllable regions for overdamped planes
bool stable_out_of_bounds = false;
if (v_controller.staticaly_stable &&
((desired_aoa == v_controller.max_input_aoa && error < 0.0) ||
(desired_aoa == v_controller.min_input_aoa && error > 0.0)))
{
stable_out_of_bounds = true;
}
double k = v_controller.transit_max_v * v_controller.transit_max_v / 2.0 / (v_controller.res_max_aoa - v_controller.res_min_aoa);
double t = -Math.Sqrt(Math.Abs(error / k));
double descend_v;
if (t < -cubic_barrier)
{
// we're still far away from desired aoa, we'll descend using parabolic function
cubic = false;
double t_step = Math.Min(0.0, t + TimeWarp.fixedDeltaTime);
double relaxation = 1.0;
if (t >= -relaxation_frame * TimeWarp.fixedDeltaTime)
{
relaxation = relaxation_factor;
}
descend_v = relaxation * k * (t * t - t_step * t_step) * Math.Sign(error) / TimeWarp.fixedDeltaTime;
output_acc = 0.0f;
}
else
{
// we're close to desired aoa, we'll descend using cubic function
cubic = true;
double kacc_quadr = Math.Abs(v_controller.kacc_quadr);
double k_cubic = kacc_quadr / 6.0 * cubic_kp;
double t_cubic = -Math.Pow(Math.Abs(error / k_cubic), 0.33);
double t_step = Math.Min(0.0, t_cubic + TimeWarp.fixedDeltaTime);
if (t >= -relaxation_frame * TimeWarp.fixedDeltaTime)
{
descend_v = relaxation_factor * error / Math.Max((relaxation_frame * TimeWarp.fixedDeltaTime), TimeWarp.fixedDeltaTime);
}
else
{
descend_v = k_cubic * (t_step * t_step * t_step - t_cubic * t_cubic * t_cubic) * Math.Sign(error) / TimeWarp.fixedDeltaTime;
}
}
if (stable_out_of_bounds)
{
descend_v = -0.2 * descend_v;
}
output_v = (float)(descend_v + desired_aoa_equilibr_v);
ControlUtils.neutralize_user_input(cntrl, axis);
v_controller.user_controlled = false;
v_controller.ApplyControl(cntrl, output_v);
return(output_v);
}