From 94c374fcf4fe5744bd73d4a954d1d06de06aa2eb Mon Sep 17 00:00:00 2001
From: annabunches <annabunches@gmail.com>
Date: Sun, 1 Aug 2021 17:46:28 -0400
Subject: [PATCH] More work on rocket stabilization. Still not quite working
 though.

---
 lib/stabilize_rocket.ks | 93 +++++++++++++----------------------------
 rocket/init.ks          |  2 +
 2 files changed, 32 insertions(+), 63 deletions(-)

diff --git a/lib/stabilize_rocket.ks b/lib/stabilize_rocket.ks
index 13833c0..1dbafbc 100644
--- a/lib/stabilize_rocket.ks
+++ b/lib/stabilize_rocket.ks
@@ -1,8 +1,9 @@
 // Provides stabilization functions, including Hover.
+// Control functions can be canceled by actuating Action Group 9.
 
 
 // Stabilizes a rocket relative to the surface, with optional vertical
-// velocity. Do not include both this and helicopter_stabilize.ks in one
+// velocity. Do not include both this and stabilize_helicopter.ks in one
 // program.
 function Hover {
   parameter vertSpeed is 0.0.
@@ -13,14 +14,29 @@ function Hover {
   lock THROTTLE to 0.0.
 
   if SHIP:SENSORS:PRES = 0 {
-    // In a vacuum, perform a more efficient vacuum maneuver.
-    // This will call hoverAtmo after initial stabilization, to make sure
-    // we don't flip upside down.
-    hoverVac(vertSpeed).
+    lock STEERING to SHIP:SRFRETROGRADE.
+    wait until VAng(SHIP:FACING:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR) < 10.
   } else {
-    hoverAtmo(vertSpeed).
+    // debug
+    print "Making vector visualization.".
+    local d1 is VecDraw(V(0,0,0), V(0,0,0), BLUE, "", 1.0, true).
+    set d1:VECUPDATER to {
+      return atmoSafeRetrograde() * 100.
+    }.
+    print "Vector visualization created.".
+    // end debug
+
+    lock STEERING to atmoSafeRetrograde().
+    wait until done or VAng(SHIP:FACING:FOREVECTOR, atmoSafeRetrograde()) < 10.
   }
 
+  local throttlePID is PIDLoop(0.1, 0.1, 0.001, 0, 1).
+  set throttlePID:SETPOINT to vertSpeed.
+
+  lock THROTTLE to throttlePID:Update(TIME:SECONDS, -SHIP:VELOCITY:SURFACE:MAG). // this is still wrong, we just burn hard and spin out of control.
+
+  
+  // Unwind everything.
   set SHIP:CONTROL:PILOTMAINTHROTTLE to THROTTLE.
   unlock THROTTLE.
   unlock STEERING.
@@ -29,70 +45,21 @@ function Hover {
   set done to false.
 }
 
-function hoverAtmo {
-  parameter vertSpeed.
-
-  local headingV is V(0,0,0).
-  lock headingV to VectorExcl(SHIP:UP:FOREVECTOR, SHIP:SRFRETROGRADE).
-
-  // set up PID controllers
-  local pitchPID is PIDLoop(5, 0.1, 0.01, 0, 90).
-  set pitchPID:SETPOINT to 0.
-
-  local throttlePID is PIDLoop(0.1, 0.1, 0.001, 0, 1).
-  set throttlePID:SETPOINT to vertSpeed.
-
-  // we use the inverse of GROUNDSPEED because pitch and acceleration are
-  // inversely proportional here.
-  local newPitch is 0.0.
-  lock newPitch to pitchPID:Update(TIME:SECONDS, -SHIP:GROUNDSPEED).
-  lock STEERING to AngleAxis(-newPitch, SHIP:FACING:STARVECTOR) * headingV.
-  lock THROTTLE to throttlePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
-  wait until done.
-}
-
-function hoverVac {
-  parameter vertSpeed.
-  // TODO: implement me!
-  hoverAtmo(vertSpeed).
-}
-
 // Convenience function for landing operations. Hover over a point with a negative velocity, shutting down on landing.
 function Land {
   set done to false.
   when SHIP:STATUS = "LANDED" then {
     set done to true.
   }
-  Hover(-0.5).
+  Hover(-4).
   set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0.
   set done to false.
 }
 
-// Rocket-style point stability control.
-// Hover, ascend, or descend at a fixed rate.
-// Adjusts the throttle to control target vertical speed.
-// Uses retrograde lock to cancel horizontal velocity.
-// TODO: how does this work
-// function PointStabilizeR {
-//   parameter vertSpeed is 0.0.
-
-//   set done to false.
-//   SAS off.
-
-//   lock STEERING to SHIP:RETROGRADE.
-//   wait until VectorAngle(
-//     SHIP:FACING:FOREVECTOR,
-//     SHIP:SRFRETROGRADE:FOREVECTOR) < 0.05.
-
-//   local throttlePID is PIDLoop(0.1, 0.001, 0.001, 0, 1).
-//   set throttlePID:SETPOINT to vertSpeed.
-//   lock THROTTLE to throttlePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
-//   wait until done.
-
-//   set SHIP:CONTROL:PILOTMAINTHROTTLE to THROTTLE.
-//   unlock THROTTLE.
-//   unlock STEERING.
-//   SAS on.
-//   set done to false.
-//   print "Stabilized operation ended. Returning control to pilot.".
-// }
+function atmoSafeRetrograde {
+  if VAng(SHIP:SRFRETROGRADE:FOREVECTOR, SHIP:UP:FOREVECTOR) < 45 {
+    return SHIP:SRFRETROGRADE:FOREVECTOR.
+  }
+  local planeRetro is VXCL(SHIP:UP:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR).
+  return AngleAxis(45, SHIP:FACING:STARVECTOR) * planeRetro.
+}
diff --git a/rocket/init.ks b/rocket/init.ks
index ea25d6c..eefdeaa 100644
--- a/rocket/init.ks
+++ b/rocket/init.ks
@@ -2,6 +2,8 @@ runoncepath("/lib/navigation").
 runoncepath("/lib/throttle").
 runoncepath("/lib/stabilize_rocket").
 
+clearguis().
+
 global done is false.
 on AG9 {
   set done to true.