// 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_rocket.ks in one program.

// Helicopter-style point stability control.
// 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 Hover {
  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.".
}