diff --git a/lib/stabilize_rocket.ks b/lib/stabilize_rocket.ks index b9e0c88..d8dcf4a 100644 --- a/lib/stabilize_rocket.ks +++ b/lib/stabilize_rocket.ks @@ -11,16 +11,20 @@ runoncepath("/lib/sensors"). function Land { set done to false. + assumeControl(). alignForHover(). - local pid is hoverPID(). + 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 (-1 * (SHIP:ALTITUDE - SHIP:GEOPOSITION:TERRAINHEIGHT) / 10) - 5. + set pid:SETPOINT to targetVel. wait 0.001. } - + + restoreControl(). set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0. - set done to false. } // Stabilizes a rocket relative to the surface, with optional vertical @@ -28,24 +32,15 @@ function Land { function Hover { parameter vertSpeed is 0.0. - set done to false. - SAS off. - lock THROTTLE to 0.0. - + assumeControl(). alignForHover(). - local throttlePID is hoverPID(vertSpeed). + 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. - restoreControls(). -} - -function hoverPID { - parameter initSetpoint is 0. - local pid is PIDLoop(0.01, 0.01, 0.001, 0, 1). - set pid:SETPOINT to initSetpoint. - return pid. + restoreControl(). } function alignForHover { @@ -70,7 +65,13 @@ function alignForHover { } } -function restoreControls { +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.