private bool HoverThrust(double tar_altitude)
{
double vessel_up_ratio = State.Vessel.VelocityMag > 100d ?
-State.Vessel.Velocity.Norm() * State.Vessel.SurfUp :
State.Vessel.Direction * State.Vessel.SurfUp;
if (vessel_up_ratio < 0.5d)
{
Command.SetThrottle(0d);
return(false);
}
double max_thrust_up = vessel_up_ratio * State.Vessel.AvailableThrust;
double max_acc_up = Math.Max(0.01d, max_thrust_up / State.Vessel.Mass * LANDING_MAX_THROTTLE - State.Vessel.GravityMag);
double max_acc_down = State.Vessel.GravityMag * LANDING_MAX_THROTTLE;
LinearPlanner.Hover(State.Vessel.Altitude - tar_altitude,
0.3d, max_acc_up, max_acc_down,
out double tar_vel, out double tar_acc);
tar_acc += (tar_vel - State.Vessel.VelocityUp) * 2d;
double tar_throttle = Math.Clamp((tar_acc + State.Vessel.GravityMag) * State.Vessel.Mass / max_thrust_up, 0d, 1d);
Command.SetThrottle(tar_throttle);
return(false);
}
private Status AtitudeControl()
{
if (Command.DirectionVector is null)
{
return(AtitudeControlSetRPY(0d, 0d, 0d));
}
Vector3d pitch_axis = State.Vessel.Right;
Vector3d yaw_axis = -State.Vessel.Up;
Vector3d torque = State.Vessel.AvailablePosTorque;
Vector3d moi = State.Vessel.MomentOfInertia;
//Console.WriteLine("{0}\t{1}", torque.ToString("0.000"), State.Vessel.AvailableTorque.Item2.ToString("0.000"));
//atitude_controller_.LinearK = 1d;// MathLib.Lerp(2d, 1d, MathLib.InverseLerpWithClamp(0d, 0.1d, Command.Throttle));
//atitude_controller_.Kp = atitude_controller_.LinearK * 2d;
atitude_controller_.MaxPitchAngAcc = -torque.X / moi.X * pitch_axis;
atitude_controller_.MaxYawAngAcc = -torque.Z / moi.Z * yaw_axis;
atitude_controller_.Update(
Command.DirectionVector,
State.Vessel.Direction,
State.Vessel.AngularVelocity,
out double pitch,
out double yaw);
double acc = 0d, vel = 0d;
double roll_torque = torque.Y / moi.Y;
if (Command.HeadingMode == Command.Type.VALUE)
{
Vector3d turn_v = Vector3d.Cross(State.Vessel.SurfUp, State.Vessel.Direction);
double heading = Command.HeadingAngle;
Vector3d origin_down = State.Vessel.SurfNorth * Math.Sin(heading) + State.Vessel.SurfEast * Math.Cos(heading);
Vector3d tar_down = turn_v.RotationMatrix() * origin_down;
Vector3d roll_v = Vector3d.Cross(-State.Vessel.Up, tar_down);
double roll_ang = roll_v * State.Vessel.Forward;
LinearPlanner.Hover(roll_ang,
altitude_controller_roll_linear_k_,
roll_torque * altitude_controller_roll_max_act_,
roll_torque * altitude_controller_roll_max_act_,
out vel, out acc);
}
double vel_err = State.Vessel.AngularVelocity * State.Vessel.Forward - vel;
double roll = (acc + vel_err * altitude_controller_roll_kp_) / roll_torque;
altitude_controller_roll_int_ = Math.Clamp(altitude_controller_roll_int_ + roll * altitude_controller_roll_ki_, -0.5d, 0.5d);
roll = Math.Clamp(roll + altitude_controller_roll_int_, -1d, 1d);
double max_py = Math.Max(0.2d, 1d - Math.Abs(roll) / 1.5d);
pitch = Math.Clamp(pitch, -max_py, max_py);
yaw = Math.Clamp(yaw, -max_py, max_py);
//return AtitudeControlSetRPY(0, 0, 0);
return(AtitudeControlSetRPY(roll, pitch, yaw));
}
private bool HoverRcs(Vector3d tar_pos = null, RcsLayout rcs_layout = RcsLayout.TOP)
{
if (!Trajectory.ResultAvailable)
{
return(false);
}
double vessel_up_ratio = -State.Vessel.Direction * State.Vessel.Velocity.Norm();
if (vessel_up_ratio < 0.9d && State.Vessel.VelocityMag > 50d)
{
Command.SetRcsRight(0d);
Command.SetRcsUp(0d);
return(false);
}
double rcs_force_limit = RcsMaxHorizonForce(rcs_layout);
double max_rcs_acc = rcs_force_limit / State.Vessel.Mass * 0.9d;
Vector3d tar_vel_h;
Vector3d tar_acc_h;
if (tar_pos == null)
{
tar_vel_h = Vector3d.Zero;
tar_acc_h = Vector3d.Zero;
}
else
{
Vector3d err_v = tar_pos - State.Vessel.Position;// Trajectory.ImpactPositionWithAction;
Vector3d err_h = VectorHorizonPart(err_v);
double err = err_h.Length();
LinearPlanner.Hover(
err, 1d, max_rcs_acc, max_rcs_acc,
out double tar_vel, out double tar_acc);
tar_vel_h = -err_h.Norm() * tar_vel;
tar_acc_h = -err_h.Norm() * tar_acc;
}
tar_acc_h += (tar_vel_h - State.Vessel.VelocityHorizon) * 1d;
RcsSetByForce(
Math.Clamp(tar_acc_h * State.Vessel.Right * State.Vessel.Mass, -rcs_force_limit, rcs_force_limit),
Math.Clamp(tar_acc_h * State.Vessel.Up * State.Vessel.Mass, -rcs_force_limit, rcs_force_limit));
return(false);
}
public void ApproachCalculatePcs(
Vector3d dst_dir, Vector3d tar_vec_lateral,
double tar_distance_lateral, double tar_distance_facing,
Vector3d rel_vel_vec,
out Vector3d tar_acc_vec)
{
RcsForceOnSpecificDirection(dst_dir, out double f_facing, out double fr_facing);
double a_facing = f_facing / State.Vessel.Mass, ar_facing = fr_facing / State.Vessel.Mass;
double k = tar_distance_facing < 0d ?
Math.Clamp(-tar_distance_facing, 0.01d, 1d) :
Math.Clamp((tar_distance_facing - 1d) / 100d, 0.01d, 0.1d);
LinearPlanner.Hover(
tar_distance_facing, k,
a_facing * 0.8d, ar_facing * 0.8d,
out double route_vel_facing, out double route_acc_facing);
Vector3d rel_vel_facing_vec = rel_vel_vec * dst_dir * dst_dir;
Vector3d tar_rel_vel_facing_vec = route_vel_facing * dst_dir;
Vector3d tar_rel_acc_facing_vec = route_acc_facing * dst_dir
+ (tar_rel_vel_facing_vec - rel_vel_facing_vec) * 2d;
Vector3d lat_dir = tar_vec_lateral.Norm();
RcsForceOnSpecificDirection(lat_dir, out double f_lat, out double fr_lat);;
double a_lat = f_lat / State.Vessel.Mass, ar_lat = fr_lat / State.Vessel.Mass;
LinearPlanner.Hover(
tar_distance_lateral, 1d,
a_lat * 0.8d, ar_lat * 0.8d,
out double route_vel_lat, out double route_acc_lat);
route_vel_lat = -route_vel_lat;
route_acc_lat = -route_acc_lat;
Vector3d rel_vel_lat_vec = rel_vel_vec - rel_vel_facing_vec;
Vector3d tar_rel_vel_lat_vec = route_vel_lat * lat_dir;
Vector3d tar_rel_acc_lat_vec = route_acc_lat * lat_dir +
(tar_rel_vel_lat_vec - rel_vel_lat_vec) * 5d;
tar_acc_vec = tar_rel_acc_facing_vec + tar_rel_acc_lat_vec;
}