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