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