protected override float process_desired_v(float des_v, bool user_input)
{
float rad_max_aoa = max_aoa * dgr2rad;
res_max_aoa = 100.0f;
res_min_aoa = -100.0f;
res_equilibr_v_upper = 0.0f;
res_equilibr_v_lower = 0.0f;
float cur_aoa = imodel.AoA(axis);
float abs_cur_aoa = Math.Abs(cur_aoa);
bool moderated = false;
// AoA moderation section
if (moderate_aoa && imodel.dyn_pressure > moder_cutoff_ias * moder_cutoff_ias)
{
moderated = true;
if (abs_cur_aoa < rad_max_aoa * 1.5f)
{
// We're in linear regime so we can update our limitations
// get equilibrium aoa and angular_v for 1.0 input
try
{
eq_A[0, 0] = lin_model.A[0, 0];
eq_A[0, 1] = lin_model.A[0, 1];
eq_A[1, 0] = lin_model.A[1, 0];
eq_A[1, 1] = 0.0;
eq_b[0, 0] = -(lin_model.A[0, 2] + lin_model.A[0, 3] + lin_model.B[0, 0] + lin_model.C[0, 0]);
eq_b[1, 0] = -(lin_model.A[1, 2] + lin_model.A[1, 3] + lin_model.B[1, 0] + lin_model.C[1, 0]);
eq_A.old_lu = true;
eq_x = eq_A.SolveWith(eq_b);
if (!double.IsInfinity(eq_x[0, 0]) && !double.IsNaN(eq_x[0, 0]))
{
if (eq_x[0, 0] < 0.0)
{
// plane is statically unstable, eq_x solution is equilibrium on it's minimal stable aoa
min_input_aoa = (float)Common.simple_filter(0.6 * eq_x[0, 0], min_input_aoa, moder_filter / 2.0);
min_input_v = (float)Common.simple_filter(0.6 * eq_x[1, 0], min_input_v, moder_filter / 2.0);
staticaly_stable = false;
}
else
{
// plane is statically stable, eq_x solution is equilibrium on it's maximal stable aoa
max_input_aoa = (float)Common.simple_filter(eq_x[0, 0], max_input_aoa, moder_filter);
max_input_v = (float)Common.simple_filter(eq_x[1, 0], max_input_v, moder_filter);
staticaly_stable = true;
}
// get equilibrium aoa and angular_v for -1.0 input
eq_b[0, 0] = -lin_model.C[0, 0] + lin_model.A[0, 2] + lin_model.A[0, 3] + lin_model.B[0, 0];
eq_b[1, 0] = lin_model.A[1, 2] + lin_model.A[1, 3] + lin_model.B[1, 0] - lin_model.C[1, 0];
eq_x = eq_A.SolveWith(eq_b);
if (!double.IsInfinity(eq_x[0, 0]) && !double.IsNaN(eq_x[0, 0]))
{
if (eq_x[0, 0] >= 0.0)
{
// plane is statically unstable, eq_x solution is equilibrium on it's maximal stable aoa
max_input_aoa = (float)Common.simple_filter(0.6 * eq_x[0, 0], max_input_aoa, moder_filter / 2.0);
max_input_v = (float)Common.simple_filter(0.6 * eq_x[1, 0], max_input_v, moder_filter / 2.0);
}
else
{
// plane is statically stable, eq_x solution is equilibrium on it's minimal stable aoa
min_input_aoa = (float)Common.simple_filter(eq_x[0, 0], min_input_aoa, moder_filter);
min_input_v = (float)Common.simple_filter(eq_x[1, 0], min_input_v, moder_filter);
}
}
}
}
catch (MSingularException) { }
// get equilibrium v for max_aoa
eq_A[0, 0] = lin_model.A[0, 1];
eq_A[0, 1] = lin_model.A[0, 2] + lin_model.A[0, 3] + lin_model.B[0, 0];
eq_A[1, 0] = lin_model.A[1, 1];
eq_A[1, 1] = lin_model.A[1, 2] + lin_model.A[1, 3] + lin_model.B[1, 0];
eq_b[0, 0] = -(lin_model.A[0, 0] * rad_max_aoa + lin_model.C[0, 0]);
eq_b[1, 0] = -(lin_model.A[1, 0] * rad_max_aoa + lin_model.C[1, 0]);
eq_A.old_lu = true;
try
{
eq_x = eq_A.SolveWith(eq_b);
double new_max_aoa_v = eq_x[0, 0];
eq_b[0, 0] = -(lin_model.A[0, 0] * -rad_max_aoa + lin_model.C[0, 0]);
eq_b[1, 0] = -(lin_model.A[1, 0] * -rad_max_aoa + lin_model.C[1, 0]);
eq_x = eq_A.SolveWith(eq_b);
double new_min_aoa_v = eq_x[0, 0];
if (!double.IsInfinity(new_max_aoa_v) && !double.IsNaN(new_max_aoa_v) &&
!double.IsInfinity(new_min_aoa_v) && !double.IsNaN(new_min_aoa_v))
{
max_aoa_v = (float)Common.simple_filter(new_max_aoa_v, max_aoa_v, moder_filter);
min_aoa_v = (float)Common.simple_filter(new_min_aoa_v, min_aoa_v, moder_filter);
}
}
catch (MSingularException) { }
}
// let's apply moderation with controllability region
if (max_input_aoa < res_max_aoa)
{
res_max_aoa = max_input_aoa;
res_equilibr_v_upper = max_input_v;
}
if (min_input_aoa > res_min_aoa)
{
res_min_aoa = min_input_aoa;
res_equilibr_v_lower = min_input_v;
}
// apply simple AoA moderation
if (rad_max_aoa < res_max_aoa)
{
res_max_aoa = rad_max_aoa;
res_equilibr_v_upper = max_aoa_v;
}
if (-rad_max_aoa > res_min_aoa)
{
res_min_aoa = -rad_max_aoa;
res_equilibr_v_lower = min_aoa_v;
}
}
// Lift acceleration moderation section
if (moderate_g && imodel.dyn_pressure > moder_cutoff_ias * moder_cutoff_ias)
{
moderated = true;
if (Math.Abs(lin_model.A[0, 0]) > 1e-5 && abs_cur_aoa < rad_max_aoa * 1.5f)
{
// model may be sane, let's update limitations
double gravity_acc = 0.0;
switch (axis)
{
case PITCH:
gravity_acc = imodel.pitch_gravity_acc + imodel.pitch_noninert_acc;
break;
case YAW:
gravity_acc = imodel.yaw_gravity_acc + imodel.yaw_noninert_acc;
break;
default:
gravity_acc = 0.0;
break;
}
// get equilibrium aoa and angular v for max_g g-force
max_g_v = (float)Common.simple_filter(
(max_g_force * 9.81 + gravity_acc) / imodel.surface_v_magnitude,
max_g_v, moder_filter);
min_g_v = (float)Common.simple_filter(
(-max_g_force * 9.81 + gravity_acc) / imodel.surface_v_magnitude,
min_g_v, moder_filter);
// get equilibrium aoa for max_g
eq_A[0, 0] = lin_model.A[0, 0];
eq_A[0, 1] = lin_model.A[0, 2] + lin_model.A[0, 3] + lin_model.B[0, 0];
eq_A[1, 0] = lin_model.A[1, 0];
eq_A[1, 1] = lin_model.A[1, 2] + lin_model.A[1, 3] + lin_model.B[1, 0];
eq_b[0, 0] = -(max_g_v + lin_model.C[0, 0]);
eq_b[1, 0] = -lin_model.C[1, 0];
eq_A.old_lu = true;
try
{
eq_x = eq_A.SolveWith(eq_b);
double new_max_g_aoa = eq_x[0, 0];
eq_b[0, 0] = -(min_g_v + lin_model.C[0, 0]);
eq_x = eq_A.SolveWith(eq_b);
double new_min_g_aoa = eq_x[0, 0];
if (!double.IsInfinity(new_max_g_aoa) && !double.IsNaN(new_max_g_aoa) &&
!double.IsInfinity(new_min_g_aoa) && !double.IsNaN(new_min_g_aoa))
{
max_g_aoa = (float)Common.simple_filter(new_max_g_aoa, max_g_aoa, moder_filter);
min_g_aoa = (float)Common.simple_filter(new_min_g_aoa, min_g_aoa, moder_filter);
}
}
catch (MSingularException) { }
}
// apply moderation
if (max_g_aoa < 2.0 && max_g_aoa > 0.0 && min_g_aoa > -2.0 && max_g_aoa > min_g_aoa) // sanity check
{
if (max_g_aoa < res_max_aoa)
{
res_max_aoa = max_g_aoa;
res_equilibr_v_upper = max_g_v;
}
if (min_g_aoa > res_min_aoa)
{
res_min_aoa = min_g_aoa;
res_equilibr_v_lower = min_g_v;
}
}
}
// let's get non-overshooting max v value, let's call it transit_max_v
// we start on 0.0 aoa with transit_max_v and we must not overshoot res_max_aoa
// while applying -1.0 input all the time
if (abs_cur_aoa < rad_max_aoa * 1.5f && moderated)
{
double transit_max_aoa = Math.Min(rad_max_aoa, res_max_aoa);
state_mat[0, 0] = staticaly_stable ? transit_max_aoa / 3.0 : transit_max_aoa;
state_mat[2, 0] = -1.0;
state_mat[3, 0] = -1.0;
input_mat[0, 0] = -1.0;
double acc = lin_model.eval_row(1, state_mat, input_mat);
float new_dyn_max_v = transit_v_mult * (float)Math.Sqrt(2.0 * transit_max_aoa * (-acc));
if (float.IsNaN(new_dyn_max_v))
{
if (old_dyn_max_v != 0.0f)
{
transit_max_v = old_dyn_max_v;
}
else
{
old_dyn_max_v = max_v_construction;
}
}
else
{
// for cases when static authority is too small to comply to long-term dynamics,
// we need to artificially increase it
if (new_dyn_max_v < res_equilibr_v_upper * 1.2 || new_dyn_max_v < -res_equilibr_v_lower * 1.2)
{
new_dyn_max_v = 1.2f * Math.Max(Math.Abs(res_equilibr_v_upper), Math.Abs(res_equilibr_v_lower));
}
new_dyn_max_v = Common.Clampf(new_dyn_max_v, max_v_construction);
transit_max_v = (float)Common.simple_filter(new_dyn_max_v, transit_max_v, moder_filter);
old_dyn_max_v = transit_max_v;
}
}
else
{
transit_max_v = max_v_construction;
}
// if the user is in charge, let's hold surface-relative angular elocity
float v_offset = 0.0f;
if (user_input && vessel.obt_speed > 1.0)
{
if (FlightGlobals.speedDisplayMode == FlightGlobals.SpeedDisplayModes.Surface)
{
Vector3 planet2vessel = vessel.GetWorldPos3D() - vessel.mainBody.position;
Vector3 still_ang_v = Vector3.Cross(vessel.obt_velocity, planet2vessel) / planet2vessel.sqrMagnitude;
Vector3 principal_still_ang_v = imodel.world_to_cntrl_part * still_ang_v;
v_offset = principal_still_ang_v[axis];
}
}
if (user_input)
{
v_offset += neutral_offset;
}
// desired_v moderation section
if (user_controlled)
{
float normalized_des_v = des_v / max_v_construction;
if (float.IsInfinity(normalized_des_v) || float.IsNaN(normalized_des_v))
{
normalized_des_v = 0.0f;
}
normalized_des_v = Common.Clampf(normalized_des_v, 1.0f);
if (moderated)
{
float max_v = Mathf.Min(max_v_construction, transit_max_v);
float min_v = -max_v;
// upper aoa limit moderation
scaled_aoa_up = Common.Clampf((res_max_aoa - cur_aoa) * 2.0f / (res_max_aoa - res_min_aoa), 1.0f);
if (scaled_aoa_up < 0.0f)
{
max_v = Mathf.Min(max_v, scaled_aoa_up * max_v + (1.0f + scaled_aoa_up) * Math.Min(res_equilibr_v_upper, max_v));
}
else
{
max_v = Mathf.Min(max_v, scaled_aoa_up * max_v + (1.0f - scaled_aoa_up) * Math.Min(res_equilibr_v_upper, max_v));
}
// lower aoa limit moderation
scaled_aoa_down = Common.Clampf((res_min_aoa - cur_aoa) * 2.0f / (res_min_aoa - res_max_aoa), 1.0f);
if (scaled_aoa_down < 0.0f)
{
min_v = Mathf.Max(min_v, scaled_aoa_down * min_v + (1.0f + scaled_aoa_down) * Math.Max(res_equilibr_v_lower, min_v));
}
else
{
min_v = Mathf.Max(min_v, scaled_aoa_down * min_v + (1.0f - scaled_aoa_down) * Math.Max(res_equilibr_v_lower, min_v));
}
// now let's restrain v
scaled_restrained_v = Common.Clampf(v_offset, min_v, max_v);
scaled_restrained_v = Mathf.Lerp(scaled_restrained_v, des_v >= 0.0 ? max_v : min_v, Mathf.Abs(normalized_des_v));
}
else
{
scaled_restrained_v = transit_max_v * normalized_des_v + v_offset;
}
des_v = scaled_restrained_v;
}
return(des_v);
}
protected override float get_required_input(FlightCtrlState cntrl, float target_value)
{
float new_input = 0.0f;
if (AtmosphereAutopilot.AeroModel == AtmosphereAutopilot.AerodinamycsModel.FAR)
{
authority = lin_model.B[1, 0];
// check if we have inadequate model authority
if (authority < 1e-4)
{
float user_input = axis == PITCH ? cntrl.pitch : cntrl.yaw;
if (user_input != 0.0f)
{
return(user_input);
}
else
{
return(Common.Clampf(target_value, 1.0f));
}
}
// get model prediction for next frame
cur_state[0, 0] = imodel.AoA(axis);
cur_state[1, 0] = imodel.AngularVel(axis);
cur_state[2, 0] = imodel.ControlSurfPos(axis);
cur_state[3, 0] = imodel.GimbalPos(axis);
input_mat[0, 0] = cur_state[2, 0];
double cur_acc_prediction = lin_model.eval_row(1, cur_state, input_mat);
double acc_error = target_value - cur_acc_prediction;
new_input = (float)(imodel.ControlSurfPos(axis) + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
// Exponential blend can mess with rotation model, let's check it
if (FlightModel.far_blend_collapse(imodel.ControlSurfPos(axis), new_input))
{
// we need to recalculate new_input according to undelayed model
authority = lin_model_undelayed.B[1, 0];
new_input = (float)(imodel.ControlSurfPos(axis) + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
}
model_predicted_acc = cur_acc_prediction + authority * (new_input - cur_state[2, 0]);
}
else
{
authority = lin_model_undelayed.B[1, 0];
// check if we have inadequate model authority
if (authority < 1e-4)
{
float user_input = axis == PITCH ? cntrl.pitch : cntrl.yaw;
if (user_input != 0.0f)
{
return(user_input);
}
else
{
return(Common.Clampf(target_value, 1.0f));
}
}
// get model prediction for next frame
cur_state[0, 0] = imodel.AoA(axis);
cur_state[1, 0] = imodel.AngularVel(axis);
cur_state[2, 0] = imodel.GimbalPos(axis);
input_mat[0, 0] = imodel.ControlSurfPos(axis);
double cur_acc_prediction = lin_model_undelayed.eval_row(1, cur_state, input_mat);
double acc_error = target_value - cur_acc_prediction;
new_input = (float)(input_mat[0, 0] + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
if (Math.Abs(new_input - input_mat[0, 0]) / TimeWarp.fixedDeltaTime > SyncModuleControlSurface.CSURF_SPD)
{
// we're exceeding control surface speed
cur_state[3, 0] = cur_state[2, 0];
cur_state[2, 0] = input_mat[0, 0];
if (new_input > input_mat[0, 0])
{
cur_state[2, 0] += TimeWarp.fixedDeltaTime * SyncModuleControlSurface.CSURF_SPD;
}
else
{
cur_state[2, 0] -= TimeWarp.fixedDeltaTime * SyncModuleControlSurface.CSURF_SPD;
}
cur_state[2, 0] = Common.Clamp(cur_state[2, 0], 1.0);
input_mat[0, 0] = cur_state[2, 0];
cur_acc_prediction = lin_model.eval_row(1, cur_state, input_mat);
acc_error = target_value - cur_acc_prediction;
authority = lin_model.B[1, 0];
new_input = (float)(cur_state[2, 0] + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
}
model_predicted_acc = cur_acc_prediction + authority * (new_input - input_mat[0, 0]);
}
if (write_telemetry)
{
prediction_writer.Write(model_predicted_acc.ToString("G8") + ',');
}
return(new_input);
}