From 2099d0a284c79ee3181cd23664bc51c86d3eef6b Mon Sep 17 00:00:00 2001 From: annabunches Date: Fri, 6 Aug 2021 04:57:07 -0400 Subject: [PATCH] Fix up hover script and improve bootloading. --- boot/rocket.ks | 3 +- boot/satellite.ks | 2 +- lib/boot.ks | 14 +++++- lib/sensors.ks | 26 +++++++++++ lib/stabilize_rocket.ks | 97 +++++++++++++++++++++++------------------ scratch/misc.ks | 24 ++++------ 6 files changed, 105 insertions(+), 61 deletions(-) create mode 100644 lib/sensors.ks diff --git a/boot/rocket.ks b/boot/rocket.ks index a4e0a52..63ae57c 100644 --- a/boot/rocket.ks +++ b/boot/rocket.ks @@ -11,7 +11,8 @@ local compiled is list( ). local copied is list( - "/lib/ui" + "/lib/ui", + "/lib/sensors" ). Bootstrap( diff --git a/boot/satellite.ks b/boot/satellite.ks index e650bf2..0994822 100644 --- a/boot/satellite.ks +++ b/boot/satellite.ks @@ -6,7 +6,7 @@ local compiled is list( "/lib/navigation", "/lib/node", "/lib/throttle", - "/prog/nodestats", + "/prog/nodestats" ). local copied is list( diff --git a/lib/boot.ks b/lib/boot.ks index 922a2e7..9db96c6 100644 --- a/lib/boot.ks +++ b/lib/boot.ks @@ -35,7 +35,14 @@ function compileLibs { if lib:StartsWith("/prog") { set tgt to lib:Remove(0, 5). } - compile "0:" + lib to "1:" + tgt. + local res is true. + compile "0:" + lib. + set res to copypath("0:" + lib + ".ksm", "1:" + tgt). + deletepath("0:" + lib + ".ksm"). + if not res { + print "Can't copy compiled file '" + lib + "', aborting.". + print ERR. // intentional error to trap execution + } } } @@ -46,7 +53,10 @@ function copyLibs { if lib:StartsWith("/prog") { set tgt to lib:Remove(0, 5). } - copypath("0:" + lib, "1:" + tgt). + if not copypath("0:" + lib, "1:" + tgt) { + print "Can't copy file '" + lib + "', aborting.". + print ERR. // intentional error to trap execution + } } } diff --git a/lib/sensors.ks b/lib/sensors.ks new file mode 100644 index 0000000..64447b5 --- /dev/null +++ b/lib/sensors.ks @@ -0,0 +1,26 @@ + +// we rebuild the sensor list with every invocation, because things change +function buildSensorList { + local sList is Lexicon(). + list SENSORS in SensorList. + for s in SensorList { + sList:add(s:type, s). + } + return sList. +} + +function HasSensor { + parameter s. + local sList is buildSensorList(). + return sList:HasKey(s). +} + +function ReadSensor { + parameter s. + + local sList is buildSensorList(). + if not sList:HasKey(s) { + return -1. // TODO: is -1 an impossible result for all sensors? I suspect not... + } + return sList[s]. +} diff --git a/lib/stabilize_rocket.ks b/lib/stabilize_rocket.ks index 15c1465..b9e0c88 100644 --- a/lib/stabilize_rocket.ks +++ b/lib/stabilize_rocket.ks @@ -5,6 +5,24 @@ // // 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. + + alignForHover(). + local pid is hoverPID(). + lock THROTTLE to pid:Update(TIME:SECONDS, SHIP:VERTICALSPEED). + until done or SHIP:STATUS = "LANDED" { + set pid:SETPOINT to (-1 * (SHIP:ALTITUDE - SHIP:GEOPOSITION:TERRAINHEIGHT) / 10) - 5. + wait 0.001. + } + + set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0. + set done to false. +} + // Stabilizes a rocket relative to the surface, with optional vertical // velocity. function Hover { @@ -12,33 +30,47 @@ function Hover { set done to false. SAS off. - lock THROTTLE to 0.0. - if SHIP:SENSORS:PRES = 0 { - lock STEERING to SHIP:SRFRETROGRADE. - wait until VAng(SHIP:FACING:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR) < 10. - } else { - // debug - print "Making vector visualization.". - local d1 is VecDraw(V(0,0,0), V(0,0,0), BLUE, "", 1.0, true). - set d1:VECUPDATER to { - return atmoSafeRetrograde() * 100. - }. - print "Vector visualization created.". - // end debug + alignForHover(). + local throttlePID is hoverPID(vertSpeed). + lock THROTTLE to throttlePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED). + wait until done. - lock STEERING to atmoSafeRetrograde(). - wait until done or VAng(SHIP:FACING:FOREVECTOR, atmoSafeRetrograde()) < 10. - } - - 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:VELOCITY:SURFACE:MAG). // this is still wrong, we just burn hard and spin out of control. - - // 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. +} + +function alignForHover { + if ReadSensor("pres") = 0 { + // if we're in a vacuum, cancel all velocity first... + 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 cancel vertical velocity + 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 restoreControls { set SHIP:CONTROL:PILOTMAINTHROTTLE to THROTTLE. unlock THROTTLE. unlock STEERING. @@ -46,22 +78,3 @@ function Hover { print "Stabilized operation ended. Returning control to pilot.". set done to false. } - -// Convenience function for landing operations. Hover over a point with a negative velocity, shutting down on landing. -function Land { - set done to false. - when SHIP:STATUS = "LANDED" then { - set done to true. - } - Hover(-4). - set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0. - set done to false. -} - -function atmoSafeRetrograde { - if VAng(SHIP:SRFRETROGRADE:FOREVECTOR, SHIP:UP:FOREVECTOR) < 45 { - return SHIP:SRFRETROGRADE:FOREVECTOR. - } - local planeRetro is VXCL(SHIP:UP:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR). - return AngleAxis(45, SHIP:FACING:STARVECTOR) * planeRetro. -} diff --git a/scratch/misc.ks b/scratch/misc.ks index cf0fe7e..3f353ab 100644 --- a/scratch/misc.ks +++ b/scratch/misc.ks @@ -1,15 +1,9 @@ -// Check for various sensors and set appropriate global constants. -// Currently only checks for grav sensor, as that's the only one used by this library. -// global HAS_GRAV_SENSOR is false. -// function SensorCheck { -// local SensorList is 0. -// list SENSORS in SensorList. -// for s in SensorList { -// if s:type = "grav" { -// print "Gravometric sensor detected". -// set HAS_GRAV_SENSOR to true. -// return. -// } -// } -// set HAS_GRAV_SENSOR to false. -// } +// This alternative approach to hovering is ultimately not worth the complexity to use, but it's really neat! It maintains *whatever your current vertical velocity is* using the fact that F = μ*m/(r^2). +// In practice, by the time the PID controller settles enough to make this worth +// using, the efficiency gains have mostly dried up anyway. + +// lock STEERING to SHIP:UP. +// wait until done or abs(SHIP:VERTICALSPEED - vertSpeed) < 0.05. +// print "Maintaining velocity.". +// lock targetForce to (SHIP:BODY:MU * SHIP:MASS) / ((SHIP:ALTITUDE + SHIP:BODY:RADIUS)^2). +// lock THROTTLE to targetForce / SHIP:AVAILABLETHRUST.