protected void tune_steering()
{
VSL.Controls.GimbalLimit = 0;
//calculate attitude error and Aligned state
Ef = Utils.Clamp(VSL.Controls.AttitudeError / 180, ATCB.MinEf, 1);
//tune lowpass filter
AAf_filter.Tau = (1 - Mathf.Sqrt(Ef)) * ATCB.AALowPassF;
//tune PID parameters
angularV = VSL.vessel.angularVelocity;
angularM = Vector3.Scale(angularV, VSL.Physics.MoI);
var minI = steering.MinI();
var AA = VSL.Torque.MaxCurrent.AA.Exclude(minI);
var AAm = Utils.ClampL(AA.MaxComponentF(), 1e-5f);
var slow = VSL.Engines.SlowTorque? 1 + VSL.Engines.TorqueResponseTime * AAm * ATCB.SlowTorqueF : 1;
AAf = AAf_filter.Update(Mathf.Clamp(1 / AAm, ATCB.MinAAf, ATCB.MaxAAf));
AAm = Utils.ClampH(AAm, ATCB.MaxAA);
PIf = AAf * Utils.ClampL(1 - Ef, 1 / ATCB.MaxEf) * ATCB.MaxEf / slow;
steering_pid.P = ATCB.PID.P * PIf;
steering_pid.I = ATCB.PID.I * PIf;
steering_pid.D = ATCB.PID.D * Utils.ClampH(Utils.ClampH(2 - Ef - AAm / ATCB.MaxAA, 1) + angularM.magnitude * ATCB.AngularMf, 1) * AAf * AAf * slow * slow;
// Log("\nsteering: {}", steering);//debug
//tune steering
var norm = AA.Inverse(0).CubeNorm();
norm[minI] = Utils.ClampH(Mathf.Abs(angularV[minI] * Mathf.Rad2Deg), 1);
steering = Vector3.Scale(steering, norm);
// Log("minI {}\nnorm: {}\nsteering*norm: {}", minI, norm, steering);//debug
//add inertia to handle constantly changing needed direction
var inertia = Vector3.Scale(angularM.Sign(),
Vector3.Scale(Vector3.Scale(angularM, angularM),
Vector3.Scale(VSL.Torque.MaxCurrent.Torque, VSL.Physics.MoI).Inverse(0)))
.ClampComponents(-Mathf.PI, Mathf.PI) /
Mathf.Lerp(ATCB.InertiaFactor, 1, VSL.Physics.MoI.magnitude * ATCB.MoIFactor);
steering = steering + inertia;
// Log("\ninertia: {}\nsteering+inertia: {}", inertia, steering);//debug
//update PID
steering_pid.Update(steering, angularV);
steering = steering_pid.Action;
steering[minI] *= norm[minI];
// Log("\npid.Act: {}", steering);//debug
correct_steering();
x_OD.Update(steering.x, TimeWarp.fixedDeltaTime);
y_OD.Update(steering.y, TimeWarp.fixedDeltaTime);
z_OD.Update(steering.z, TimeWarp.fixedDeltaTime);
// CSV(steering.x, steering.y, steering.z, x_OD.Value, y_OD.Value, z_OD.Value);//debug
steering.x *= 1 - (float)x_OD.Value;
steering.y *= 1 - (float)y_OD.Value;
steering.z *= 1 - (float)z_OD.Value;
// Log("\nx_OD:\n{}" +
// "\ny_OD:\n{}" +
// "\nz_OD:\n{}",
// x_OD, y_OD, z_OD);
}
