kOS/lib/stabilize_rocket.ks

// Provides stabilization functions, including Hover.
//
// Must define a global variable 'done', which can be set to true to cancel
// control functions.
//
// 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 {
  set done to false.

  assumeControl().
  alignForHover().

  local pid is PIDLoop(0.1, 0.01, 0.01, 0, 1).
  
  lock THROTTLE to pid:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
  local targetVel is 0.
  lock targetVel to Ceiling((-1 * (SHIP:ALTITUDE - SHIP:GEOPOSITION:TERRAINHEIGHT) / 10) - 5).
  until done or SHIP:STATUS = "LANDED" {
    set pid:SETPOINT to targetVel.
    wait 0.001.
  }

  restoreControl().
  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 {
  parameter vertSpeed is 0.0.

  assumeControl().
  alignForHover().
  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:VERTICALSPEED).
  wait until done.

  // Unwind everything.
  restoreControl().
}

function alignForHover {
  if ReadSensor("PRES") = 0 {
    // if we're in a vacuum, align with retrograde for smoother horizontal control.
    lock STEERING to SHIP:SRFRETROGRADE.
    print "Aligning with retrograde.".
    wait until done or VAng(SHIP:FACING:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR) < 0.1.
    if done {
      restoreControls().
      return.
    }
  } else {
    // ... otherwise just align vertically.
    lock STEERING to SHIP:UP.
    print "Aligning vertical.".
    wait until done or VAng(SHIP:FACING:FOREVECTOR, SHIP:UP:FOREVECTOR) < 1.
    if done {
      restoreControls().
      return.
    }
  }
}

function assumeControl {
  set done to false.
  SAS off.
  lock THROTTLE to 0.0.
}

function restoreControl {
  set SHIP:CONTROL:PILOTMAINTHROTTLE to THROTTLE.
  unlock THROTTLE.
  unlock STEERING.
  SAS on.
  print "Done.".
  set done to false.
}