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