void select_waypoint()
{
NavWaypoint navPoint = NavWaypoint.fetch;
if (navPoint == null || !navPoint.IsActive || navPoint.Body != this.vessel.mainBody)
{
MessageManager.post_quick_message("No waypoint available");
}
else
{
current_waypt.longitude = navPoint.Longitude;
current_waypt.latitude = navPoint.Latitude;
Debug.Log($"[AtmosphereAutopilot] waypoint lat {current_waypt.latitude} lon {current_waypt.longitude}");
desired_latitude.Value = (float)current_waypt.latitude;
desired_longitude.Value = (float)current_waypt.longitude;
AtmosphereAutopilot.Instance.mainMenuGUIUpdate();
WaypointMode = true;
MessageManager.post_quick_message("Waypoint selected");
}
}
void select_target()
{
var target = vessel.targetObject?.GetVessel();
if (target == null || target.mainBody != vessel.mainBody)
{
MessageManager.post_quick_message("No target to select");
}
else
{
if (!target.Landed())
{
MessageManager.post_quick_message($"target {target.vesselName} is not landed");
}
current_waypt.longitude = target.longitude;
current_waypt.latitude = target.latitude;
Debug.Log($"[AtmosphereAutopilot] target lat {current_waypt.latitude} lon {current_waypt.longitude}");
desired_latitude.Value = (float)current_waypt.latitude;
desired_longitude.Value = (float)current_waypt.longitude;
AtmosphereAutopilot.Instance.mainMenuGUIUpdate();
WaypointMode = true;
MessageManager.post_quick_message($"Waypoint now {target.vesselName}");
}
}
public override void ApplyControl(FlightCtrlState cntrl)
{
if (vessel.LandedOrSplashed())
{
return;
}
if (thrust_c.spd_control_enabled)
{
thrust_c.ApplyControl(cntrl, thrust_c.setpoint.mps());
}
desired_velocity = Vector3d.zero;
planet2ves = vessel.ReferenceTransform.position - vessel.mainBody.position;
planet2vesNorm = planet2ves.normalized;
desired_vert_acc = Vector3d.zero;
// centrifugal acceleration to stay on desired altitude
level_acc = -planet2vesNorm * (imodel.surface_v - Vector3d.Project(imodel.surface_v, planet2vesNorm)).sqrMagnitude / planet2ves.magnitude;
switch (current_mode)
{
default:
case CruiseMode.LevelFlight:
// simply select velocity from axis
desired_velocity = Vector3d.Cross(planet2vesNorm, circle_axis);
handle_wide_turn();
if (vertical_control)
{
if (height_mode == HeightMode.Altitude)
{
desired_velocity = account_for_height(desired_velocity);
}
else
{
desired_velocity = account_for_vertical_vel(desired_velocity);
}
}
break;
case CruiseMode.CourseHold:
if (Math.Abs(vessel.latitude) > 80.0)
{
// we're too close to poles, let's switch to level flight
LevelFlightMode = true;
goto case CruiseMode.LevelFlight;
}
// get direction vector form course
Vector3d north = vessel.mainBody.RotationAxis;
Vector3d north_projected = Vector3.ProjectOnPlane(north, planet2vesNorm);
QuaternionD rotation = QuaternionD.AngleAxis(desired_course, planet2vesNorm);
desired_velocity = rotation * north_projected;
handle_wide_turn();
if (vertical_control)
{
if (height_mode == HeightMode.Altitude)
{
desired_velocity = account_for_height(desired_velocity);
}
else
{
desired_velocity = account_for_vertical_vel(desired_velocity);
}
}
break;
case CruiseMode.Waypoint:
// set new axis
Vector3d world_target_pos = vessel.mainBody.GetWorldSurfacePosition(desired_latitude, desired_longitude, vessel.altitude);
dist_to_dest = Vector3d.Distance(world_target_pos, vessel.ReferenceTransform.position);
if (dist_to_dest > 10000.0)
{
double radius = vessel.mainBody.Radius;
dist_to_dest = Math.Acos(1 - (dist_to_dest * dist_to_dest) / (2 * radius * radius)) * radius;
}
if (dist_to_dest < 200.0)
{
// we're too close to target, let's switch to level flight
LevelFlightMode = true;
picking_waypoint = false;
MessageManager.post_quick_message("Waypoint reached");
goto case CruiseMode.LevelFlight;
}
// set new axis according to waypoint
circle_axis = Vector3d.Cross(world_target_pos - vessel.mainBody.position, vessel.GetWorldPos3D() - vessel.mainBody.position).normalized;
goto case CruiseMode.LevelFlight;
}
if (use_keys)
{
ControlUtils.neutralize_user_input(cntrl, PITCH);
ControlUtils.neutralize_user_input(cntrl, YAW);
}
double old_str = dir_c.strength;
dir_c.strength *= strength_mult;
dir_c.ApplyControl(cntrl, desired_velocity, level_acc + desired_vert_acc);
dir_c.strength = old_str;
}
public override void OnUpdate()
{
if (picking_waypoint)
{
if (!HighLogic.LoadedSceneIsFlight || !MapView.MapIsEnabled)
{
// we left map without picking
MessageManager.post_quick_message("Cancelled");
picking_waypoint = false;
AtmosphereAutopilot.Instance.mainMenuGUIUpdate();
return;
}
// Thanks MechJeb!
if (Input.GetMouseButtonDown(0) && !window.Contains(Input.mousePosition))
{
Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);
mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
Vector3d relOrigin = mouseRay.origin - vessel.mainBody.position;
Vector3d relSurfacePosition;
double curRadius = vessel.mainBody.pqsController.radiusMax;
if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, curRadius, out relSurfacePosition))
{
Vector3d surfacePoint = vessel.mainBody.position + relSurfacePosition;
current_waypt.longitude = vessel.mainBody.GetLongitude(surfacePoint);
current_waypt.latitude = vessel.mainBody.GetLatitude(surfacePoint);
picking_waypoint = false;
desired_latitude.Value = (float)current_waypt.latitude;
desired_longitude.Value = (float)current_waypt.longitude;
dist_to_dest = Vector3d.Distance(surfacePoint, vessel.ReferenceTransform.position);
AtmosphereAutopilot.Instance.mainMenuGUIUpdate();
WaypointMode = true;
MessageManager.post_quick_message("Picked");
}
else
{
MessageManager.post_quick_message("Missed");
}
}
}
else
{
if (Input.GetKeyDown(switch_key_mode))
{
use_keys = !use_keys;
MessageManager.post_status_message(use_keys ? "CF key input mode enabled" : "CF key input mode disabled");
}
if (Input.GetKeyDown(vertical_control_key))
{
vertical_control = !vertical_control;
MessageManager.post_status_message(use_keys ? "Vertical motion control enabled" : "Vertical motion control disabled");
}
if (Input.GetKeyDown(toggle_vertical_setpoint_type_key))
{
AltitudeMode = !AltitudeMode;
MessageManager.post_status_message(AltitudeMode ? "Altitude control" : "Vertical speed control");
}
// input shenanigans
if (use_keys && !FlightDriver.Pause && InputLockManager.IsUnlocked(ControlTypes.PITCH) &&
InputLockManager.IsUnlocked(ControlTypes.YAW))
{
bool pitch_key_pressed = false;
float pitch_change_sign = 0.0f;
// Pitch
if (GameSettings.PITCH_UP.GetKey() && !GameSettings.MODIFIER_KEY.GetKey())
{
pitch_change_sign = 1.0f;
pitch_key_pressed = true;
}
else if (GameSettings.PITCH_DOWN.GetKey() && !GameSettings.MODIFIER_KEY.GetKey())
{
pitch_change_sign = -1.0f;
pitch_key_pressed = true;
}
if (pitch_key_pressed)
{
if (height_mode == HeightMode.Altitude)
{
float setpoint = desired_altitude;
float new_setpoint = setpoint + pitch_change_sign * hotkey_altitude_sens * Time.deltaTime * setpoint;
desired_altitude.Value = new_setpoint;
}
else
{
float setpoint = desired_vertspeed;
float magnetic_mult = Mathf.Abs(desired_vertspeed) < 10.0f ? 0.3f : 1.0f;
float new_setpoint = setpoint + pitch_change_sign * hotkey_vertspeed_sens * Time.deltaTime * magnetic_mult;
desired_vertspeed.Value = new_setpoint;
}
need_to_show_altitude = true;
altitude_change_counter = 0.0f;
AtmosphereAutopilot.Instance.mainMenuGUIUpdate();
}
// Yaw (Course)
bool yaw_key_pressed = false;
float yaw_change_sign = 0.0f;
if (GameSettings.YAW_RIGHT.GetKey() && !GameSettings.MODIFIER_KEY.GetKey())
{
yaw_key_pressed = true;
yaw_change_sign = 1.0f;
}
else if (GameSettings.YAW_LEFT.GetKey() && !GameSettings.MODIFIER_KEY.GetKey())
{
yaw_key_pressed = true;
yaw_change_sign = -1.0f;
}
if (yaw_key_pressed)
{
float setpoint = desired_course;
float new_setpoint = setpoint + yaw_change_sign * hotkey_course_sens * Time.deltaTime;
if (new_setpoint > 360.0f)
{
new_setpoint -= 360.0f;
}
if (new_setpoint < 0.0f)
{
new_setpoint = 360.0f + new_setpoint;
}
desired_course.Value = new_setpoint;
need_to_show_course = true;
course_change_counter = 0.0f;
}
if (need_to_show_course)
{
course_change_counter += Time.deltaTime;
}
if (course_change_counter > 1.0f)
{
course_change_counter = 0;
need_to_show_course = false;
}
if (need_to_show_altitude)
{
altitude_change_counter += Time.deltaTime;
if (height_mode == HeightMode.VerticalSpeed && altitude_change_counter > 0.2f)
{
if (Mathf.Abs(desired_vertspeed) < hotkey_vertspeed_snap)
{
desired_vertspeed.Value = 0.0f;
}
}
}
if (altitude_change_counter > 1.0f)
{
altitude_change_counter = 0;
need_to_show_altitude = false;
}
}
else
{
need_to_show_altitude = false;
altitude_change_counter = 0;
need_to_show_course = false;
course_change_counter = 0;
}
}
}
protected override void _drawGUI(int id)
{
close_button();
GUILayout.BeginVertical();
// cruise flight control modes
LevelFlightMode = GUILayout.Toggle(LevelFlightMode, "Level", GUIStyles.toggleButtonStyle);
GUILayout.Space(5.0f);
CourseHoldMode = GUILayout.Toggle(CourseHoldMode, "Heading", GUIStyles.toggleButtonStyle);
GUILayout.BeginHorizontal();
GUILayout.Label("desired course", GUIStyles.labelStyleLeft);
desired_course.DisplayLayout(GUIStyles.textBoxStyle);
GUILayout.EndHorizontal();
GUILayout.Space(5.0f);
string waypoint_btn_str;
if (WaypointMode)
{
waypoint_btn_str = "WPT " + (dist_to_dest / 1000.0).ToString("#0.0") + " km";
}
else
{
waypoint_btn_str = "Waypoint";
}
WaypointMode = GUILayout.Toggle(WaypointMode, waypoint_btn_str,
GUIStyles.toggleButtonStyle);
GUILayout.BeginHorizontal();
GUILayout.Label("map:", GUIStyles.labelStyleRight);
if (GUILayout.Button("Pick", GUIStyles.toggleButtonStyle) && !picking_waypoint)
{
if (this.Active)
{
start_picking_waypoint();
}
else
{
MessageManager.post_quick_message("Can't pick waypoint when the Cruise Flight controller is disabled");
}
}
GUILayout.Label("from:", GUIStyles.labelStyleRight);
if (GUILayout.Button("Tgt", GUIStyles.toggleButtonStyle))
{
if (this.Active)
{
select_target();
}
else
{
MessageManager.post_quick_message("Can't select target when the Cruise Flight controller is disabled");
}
}
if (GUILayout.Button("Wpt", GUIStyles.toggleButtonStyle))
{
if (this.Active)
{
select_waypoint();
}
else
{
MessageManager.post_quick_message("Can't select waypoint when the Cruise Flight controller is disabled");
}
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
desired_latitude.DisplayLayout(GUIStyles.textBoxStyle); //, GUILayout.Width(60.0f));
desired_longitude.DisplayLayout(GUIStyles.textBoxStyle); //, GUILayout.Width(60.0f));
GUILayout.EndHorizontal();
GUILayout.Space(10.0f);
// speed
thrust_c.SpeedCtrlGUIBlock();
GUILayout.Space(10.0f);
// vertical motion
vertical_control = GUILayout.Toggle(vertical_control, "Vertical motion", GUIStyles.toggleButtonStyle);
GUILayout.BeginHorizontal();
GUILayout.BeginVertical();
AltitudeMode = GUILayout.Toggle(AltitudeMode, "Altitude", GUIStyles.toggleButtonStyle); // GUILayout.Width(90.0f)
desired_altitude.DisplayLayout(GUIStyles.textBoxStyle); // GUILayout.Width(90.0f)
GUILayout.EndVertical();
GUILayout.BeginVertical();
VerticalSpeedMode = GUILayout.Toggle(VerticalSpeedMode, "Vertical speed", GUIStyles.toggleButtonStyle);
desired_vertspeed.DisplayLayout(GUIStyles.textBoxStyle);
GUILayout.EndVertical();
GUILayout.EndHorizontal();
GUILayout.Space(10.0f);
// status
//GUILayout.Label("Status", GUIStyles.labelStyleCenter);
//GUILayout.BeginHorizontal();
//GUILayout.BeginVertical();
//GUILayout.Label("Latitude", GUIStyles.labelStyleCenter);
//GUILayout.Label(vessel.latitude.ToString("G6"), GUIStyles.labelStyleCenter);
//GUILayout.EndVertical();
//GUILayout.BeginVertical();
//GUILayout.Label("Longitude", GUIStyles.labelStyleCenter);
//GUILayout.Label(vessel.longitude.ToString("G7"), GUIStyles.labelStyleCenter);
//GUILayout.EndVertical();
//GUILayout.BeginVertical();
//if (WaypointMode)
//{
// GUILayout.Label("Dist (km)", GUIStyles.labelStyleCenter);
// GUILayout.Label((dist_to_dest / 1000.0).ToString("#0.0"), GUIStyles.labelStyleCenter);
//}
//else
//{
// GUILayout.Label("Alt (m)", GUIStyles.labelStyleCenter);
// GUILayout.Label(vessel.altitude.ToString("G5") + " m", GUIStyles.labelStyleCenter);
//}
//GUILayout.EndVertical();
//GUILayout.EndHorizontal();
//GUILayout.Space(10.0f);
// advanced options
bool adv_o = advanced_options;
advanced_options = GUILayout.Toggle(advanced_options, "Advanced options", GUIStyles.toggleButtonStyle);
if (advanced_options)
{
GUILayout.Space(5.0f);
AutoGUI.AutoDrawObject(this);
}
else if (adv_o)
{
window.height = 100.0f;
}
GUILayout.EndVertical();
GUI.DragWindow();
}
void start_picking_waypoint()
{
MapView.EnterMapView();
MessageManager.post_quick_message("Pick waypoint");
picking_waypoint = true;
}