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