public bool Hover(double tar_altitude, Vector3d tar_pos = null, RcsLayout rcs_layout = RcsLayout.TOP)
{
ActiveVessel.AutoPilot.TargetHeading = 90f;
HoverDirection();
HoverThrust(tar_altitude);
HoverRcs(tar_pos, rcs_layout);
return(false);
}
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);
}
private bool LandingRcs(Vector3d tar_pos, double tar_altitude, RcsLayout rcs_layout)
{
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) ||
State.Vessel.Direction * State.Vessel.SurfUp < 0.5d)
{
Command.SetRcsRight(0d);
Command.SetRcsUp(0d);
return(false);
}
double rcs_force_limit = RcsMaxHorizonForce(rcs_layout);
Vector3d v_err = tar_pos - Trajectory.ImpactPositionWithAction;
Vector3d tar_acc = v_err / 2d;
landing_rcs_tar_acc_i_ += tar_acc * 0.03d;
double int_upper_bound = Math.Min(
Math.Abs(tar_altitude - State.Vessel.Altitude) / 1000d,
Math.Abs(State.Vessel.VelocityUp) / 100d);
int_upper_bound = Math.Min(int_upper_bound, 5d);
if (landing_rcs_tar_acc_i_.Length() > int_upper_bound)
{
landing_rcs_tar_acc_i_ *= int_upper_bound / landing_rcs_tar_acc_i_.Length();
}
tar_acc += landing_rcs_tar_acc_i_;
RcsSetByForce(
Math.Clamp(tar_acc * State.Vessel.Right * State.Vessel.Mass, -rcs_force_limit, rcs_force_limit),
Math.Clamp(tar_acc * State.Vessel.Up * State.Vessel.Mass, -rcs_force_limit, rcs_force_limit));
return(false);
}
/// <summary>
/// 调整落地位置后，控制火箭着陆。
/// </summary>
/// <param name="tar_pos">目标坐标</param>
/// <param name="tar_altitude">目标海拔</param>
/// <param name="rcs_layout">RCS 布局</param>
/// <param name="gear_deploy_time">着陆架部署需要时间</param>
/// <param name="heading">Radian</param>
/// <returns></returns>
public bool Landing(
Vector3d tar_pos, double tar_altitude, RcsLayout rcs_layout,
double gear_deploy_time,
double?heading = null)
{
//if (RcsAltitudeControl && Trajectory.ResultAvailable && Trajectory.NextBurnTime < 0.5d)
// RcsAltitudeControl = false;
tar_pos = TargetPositionCompensate2(tar_pos, tar_altitude);
if (State.Vessel.Altitude - tar_altitude < 10000d && -State.Vessel.VelocityUp < 20d)
{
LandingRcsLast(tar_pos, rcs_layout);
if (!gear_deployed_ && Trajectory.ImpactTime <= gear_deploy_time)
{
gear_deployed_ = ActiveVessel.Control.Gear = true;
}
if (-State.Vessel.VelocityUp < landing_min_velocity_ / 2d)
{
return(true);
}
}
else
{
LandingRcs(tar_pos, tar_altitude, rcs_layout);
}
LandingDirection(tar_pos, tar_altitude, rcs_layout);
LandingThrust(tar_altitude);
if (heading.HasValue)
{
Command.SetHeadingAngle(heading.Value);
}
else
{
Command.StableHeading();
}
return(false);
}
public bool LandingDirection(Vector3d tar_pos, double tar_altitude, RcsLayout rcs_layout)
{
if (!Trajectory.ResultStable)
{
Command.SetTargetDirection(-State.Vessel.Velocity);
return(false);
}
Vector3d tar_pos_v_hori = tar_pos - (Trajectory.ImpactPositionWithAction + Trajectory.ImpactPositionWithActionChangeRate * 2d);
tar_pos_v_hori = VectorHorizonPart(tar_pos_v_hori);
double distance = tar_pos_v_hori.Length();
double throttle = -1d;
double up_ratio = Math.Max(State.Vessel.SurfUp * State.Vessel.Direction, 0.01d);
double vel_tilt = Math.Acos(up_ratio);
double grav_tilt = Math.Acos(State.Vessel.GravityMag * 0.8d * State.Vessel.Mass / State.Vessel.AvailableThrust);
double adjust_tilt = Math.Min(Math.PI / 2d, Math.Max(vel_tilt, grav_tilt));
double adjust_throttle = 1d;
if (Trajectory.NextBurnTime > LANDING_ADJUST2_TURN_TIME)
{
double v = State.Vessel.VelocityHorizonMag;
double a = State.Vessel.AvailableThrust * adjust_throttle / State.Vessel.Mass * Math.Sin(adjust_tilt);
double s = VectorHorizonPart(State.Vessel.Position - tar_pos).Length();
double r = State.Vessel.Position.Length();
s *= (r + State.Vessel.VelocityUp * v / a) / r;
Vector3d tar = State.Vessel.Position - tar_pos;
Vector3d tar_pos_v_hori_without_comp = VectorHorizonPart(tar_pos - Trajectory.ImpactPositionWithAction);
double distance_without_comp = tar_pos_v_hori_without_comp.Length();
if (distance_without_comp > 200d && landing_adjust_throttle_ >= 0d ||
((tar.Norm() * State.Vessel.SurfUp > Math.Cos(20d / 180d * Math.PI) ||
s < v * v / 2d / a + v * LANDING_ADJUST2_TURN_TIME * 2d ||
a * (Trajectory.NextBurnTime - LANDING_ADJUST2_TURN_TIME * 2d) * 5d < v) &&
distance_without_comp > 200d + 100d * Trajectory.NextBurnTime))
{
tar_pos_v_hori = tar_pos_v_hori_without_comp;
distance = distance_without_comp;
landing_lift_angle_ = -adjust_tilt;
throttle = adjust_throttle;
}
else if (Trajectory.LiftEstimationForceMin > 1d)
{
landing_lift_angle_ = LANDING_ADJUST2_MIN_TILT_RAD;
}
else
{
Command.SetTargetDirection(Trajectory.EnterAtmosphereDirection);
landing_adjust_throttle_ = -1d;
return(false);
}
}
else
{
double thrust_f = Trajectory.LiftEstimationThrustAve * Math.Sin(Trajectory.LiftEstimationAngle);
double rcs_f = RcsMaxHorizonForce();
double diff = thrust_f + rcs_f - Trajectory.LiftEstimationForceAve;
landing_lift_angle_ = -Trajectory.LiftEstimationAngle * Math.Clamp(diff / State.Vessel.Mass, -1d, 1d);
}
Vector3d tar_dir;
double turn_angle = 0d;
if (throttle < 0d)
{
if (rcs_layout == RcsLayout.SYMMETRICAL)
{
tar_dir = -State.Vessel.Velocity.Norm();
}
else
{
double turn_angle_ratio = Math.Clamp(distance * 100d / Math.Clamp(State.Vessel.Altitude - tar_altitude, 1000d, 50000d), 0d, 1d);
double turn_angle_limit = Math.Clamp((State.Vessel.Altitude - tar_altitude - 100d) / 500d, 0d, 1d);
turn_angle = -landing_lift_angle_ * turn_angle_ratio * turn_angle_limit;
Vector3d turn_v1 = Vector3d.Cross(Trajectory.ImpactPositionWithAction.Norm(), tar_pos.Norm()).Norm();
Vector3d turn_v = turn_angle * turn_v1;
Matrix3d r = turn_v.RotationMatrix();
tar_dir = r * (-State.Vessel.Velocity).Norm();
}
if (tar_dir * State.Vessel.SurfUp < 0d)
{
tar_dir = VectorHorizonPart(tar_dir).Norm();
if (State.Vessel.Direction * tar_dir < -0.5d)
{
tar_dir = State.Vessel.SurfUp;
}
}
}
else
{
Vector3d hori_dir = tar_pos_v_hori;
tar_dir = hori_dir.Norm() * Math.Sin(adjust_tilt) + State.Vessel.SurfUp * Math.Cos(adjust_tilt);
}
double up_dir_ratio = MathLib.InverseLerpWithClamp(50d, 0d, State.Vessel.VelocityMag);
tar_dir = tar_dir * (1d - up_dir_ratio) + State.Vessel.SurfUp * up_dir_ratio;
if (tar_dir * State.Vessel.SurfUp < 0d)
{
tar_dir = VectorHorizonPart(tar_dir);
}
tar_dir = tar_dir.Norm();
Command.SetTargetDirection(tar_dir);
double dir_error = State.Vessel.Direction * tar_dir;
landing_adjust_throttle_ = throttle < 0d ? -1d : Math.Max(0d, throttle * (dir_error - 0.95d) * 20d);
//if (ActiveVessel == SpaceCenter.ActiveVessel)
//{
// Conn.Drawing().Clear();
// Conn.Drawing().AddDirection(
// SpaceCenter.TransformDirection(tar_dir.ToTuple(), OrbitBodyFrame, ActiveVessel.ReferenceFrame),
// ActiveVessel.ReferenceFrame,
// 30f);
// Conn.Drawing().AddDirection(
// SpaceCenter.TransformDirection(tar_pos_v_hori.Norm().ToTuple(), OrbitBodyFrame, ActiveVessel.ReferenceFrame),
// ActiveVessel.ReferenceFrame,
// 50f);
// Conn.Drawing().AddLine(
// State.Vessel.Position.ToTuple(),
// Trajectory.ImpactPositionWithAction.ToTuple(),
// OrbitBodyFrame);
// Conn.Drawing().AddLine(
// State.Vessel.Position.ToTuple(),
// tar_pos.ToTuple(),
// OrbitBodyFrame);
// Console.WriteLine("{0:0.0}\t{1:0}\t{2:00}\t{3:0.00}\t{4:0.00}",
// Trajectory.NextBurnTime,
// throttle,
// landing_lift_angle / Math.PI * 180d,
// distance,
// turn_angle / Math.PI * 180d);
//}
return(false);
}
