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