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); }