Add helicopter hovering code that works.

boot/helicopter_debug.ks

@@ -1,6 +1,6 @@
-copypath("0:/helicopter/init", "1:/init").
+copypath("0:/lib/control", "1:/lib/control").
 copypath("0:/helicopter/hover", "1:/hover").
-copypath("0:/debug", "1:/debug").
+copypath("0:/helicopter/init", "1:/init").
 
 CORE:PART:GETMODULE("kOSProcessor"):DOEVENT("Open Terminal").
 run "1:/init".

debug/vecrender.ks

@@ -1,25 +1,83 @@
+// requires a global list called draws, apparently
 @lazyglobal off.
 
-parameter vec is false.
+parameter vec is V(0,0,0).
 
-local draws is list().
+function RenderVectorWithComponents {
-
-function DrawVectorWithComponents {
   parameter vec.
+  parameter prefix is "".
 
+  if prefix <> "" {
+    set prefix to prefix + " ".
+  }
+
+  local dv is 0.
+  local dx is 0.
+  local dy is 0.
+  local dz is 0.
+  set dv to VecDraw(V(0,0,0), vec, WHITE, prefix + "v", 1.0, true).
+  draws:add(dv).
+  set dx to VecDraw(V(0,0,0), V(vec:X,0,0), RED, prefix + "x", 1.0, true).
+  draws:add(dx).
+  set dy to VecDraw(V(0,0,0), V(0,vec:Y,0), GREEN, prefix + "y", 1.0, true).
+  draws:add(dy).
+  set dz to VecDraw(V(0,0,0), V(0,0,vec:Z), BLUE, prefix + "z", 1.0, true).
+  draws:add(dz).
+}
+
+function RenderVectorShipRelative {
+  parameter vec.
+  parameter prefix is "".
+
+  if prefix <> "" {
+    set prefix to prefix + " ".
+  }
+
+  // this seems backwards, but velocity vectors at least seem to work this way.
+  local axes is 0. lock axes to lookdirup(SHIP:FACING:FOREVECTOR, SHIP:UP:FOREVECTOR).
+  local dv is 0.
+  local dx is 0.
+  local dy is 0.
+  local dz is 0.
+
+  set dv to VecDraw(V(0,0,0), vec, WHITE, prefix + "v", 1.0, true).
+  draws:add(dv).
+
+  set dx to VecDraw(V(0,0,0), V(vec:X,0,0), RED, prefix + "x", 1.0, true).
+  draws:add(dx).
+
+  set dy to VecDraw(V(0,0,0), V(0,vec:Y,0), GREEN, prefix + "y", 1.0, true).
+  draws:add(dy).
+
+  set dz to VecDraw(V(0,0,0), V(0,0,vec:Z), BLUE, prefix + "z", 1.0, true).
+  draws:add(dz).
+}
+
+function RenderDirAxes {
+  parameter dir.
+  parameter prefix is "".
+
+  if prefix <> "" {
+    set prefix to prefix + " ".
+  }
+
+  local dx is 0.
+  local dy is 0.
+  local dz is 0.
+  set dx to VecDraw(V(0,0,0), dir:RIGHTVECTOR * 100, RED, prefix + "x", 1.0, true).
+  draws:add(dx).
+  set dy to VecDraw(V(0,0,0), dir:TOPVECTOR * 100, GREEN, prefix + "y", 1.0, true).
+  draws:add(dy).
+  set dz to VecDraw(V(0,0,0), dir:FOREVECTOR * 100, BLUE, prefix + "z", 1.0, true).
+  draws:add(dz).
+}
+
+if vec <> V(0,0,0) {
+   // RenderVectorShipRelative(vec).
+  RenderDirAxes(vec).
+}
+
+function ClearRenders {
   set draws to list().
-  local d is 0.
+  ClearVecDraws().
-
-  set d to VecDraw(V(0,0,0), vec, WHITE, "v", 1.0, true).
-  list:add(d).
-  set d to VecDraw(V(0,0,0), V(vec:X,0,0), RED, "x", 1.0, true).
-  list:add(d).
-  set d to VecDraw(V(0,0,0), V(0,vec:Y,0), GREEN, "y", 1.0, true).
-  list:add(d).
-  set d to VecDraw(V(0,0,0), V(0,0,vec:Z), BLUE, "z", 1.0, true).
-  list:add(d).
-}
-
-if vec <> false {
-    DrawVectorWithComponents(vec).
 }

helicopter/hover.ks

@@ -1,48 +1,4 @@
 // This script assumes you have bound collective (aka deploy angle) to main throttle.
-runoncepath("/debug/vecrender"). // debug
+runoncepath("lib/control").
 
-// SAS off. // debug: re-enable when adjusting heading
+Stabilize().
-local pitchPID is PIDLoop(0.01, 0.001, 0.001, -45, 45).
-set pitchPID:SETPOINT to 0.
-local rollPID is PIDLoop(0.01, 0.001, 0.001, -70, 70).
-set rollPID:SETPOINT to 0.
-local collectivePID is PIDLoop(0.1, 0.1, 0.001, 0, 1).
-set collectivePID:SETPOINT to 0.
-
-
-// local x is 0. lock x to SHIP:FACING:FOREVECTOR * SHIP:VELOCITY:SURFACE.
-// local y is 0. lock y to (SHIP:UP:FOREVECTOR * SHIP:VELOCITY:SURFACE) / (SHIP:VELOCITY:SURFACE:MAG * cos(VectorAngle(SHIP:UP:FOREVECTOR, SHIP:VELOCITY:SURFACE))).
-// local z is 0. lock z to SHIP:FACING:RIGHTVECTOR * SHIP:VELOCITY:SURFACE.
-// local spd is 0. lock spd to SHIP:VELOCITY:SURFACE:MAG.
-
-local done is false.
-on AG9 {
-  set done to true.
-}
-
-until done {
-  // local newPitch is pitchPID:Update(TIME:SECONDS, spd). // was x
-  // local newRoll is rollPid:Update(TIME:SECONDS, spd). // was z
-  local newThrot is collectivePID:Update(TIME:SECONDS, SHIP:VELOCITY:SURFACE:Y). // was y
-
-  // debug
-  print "Yaw = " + mod(360 - SHIP:BEARING, 360).
-  print "Pitch = " + newPitch.
-  print "Roll = " + newRoll.
-  print "Throt = " + newThrot.
-  print "Vertical Speed = " + SHIP:VELOCITY:SURFACE:Y.
-  DrawVectorWithComponents(SHIP:VELOCITY:SURFACE).
-  
-  set SHIP:CONTROL:PILOTMAINTHROTTLE to newThrot.
-  // lock STEERING to Heading(
-  //   mod(360 - SHIP:BEARING, 360),
-  //   newPitch,
-  //   newRoll
-  //   ).
-  wait 0.001.
-}
-
-// Because helicopters can hover stably, once we achieve a hover we should be able to turn SAS on and call it good.
-unlock STEERING.
-SAS on.
-print "Hover operation canceled. Returning control.".

helicopter/init.ks

@@ -1,3 +1,7 @@
+runoncepath("lib/control").
+
+global done is false.
+
 // Main UI.
 local interface is gui(250, 300).
 set interface:X to 200.
@@ -8,9 +12,19 @@ local y is interface:AddVLayout().
 local x is y:AddHLayout().
 
 set x:AddButton("HOVER"):onClick to {
-  run "hover".
+  Stabilize().
 }.
 
+set x:AddButton("LAND"):onClick to {
+  Stabilize(-2).
+}.
+
+
+on AG9 {
+  set done to true.
+  return true.
+}
+
 interface:show().
 
 wait until false.

lib/control.ks

@@ -0,0 +1,55 @@
+// functions that execute comprehensive control schemes.
+//
+// These typically assume a global variable 'done' that will be
+// set externally when they should return.
+
+
+// Hover, ascend, or descend at a fixed rate.
+// Adjusts the throttle to control ascent or descent.
+// Adjusts pitch and roll to maintain zero lateral velocity.
+function Stabilize {
+  parameter vertSpeed is 0.0.
+
+  set done to false.
+  SAS off.
+
+  // set up PID controllers
+  local pitchPID is PIDLoop(5, 0.1, 0.01, -45, 45).
+  set pitchPID:SETPOINT to 0.
+
+  local rollPID is PIDLoop(5, 0.1, 0.01, -70, 70).
+  set rollPID:SETPOINT to 0.
+
+  local collectivePID is PIDLoop(0.1, 0.1, 0.001, 0, 1).
+  set collectivePID:SETPOINT to vertSpeed.
+
+  // keep the craft facing a single direction so vectoring is consistent
+  local yaw is mod(360 - SHIP:BEARING, 360).
+
+  // TOPVECTOR (y) is forward
+  // FOREVECTOR (z) is up
+  // RIGHTVECTOR (x) is left
+  lock axes to lookdirup(SHIP:UP:VECTOR, SHIP:FACING:VECTOR).
+  
+  until done {
+    local newPitch is pitchPID:Update(TIME:SECONDS, SHIP:VELOCITY:SURFACE * axes:TOPVECTOR).
+    local newRoll is rollPID:Update(TIME:SECONDS, SHIP:VELOCITY:SURFACE * axes:RIGHTVECTOR).
+    local newThrot is collectivePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
+
+    print "Forward speed: " + SHIP:VELOCITY:SURFACE * axes:TOPVECTOR.
+    print "Lateral speed: " + SHIP:VELOCITY:SURFACE * axes:RIGHTVECTOR.
+    
+    set SHIP:CONTROL:PILOTMAINTHROTTLE to newThrot.
+    lock STEERING to Heading(
+      yaw,
+      -newPitch,
+      newRoll
+      ).
+    wait 0.001.
+  }
+
+  unlock STEERING.
+  SAS on.
+  set done to false.
+  print "Stabilized operation ended. Returning control to pilot.".
+}