protected virtual float get_required_input(FlightCtrlState cntrl, float target_value)
{
return(ControlUtils.getControlFromState(cntrl, axis));
}
public override void ApplyControl(FlightCtrlState cntrl)
{
if (vessel.LandedOrSplashed())
{
return;
}
if (thrust_c.spd_control_enabled)
{
thrust_c.ApplyControl(cntrl, thrust_c.setpoint.mps());
}
desired_velocity = Vector3d.zero;
planet2ves = vessel.ReferenceTransform.position - vessel.mainBody.position;
planet2vesNorm = planet2ves.normalized;
desired_vert_acc = Vector3d.zero;
// centrifugal acceleration to stay on desired altitude
level_acc = -planet2vesNorm * (imodel.surface_v - Vector3d.Project(imodel.surface_v, planet2vesNorm)).sqrMagnitude / planet2ves.magnitude;
switch (current_mode)
{
default:
case CruiseMode.LevelFlight:
// simply select velocity from axis
desired_velocity = Vector3d.Cross(planet2vesNorm, circle_axis);
handle_wide_turn();
if (vertical_control)
{
if (height_mode == HeightMode.Altitude)
{
desired_velocity = account_for_height(desired_velocity);
}
else
{
desired_velocity = account_for_vertical_vel(desired_velocity);
}
}
break;
case CruiseMode.CourseHold:
if (Math.Abs(vessel.latitude) > 80.0)
{
// we're too close to poles, let's switch to level flight
LevelFlightMode = true;
goto case CruiseMode.LevelFlight;
}
// get direction vector form course
Vector3d north = vessel.mainBody.RotationAxis;
Vector3d north_projected = Vector3.ProjectOnPlane(north, planet2vesNorm);
QuaternionD rotation = QuaternionD.AngleAxis(desired_course, planet2vesNorm);
desired_velocity = rotation * north_projected;
handle_wide_turn();
if (vertical_control)
{
if (height_mode == HeightMode.Altitude)
{
desired_velocity = account_for_height(desired_velocity);
}
else
{
desired_velocity = account_for_vertical_vel(desired_velocity);
}
}
break;
case CruiseMode.Waypoint:
// set new axis
Vector3d world_target_pos = vessel.mainBody.GetWorldSurfacePosition(desired_latitude, desired_longitude, vessel.altitude);
dist_to_dest = Vector3d.Distance(world_target_pos, vessel.ReferenceTransform.position);
if (dist_to_dest > 10000.0)
{
double radius = vessel.mainBody.Radius;
dist_to_dest = Math.Acos(1 - (dist_to_dest * dist_to_dest) / (2 * radius * radius)) * radius;
}
if (dist_to_dest < 200.0)
{
// we're too close to target, let's switch to level flight
LevelFlightMode = true;
picking_waypoint = false;
MessageManager.post_quick_message("Waypoint reached");
goto case CruiseMode.LevelFlight;
}
// set new axis according to waypoint
circle_axis = Vector3d.Cross(world_target_pos - vessel.mainBody.position, vessel.GetWorldPos3D() - vessel.mainBody.position).normalized;
goto case CruiseMode.LevelFlight;
}
if (use_keys)
{
ControlUtils.neutralize_user_input(cntrl, PITCH);
ControlUtils.neutralize_user_input(cntrl, YAW);
}
double old_str = dir_c.strength;
dir_c.strength *= strength_mult;
dir_c.ApplyControl(cntrl, desired_velocity, level_acc + desired_vert_acc);
dir_c.strength = old_str;
}
/// <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);
}
