97 lines
2.3 KiB
Plaintext
97 lines
2.3 KiB
Plaintext
// 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").
|
|
|
|
// 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().
|
|
if done {
|
|
restoreControl().
|
|
return.
|
|
}
|
|
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.
|
|
}
|
|
|
|
// Stabilizes a rocket relative to the surface, with optional vertical
|
|
// velocity.
|
|
function Hover {
|
|
parameter vertSpeed is 0.0.
|
|
|
|
assumeControl().
|
|
alignForHover().
|
|
if done {
|
|
restoreControl().
|
|
return.
|
|
}
|
|
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 hoverVectorVac {
|
|
parameter top.
|
|
if SHIP:VERTICALSPEED > 0 {
|
|
return SHIP:UP.
|
|
}
|
|
return LookDirUp(SHIP:SRFRETROGRADE:FOREVECTOR, top).
|
|
}
|
|
|
|
function alignForHover {
|
|
set top to SHIP:FACING:FOREVECTOR.
|
|
if ReadSensor("PRES") = 0 {
|
|
// if we're in a vacuum, align with retrograde for smoother horizontal control.
|
|
lock STEERING to hoverDirVac(top).
|
|
print "Aligning with retrograde.".
|
|
wait until done or VAng(SHIP:FACING:FOREVECTOR, hoverDirVac():FOREVECTOR) < 1.
|
|
if done {
|
|
return.
|
|
}
|
|
} else {
|
|
// ... otherwise just align vertically.
|
|
lock STEERING to LookDirUp(SHIP:UP:FOREVECTOR, top).
|
|
print "Aligning vertical.".
|
|
wait until done or VAng(SHIP:FACING:FOREVECTOR, SHIP:UP:FOREVECTOR) < 1.
|
|
if done {
|
|
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.
|
|
}
|