From bf6bbdc099f013a27d4c264858e2be7ddb8273f8 Mon Sep 17 00:00:00 2001
From: annabunches <annabunches@gmail.com>
Date: Sun, 8 Aug 2021 16:23:20 -0400
Subject: [PATCH] Add program to zero velocity before hover/land operations.

---
 lib/stabilize_rocket.ks | 20 ++++++++++++++++++++
 prog/rocket.ks          |  1 +
 2 files changed, 21 insertions(+)

diff --git a/lib/stabilize_rocket.ks b/lib/stabilize_rocket.ks
index a3dbf80..cbe383b 100644
--- a/lib/stabilize_rocket.ks
+++ b/lib/stabilize_rocket.ks
@@ -6,6 +6,7 @@
 // Do not include both this and stabilize_helicopter.ks in one program.
 
 runoncepath("/lib/sensors").
+runoncepath("/lib/node"). // TODO: move BurnTime to a more sensible library than 'node'
 
 // Convenience function for landing operations. Hover over a point with a negative velocity, shutting down on landing.
 function Land {
@@ -13,6 +14,7 @@ function Land {
 
   assumeControl().
   alignForHover().
+
   local pid is PIDLoop(0.1, 0.01, 0.01, 0, 1).
   
   lock THROTTLE to pid:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
@@ -27,6 +29,24 @@ function Land {
   set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0.
 }
 
+function ZeroVelocity {
+  assumeControl().
+  alignForHover().
+  local processVar is 0.0.
+  print "Aligning.".
+  if ReadSensor("PRES") = 0 {
+    lock processVar to SHIP:VELOCITY:SURFACE:MAG.
+  } else {
+    lock processVar to SHIP:VERTICALSPEED.
+  }
+  local bt is BurnTime(processVar).
+  print "Burning.".
+  lock THROTTLE to 1.0.
+  wait bt.
+  lock THROTTLE to 0.0.
+  restoreControl().
+}
+
 // Stabilizes a rocket relative to the surface, with optional vertical
 // velocity.
 function Hover {
diff --git a/prog/rocket.ks b/prog/rocket.ks
index 2f60901..72770b6 100644
--- a/prog/rocket.ks
+++ b/prog/rocket.ks
@@ -51,6 +51,7 @@ MakeMenu(
   }
 ).
 
+MakeButton(rows[1], "ZERO", { ZeroVelocity(). }).
 MakeButton(rows[1], "HOVER", { Hover(). }).
 MakeButton(rows[1], "LAND", { Land(). }).