Add helicopter hovering code that works.

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.".
+}