void Awake() { aircraft = GetComponent <Aircraft>(); altitudePlanner = new LinearPlanner(aircraft.CurrentAltitudeInMeters); altitudePlanner.maxSpeed = aircraft.maxVerticalSpeed; altitudePlanner.acceleration = aircraft.verticalAcceleration; }
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); }
public Trajectory.SimulationResult LandingSimThrust(Trajectory.SimulationData data) { Vector3d up = data.pos.Norm(); Vector3d vel_dir = data.vel.Norm(); double vessel_up_ratio = data.vel.Length() > 100d ? -up * vel_dir : 1d; if (vessel_up_ratio < 0.5d) { return(new Trajectory.SimulationResult()); } double max_thrust_up = /*vessel_up_ratio */ data.available_thrust; double max_acc_up = Math.Max(0.01d, max_thrust_up / data.mass * LANDING_MAX_THROTTLE - data.g); LinearPlanner.OneWay(data.altitude - data.tar_altitude, 0.5d, max_acc_up, landing_min_velocity_, out double tar_vel, out double tar_acc); tar_acc += (tar_vel - data.vel * up) * 2d; double tar_throttle = Math.Clamp((tar_acc + data.g) * data.mass / max_thrust_up, 0d, 1d); return(new Trajectory.SimulationResult( -tar_throttle * data.available_thrust * vel_dir, tar_throttle * data.available_thrust, tar_throttle)); }
void Awake() { aircraft = GetComponent <Aircraft>(); headingPlanner = new LinearPlanner(aircraft.CurrentHeadingInDegrees); headingPlanner.maxSpeed = aircraft.maxTurnSpeed; headingPlanner.acceleration = aircraft.turnAcceleration; //headingPlanner.debug = true; }
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); }
private bool LandingThrust(double tar_altitude) { if (landing_adjust_throttle_ > 0d) { Command.SetThrottle(landing_adjust_throttle_); return(false); } 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); LinearPlanner.OneWay(State.Vessel.Altitude - tar_altitude, 0.5d, max_acc_up, landing_min_velocity_, 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); //sw.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t{6}\t{7}", // Data.UT, // State.Vessel.Velocity, // State.Vessel.Position, // tar_throttle, // State.Vessel.Mass, // State.Vessel.AvailableThrust, // State.Vessel.Gravity, // new Vector3d(ActiveVessel.Flight().AerodynamicForce)); 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; }
public void AddPlan(float startDelay, float targetHeading) { Debug.Log("Creating plan at " + startDelay + " to go to " + targetHeading); int insertAt = 0; while (insertAt < futurePlans.Count) { float startTime = futurePlans[insertAt].startDelay; if (startTime > startDelay) { break; } else { insertAt++; } } float headingAtInsertTime = GetHeadingInFuture(startDelay); Debug.Log("Will be going at heading " + headingAtInsertTime); LinearPlanner planner = new LinearPlanner(headingAtInsertTime); planner.maxSpeed = aircraft.maxTurnSpeed; planner.acceleration = aircraft.turnAcceleration; planner.TargetValue = targetHeading; FuturePlan newPlan = new FuturePlan(startDelay, planner); newPlan.startHeading = headingAtInsertTime; futurePlans.Insert(insertAt, newPlan); UpdateAllPlans(); }
public void ApproachCalculateThrust( Vector3d dst_vel, Vector3d dst_acc, Vector3d tar_vec_dir, double tar_distance, Vector3d rel_vel_vec, out Vector3d tar_acc_vec, out Vector3d tar_dir, out double route_vel, out double tar_vel_err_mag, out double tar_thrust_acc, out Vector3d tar_thrust_acc_vec, out double throttle) { double turn_time = ApproachEstimatedTurnTime(-tar_vec_dir); double rel_vel = rel_vel_vec * tar_vec_dir; double thrust_acc_max = State.Vessel.AvailableThrust / State.Vessel.Mass; double thrust_acc = thrust_acc_max * 0.9d; LinearPlanner.OneWay( tar_distance - rel_vel * turn_time, 1d, thrust_acc, 0d, out double route_vel_tmp, out double route_acc_tmp); route_vel = -route_vel_tmp; double route_acc = -route_acc_tmp; Vector3d tar_vel_vec = dst_vel + route_vel * tar_vec_dir; Vector3d tar_vel_err = tar_vel_vec - State.Vessel.Velocity; tar_vel_err_mag = tar_vel_err.Length(); Vector3d tar_vel_err_on_dir = tar_vel_err * tar_vec_dir * tar_vec_dir; Vector3d tar_vel_err_lateral = tar_vel_err - tar_vel_err_on_dir; double lateral_pi = approach_stage_ == ApproachStage.MIX ? 2.0d : Math.Max(0.1d, tar_vel_err_lateral.Length() / tar_vel_err_on_dir.Length() / 10d); tar_acc_vec = dst_acc - State.Vessel.Gravity + route_acc * tar_vec_dir + tar_vel_err_lateral * lateral_pi + tar_vel_err_on_dir * 2d; tar_dir = tar_acc_vec.Norm(); tar_thrust_acc = Math.Max(0d, tar_acc_vec * State.Vessel.Direction); tar_thrust_acc_vec = tar_thrust_acc * State.Vessel.Direction; double min_thrust = approach_stage_ == ApproachStage.MIX ? 0.0d : 0.05d; throttle = Math.Max(tar_thrust_acc / thrust_acc_max, min_thrust); }
public FuturePlan(float _startDelay, LinearPlanner _linearPlanner) { startDelay = _startDelay; linearPlanner = _linearPlanner; }