/// <summary>
/// Main control function
/// </summary>
/// <param name="cntrl">Control state to change</param>
public override void ApplyControl(FlightCtrlState cntrl)
{
if (vessel.LandedOrSplashed)
{
return;
}
ac.ApplyControl(cntrl, 0.0f, 0.0f);
yc.ApplyControl(cntrl, 0.0f, 0.0f);
rc.ApplyControl(cntrl, 0.0f);
}
/// <summary>
/// Main control function
/// </summary>
/// <param name="desired_vel">Desired velocity direction in surface reference frame.</param>
/// <param name="desired_acceleration">Desired acceleration.</param>
public void ApplyControl(FlightCtrlState state, Vector3d desired_vel, Vector3d desired_acceleration)
{
Vector3d planet2ves = vessel.ReferenceTransform.position - vessel.mainBody.position;
Vector3d planet2vesNorm = planet2ves.normalized;
Vector3d shift_acc = Vector3d.zero;
// centrifugal acceleration to stay on desired altitude
//Vector3d level_acc = -planet2vesNorm * (imodel.surface_v - Vector3d.Project(imodel.surface_v, planet2vesNorm)).sqrMagnitude / planet2ves.magnitude;
// Rotation vector
Vector3d desired_turn_acc_dir = Vector3d.Cross(
Vector3d.Cross(imodel.surface_v, desired_vel).normalized,
imodel.surface_v).normalized;
angular_error = Vector3d.Angle(imodel.surface_v.normalized, desired_vel) * dgr2rad;
max_lift_acc = Math.Max(0.01, max_lift_acceleration(PITCH));
max_sideslip_acc = Math.Max(0.001, max_lift_acceleration(YAW));
// let's find this craft's maximum acceleration toward desired_turn_acc_dir without stalling
// it can be solved from simple quadratic equation
Vector3d max_turn_acc = non_stall_turn_acceleration(desired_turn_acc_dir, max_lift_acc);
max_turn_acc = strength * max_turn_acc * ((vessel == FlightGlobals.ActiveVessel && FlightInputHandler.fetch.precisionMode) ? 0.4 : 1.0);
// now let's take roll speed and relaxation into account
double max_angular_v = max_turn_acc.magnitude / imodel.surface_v_magnitude;
double t1 = max_roll_v / roll_acc_factor;
double t2 = (90.0 * dgr2rad - t1 * max_roll_v) / max_roll_v;
double stop_time_roll = roll_stop_k * (2.0 * t1 + t2);
if (double.IsNaN(stop_time_roll) || double.IsInfinity(stop_time_roll))
{
stop_time_roll = 2.0;
}
if (stop_time_roll <= 0.0)
{
stop_time_roll = 0.5;
}
// now let's generate desired acceleration
if (angular_error / max_angular_v > stop_time_roll)
{
// we're far away from relaxation, let's simply produce maximum acceleration
shift_acc = max_turn_acc;
}
else
{
// we're relaxing now, quadratic descend is good approximation
{
double tk = (angular_error / max_angular_v) / stop_time_roll;
if (Math.Abs(angular_error) < relaxation_margin)
{
tk *= Mathf.Lerp(1.0f, angle_relaxation_k, (float)(1.0f - Math.Abs(angular_error) / relaxation_margin));
}
shift_acc = max_turn_acc * tk;
}
}
//if (angular_error > 0.2 || Vector3d.Dot(desired_acceleration, shift_acc) < -0.1)
// target_acc = shift_acc;
//else
target_acc = desired_acceleration + shift_acc;
//current_acc = imodel.sum_acc;
// we need aoa moderation for AoA controllers to work
pitch_c.moderate_aoa = true;
yaw_c.moderate_aoa = true;
Vector3d neutral_acc = -imodel.gravity_acc - imodel.noninert_acc;
Vector3d target_lift_acc = target_acc + neutral_acc;
Vector3d target_normal_lift_acc = target_lift_acc - Vector3d.Project(target_lift_acc, imodel.surface_v);
// prevent rolling on small errors
if (angular_error < 2e-2 && target_normal_lift_acc.magnitude < neutral_acc.magnitude * 0.3)
{
target_lift_acc = Vector3d.Project(target_lift_acc, neutral_acc);
target_normal_lift_acc = target_lift_acc - Vector3d.Project(target_lift_acc, imodel.surface_v);
}
Vector3d desired_right_direction = Vector3d.Cross(target_normal_lift_acc, vessel.ReferenceTransform.up).normalized;
// let's apply roll to maintain desired_right_direction
Vector3 right_vector = imodel.virtualRotation * Vector3.right;
double new_roll_error = Math.Sign(Vector3d.Dot(right_vector, target_normal_lift_acc)) *
Math.Acos(Math.Min(Math.Max(Vector3d.Dot(desired_right_direction, right_vector), -1.0), 1.0));
// rolling to pitch up is not always as efficient as pitching down
double spine_to_zenith = Vector3d.Dot(desired_right_direction, Vector3d.Cross(imodel.surface_v, imodel.gravity_acc));
if (target_normal_lift_acc.magnitude < max_neg_g * 9.81 || !allow_spine_down || vessel.heightFromTerrain < min_rollover_alt)
{
if (Math.Abs(new_roll_error) > 90.0 * dgr2rad && spine_to_zenith < 0.0)
{
new_roll_error = new_roll_error - 180.0 * dgr2rad * Math.Sign(new_roll_error);
}
}
// filter it
if ((Math.Abs(new_roll_error) < roll_error_filter_margin * dgr2rad) && (Math.Abs(roll_error) < roll_error_filter_margin * dgr2rad))
{
roll_error = Common.simple_filter(new_roll_error, roll_error, roll_error_filter_k);
}
else
{
roll_error = new_roll_error;
}
// generate desired roll angular_v
roll_c.user_controlled = false;
roll_c.ApplyControl(state, get_desired_roll_v(roll_error));
// now let's apply pitch and yaw AoA controls
// pitch AoA
desired_pitch_lift = 0.0;
if (Math.Abs(roll_error) < 30.0 * dgr2rad || Math.Abs(roll_error - 180.0) < 30.0 * dgr2rad)
{
desired_pitch_lift = Vector3.Dot(imodel.pitch_tangent, target_normal_lift_acc);
}
else
{
desired_pitch_lift = Vector3.Dot(imodel.pitch_tangent, neutral_acc);
}
desired_pitch_acc = desired_pitch_lift + imodel.pitch_gravity_acc + imodel.pitch_noninert_acc;
desired_pitch_v = desired_pitch_acc / imodel.surface_v_magnitude;
// let's find equilibrium AoA for desired lift
desired_aoa = get_desired_aoa(imodel.pitch_rot_model_gen, desired_pitch_v, 0.0);
if (float.IsNaN(desired_aoa) || float.IsInfinity(desired_aoa))
{
desired_aoa = 0.0f;
}
aoa_c.user_controlled = false;
aoa_c.ApplyControl(state, desired_aoa, 0.0f);
// yaw sideslip
//if (Math.Abs(roll_angle) > 3.0 * dgr2rad)
//{
// desired_yaw_lift = 0.0;
// desired_sideslip = 0.0f;
//}
//else
//{
desired_yaw_lift = 0.0; // Vector3.Dot(imodel.yaw_tangent, normal_lift_acc);
desired_yaw_acc = desired_yaw_lift + imodel.yaw_gravity_acc + imodel.yaw_noninert_acc;
desired_yaw_v = desired_yaw_acc / imodel.surface_v_magnitude;
// let's find equilibrium sideslip for desired lift
//if (Math.Abs(desired_yaw_lift) < 0.01f)
desired_sideslip = (float)Common.simple_filter(get_desired_aoa(imodel.yaw_rot_model_gen, desired_yaw_v, 0.0), desired_sideslip, sideslip_filter_k);
if (float.IsNaN(desired_sideslip) || float.IsInfinity(desired_sideslip) || Math.Abs(desired_sideslip) < 0.001f)
{
desired_sideslip = 0.0f;
}
desired_sideslip = 0.0f;
//}
//desired_sideslip = 0.0f;
side_c.user_controlled = false;
side_c.ApplyControl(state, desired_sideslip, 0.0f);
}