/// <summary>
/// Main control function
/// </summary>
/// <param name="cntrl">Control state to change</param>
public float ApplyControl(FlightCtrlState cntrl, float target_value, float target_acc = 0.0f)
{
vel = imodel.AngularVel(axis); // get angular velocity
float user_input = ControlUtils.get_neutralized_user_input(cntrl, axis);
if (user_input != 0.0f || user_controlled)
{
// user is interfering with control
float clamp = (watch_precision_mode && FlightInputHandler.fetch.precisionMode) ?
precision_mode_factor * user_input_deriv_clamp * TimeWarp.fixedDeltaTime :
user_input_deriv_clamp * TimeWarp.fixedDeltaTime;
if (watch_precision_mode && FlightInputHandler.fetch.precisionMode)
{
user_input *= precision_mode_factor;
}
float delta_input = Common.Clampf(user_input - prev_input, clamp);
user_input = prev_input + delta_input;
prev_input = user_input;
desired_v = user_input * max_v_construction;
user_controlled = true;
}
else
{
// control from above
desired_v = Common.Clampf(target_value, max_v_construction);
}
desired_v = process_desired_v(desired_v, user_controlled); // moderation stage
output_acc = get_desired_acc(desired_v) + target_acc; // produce output acceleration
// check if we're stable on given input value
if (ApplyTrim && vessel == AtmosphereAutopilot.Instance.ActiveVessel)
{
if (Math.Abs(vel) < 0.002f)
{
time_in_regime += TimeWarp.fixedDeltaTime;
}
else
{
time_in_regime = 0.0;
}
if (time_in_regime >= 5.0)
{
ControlUtils.set_trim(axis, imodel.ControlSurfPosHistory(axis).Average());
}
}
acc_controller.ApplyControl(cntrl, output_acc);
return(output_acc);
}
double get_roll_aoa_deriv()
{
Vector3 pitch_aoa = new Vector3(imodel.AoA(PITCH), 0.0f, 0.0f);
Vector3 yaw_aoa = new Vector3(0.0f, imodel.AoA(YAW), 0.0f);
Vector3 ang_v = new Vector3(0.0f, 0.0f, imodel.AngularVel(ROLL));
Vector3 plane_vel = Vector3.Cross(ang_v, pitch_aoa + yaw_aoa);
if (axis == PITCH)
{
return(Vector3.Dot(Vector3.right, plane_vel));
}
if (axis == YAW)
{
return(Vector3.Dot(Vector3.up, plane_vel));
}
return(0.0);
}
/// <summary>
/// Main control function
/// </summary>
/// <param name="cntrl">Control state to change</param>
/// <param name="target_value">Desired angular acceleration</param>
public override float ApplyControl(FlightCtrlState cntrl, float target_value)
{
acc = (float)imodel.AngularAcc(axis);
//model_acc = (float)imodel.model_acc[axis];
desired_acc = target_value;
if (write_telemetry)
{
desire_acc_writer.Write(target_value.ToString("G8") + ',');
acc_writer.Write(acc.ToString("G8") + ',');
v_writer.Write(imodel.AngularVel(axis).ToString("G8") + ',');
//prediction_writer.Write(target_value.ToString("G8") + ',');
aoa_writer.Write(imodel.AoA(axis).ToString("G8") + ',');
airspd_writer.Write((imodel.up_srf_v + imodel.fwd_srf_v).magnitude.ToString("G8") + ',');
density_writer.Write(vessel.atmDensity.ToString("G8") + ',');
}
float cur_input_raw = get_required_input(cntrl, desired_acc);
output = cur_input_raw;
if (float.IsNaN(output) || float.IsInfinity(output))
{
output = 0.0f;
}
// fighting numerical precision issues
//if (axis == ROLL && imodel.dyn_pressure > 1000.0 && Mathf.Abs(output) < 0.006f)
// output = 0.0f;
ControlUtils.set_raw_output(cntrl, axis, output);
if (write_telemetry)
{
controlWriter.Write(csurf_output.ToString("G8") + ',');
outputWriter.Write(output.ToString("G8") + ',');
}
return(output);
}
/// <summary>
/// Main control function
/// </summary>
/// <param name="cntrl">Control state to change</param>
public override void ApplyControl(FlightCtrlState cntrl)
{
if (vessel.LandedOrSplashed)
{
landed = true;
time_after_takeoff = 0.0f;
return;
}
// disable pitch moderation for two seconds after take-off
if (landed || need_restore)
{
if (landed && !need_restore)
{
aoa_moder = pc.moderate_aoa;
g_moder = pc.moderate_g;
pc.moderate_aoa = false;
pc.moderate_g = false;
landed = false;
need_restore = true;
}
if (time_after_takeoff > 1.5f)
{
pc.moderate_aoa = aoa_moder;
pc.moderate_g = g_moder;
need_restore = false;
}
else
{
time_after_takeoff += TimeWarp.fixedDeltaTime;
}
}
if (tc.spd_control_enabled)
{
tc.ApplyControl(cntrl, tc.setpoint.mps());
}
pc.user_controlled = true;
if (coord_turn)
{
// account for yaw velocity in pitch neutral offset to assist coordinated turn
Vector3 up_level_dir = Vector3.ProjectOnPlane(vessel.ReferenceTransform.position - vessel.mainBody.position,
vessel.ReferenceTransform.up).normalized;
float yaw_v_vert_project = Vector3.Dot(im.AngularVel(YAW) * vessel.ReferenceTransform.right, up_level_dir);
float pitch_vert_project = Vector3.Dot(up_level_dir, -vessel.ReferenceTransform.forward);
if (pitch_vert_project > 0.0f)
{
float level_pitch_vel = -yaw_v_vert_project / pitch_vert_project;
pc.neutral_offset = level_pitch_vel;
}
else
{
pc.neutral_offset = 0.0f;
}
}
else
{
pc.neutral_offset = 0.0f;
}
pc.ApplyControl(cntrl, 0.0f);
if (rocket_mode)
{
yvc.user_controlled = true;
yvc.ApplyControl(cntrl, 0.0f);
}
else
{
yc.user_controlled = true;
yc.ApplyControl(cntrl, 0.0f, 0.0f);
}
rc.user_controlled = true;
rc.ApplyControl(cntrl, 0.0f);
}
