56 lines
1.6 KiB
Plaintext
56 lines
1.6 KiB
Plaintext
// 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.".
|
|
}
|