Last active
November 3, 2024 19:48
-
-
Save SofieBrink/6fe2b31896f518880b4f7b42d04f955a to your computer and use it in GitHub Desktop.
SofiePlaneLib.ks
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // !runscript 6fe2b31896f518880b4f7b42d04f955a | |
| // Sofie's Plane Lib | |
| //------------------------------------------------------------------------------------------------- | |
| // Usage Description. Parameters with {OPT} infront can be left out. | |
| // Takeoff({OPT}Vel, {OPT}Mode, {OPT}, {OPT}Target, {OPT}dTA) - Flight control function for taking off, from a runway if on one. at DesSp or as given by Vel (Setting Vel updates DesVV). | |
| // Mode sets the desired autopilot mode after takeoff, options are "Slope", "Altitude" or "Alt". The selected mode will be activated with Target (Setting Target updates DesSlope or DesA depending on Mode). | |
| // dTA determines if TerrainAvoidance should be re-enabled after takeoff (Setting dTA updates TerrainAvoidance). | |
| // FlyRunwayApproach(Runway, {OPT}Glideslope, {OPT}FinalDistance, {OPT}dTA) - Flight control function for landing on the specified runway. | |
| // dTA determines if TerrainAvoidance should be re-enabled after landing or in the case of a missed approach (Setting dTA updates TerrainAvoidance). | |
| // GoAround() - Flight control function for manually initiating a go-around while on a runway approach. | |
| // HoldVerticalSpeed({OPT}VerticalSpeed) - Flight control function for maintaining DesVV or as given by the parameter (Setting the parameter updates DesVV). | |
| // HoldVelocity({OPT}Vel, {OPT}Mode) - Flight control function for holding a desired velocity DesSp or as given by the parameter Vel. Also has the option to specify a velocity mode in parameter Mode. | |
| // HoldHeading({OPT}Heading) - Flight control function for maintaining DesH or as given by the parameter (Setting the parameter updates DesH). | |
| // HoldAltitude({OPT}Altitude, {OPT}ClimbDescentAngle) - Flight control function for maintaining DesA or as given by the parameter with a maximum and minimum rate ClimbDescentAngle or as given by the second parameter (Setting the parameters update DesA and ClimbDescentAngle). | |
| // HoldSlope({OPT}Glideslope, {OPT}ClimDescentAngle) - Flight control function for maining a verticalspeed that corrosponds with DesSlope or as given by the parameter (Setting the parameters updates DesSlope and ClimbDescentAngle) | |
| // GetRunways({OPT}Body) - Function to fill the Runways Lexicon with the runways that have been extracted from the json file with the same name as the body the craft is currently on, or as given by the parameter. | |
| // ChangeVelocityMode(Mode) - Flight control function to change between the three velocity modes. This will automatically update the setpoint so the velocity stays constant. | |
| // UpdateAttitudeLimits(Upper_Pitch_Limit, Lower_Pitch_Limit, Sideslip_Limit, Roll_Limit) - Control function to allow changing of Attitude limits inflight (Calling this will also update the PID loops). | |
| // SofiePlaneLibDisplay({OPT}StartLine) - Function to start or stop displaying data, set which data to display in SofiePlaneLibDisplayLex starts at the line as given by the parameter or set in SofiePlaneLibDisplayStart. | |
| // SofiePlaneLibSteering() - Internal function for activating script control. Can be used to re-activate the script after a manual takeover. | |
| // SofiePlaneLibUpdatePIDs(DesP_PID, DesS_PID, DesR_PID, DesA_PID, DesSp_PID) - Internal function for updating the PIDloops used by the script for various tasks, Only call if you know what you're doing! | |
| // SofiePlaneLibBearingCalculator(ref, tgt) - Internal function for calculating a signed bearing between 2 headings. | |
| // GetGroundDistanceToRunway(rw) - Internal function for checking the current ground distance to a runways as given by the parameter. | |
| // SofiePlaneLibLoop() - Internal function for starting the 15hz update loop for critical flightcontrol functionality. Only call if you know what you're doing! | |
| // SofiePlaneLibTerrainAvoidance() - Internal function to calculate parameters that are relevant for the terrain avoindance system, Automatically called by the loop DON'T CALL. | |
| //------------------------------------------------------------------------------------------------- | |
| // Variables | |
| set SofiePlaneLibPresent to true. | |
| set SofiePlaneLibVarClash to false. | |
| set SofiePlaneLibVersion to "V0.8.37". | |
| //------------------------------------------------------------------------------------------------- | |
| // Lib Helper Functions | |
| function SofiePlaneLibPrint { | |
| // Function to allow printing of messages with SPL prefix for easier debugging | |
| parameter type, message. | |
| parameter short is true. | |
| if (type = "Debug" AND not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibDebug"])) return. | |
| print (choose "SPL " if (short) else "Sofie's Plane Lib ") + type + ": " + message. | |
| } | |
| function SofiePlaneLibClearscreen { | |
| // Function to clear the terminal screen but leaving the cursor at the bottom of the terminal so it is not hidden by any potential printouts. | |
| local i is 0. | |
| until (i >= terminal:height) { | |
| print (" "). | |
| set i to i + 1. | |
| } | |
| } | |
| //------------------------------------------------------------------------------------------------- | |
| // Startup | |
| SofiePlaneLibClearscreen(). | |
| SofiePlaneLibPrint("", SofiePlaneLibVersion + " Loaded", false). | |
| set SofiePlaneLibVarLex to lex( | |
| // Lexicon containing names of all library variables | |
| "DesP", false, | |
| "DesR", false, | |
| "DesH", false, | |
| "DesS", false, | |
| "DesVV", false, | |
| "DesSp", false, | |
| "DesA", false, | |
| "DesSlope", false, | |
| "Runways", false, | |
| "ClimbDescentAngle", false, | |
| "TerrainAvoidance", false, | |
| "TurnRadiusMultiplier", false, | |
| "SofiePlaneLibTA_Alt", false, | |
| "SofiePlaneLibDirection", false, | |
| "SofiePlaneLibAttitudeLimits", false, | |
| "SofiePlaneLibPIDloops", false, | |
| "SofiePlaneLibDisplayLex", false, | |
| "SofiePlaneLibInternalVariablesLex", false | |
| ). | |
| // Check if each variable has been defined previously. Store results in the SofiePlaneLibVarLex | |
| if not(defined(DesP) OR defined(DesP@)) { SofiePlaneLibVarLex["DesP"] on. } | |
| if not(defined(DesR) OR defined(DesR@)) { SofiePlaneLibVarLex["DesR"] on. } | |
| if not(defined(DesH) OR defined(DesH@)) { SofiePlaneLibVarLex["DesH"] on. } | |
| if not(defined(DesS) OR defined(DesS@)) { SofiePlaneLibVarLex["DesS"] on. } | |
| if not(defined(DesVV) OR defined(DesVV@)) { SofiePlaneLibVarLex["DesVV"] on. } | |
| if not(defined(DesSp) OR defined(DesSp@)) { SofiePlaneLibVarLex["DesSp"] on. } | |
| if not(defined(DesA) OR defined(DesA@)) { SofiePlaneLibVarLex["DesA"] on. } | |
| if not(defined(DesSlope) OR defined(DesSlope@)) { SofiePlaneLibVarLex["DesSlope"] on. } | |
| if not(defined(Runways) OR defined(Runways@)) { SofiePlaneLibVarLex["Runways"] on. } | |
| if not(defined(ClimbDescentAngle) OR defined(ClimbDescentAngle@)) { SofiePlaneLibVarLex["ClimbDescentAngle"] on. } | |
| if not(defined(TerrainAvoidance) OR defined(TerrainAvoidance@)) { SofiePlaneLibVarLex["TerrainAvoidance"] on. } | |
| if not(defined(TurnRadiusMultiplier) OR defined(TurnRadiusMultiplier@)) { SofiePlaneLibVarLex["TurnRadiusMultiplier"] on. } | |
| if not(defined(SofiePlaneLibTA_Alt) OR defined(SofiePlaneLibTA_Alt@)) { SofiePlaneLibVarLex["SofiePlaneLibTA_Alt"] on. } | |
| if not(defined(SofiePlaneLibDirection) OR defined(SofiePlaneLibDirection@)) { SofiePlaneLibVarLex["SofiePlaneLibDirection"] on. } | |
| if not(defined(SofiePlaneLibAttitudeLimits) OR defined(SofiePlaneLibAttitudeLimits@)) { SofiePlaneLibVarLex["SofiePlaneLibAttitudeLimits"] on. } | |
| if not(defined(SofiePlaneLibPIDloops) OR defined(SofiePlaneLibPIDloops@)) { SofiePlaneLibVarLex["SofiePlaneLibPIDloops"] on. } | |
| if not(defined(SofiePlaneLibDisplayLex) OR defined(SofiePlaneLibDisplayLex@)) { SofiePlaneLibVarLex["SofiePlaneLibDisplayLex"] on. } | |
| if not(defined(SofiePlaneLibInternalVariablesLex) OR defined(SofiePlaneLibInternalVariablesLex@)) { SofiePlaneLibVarLex["SofiePlaneLibInternalVariablesLex"] on. } | |
| // Check all keys for var clashing | |
| for key in SofiePlaneLibVarLex:keys { | |
| if (not(SofiePlaneLibVarLex[key])) { | |
| SofiePlaneLibPrint("Error", "Var clash found on: " + key). | |
| set SofiePlaneLibVarClash to true. | |
| } | |
| } | |
| // Define Undefined Variables | |
| if (SofiePlaneLibVarLex["DesP"]) { lock DesP to 0. } | |
| if (SofiePlaneLibVarLex["DesR"]) { lock DesR to 0. } | |
| if (SofiePlaneLibVarLex["DesH"]) { lock DesH to 0. } | |
| if (SofiePlaneLibVarLex["DesS"]) { lock DesS to 0. } | |
| if (SofiePlaneLibVarLex["DesVV"]) { lock DesVV to 0. } | |
| if (SofiePlaneLibVarLex["DesSp"]) { lock DesSp to 0. } | |
| if (SofiePlaneLibVarLex["DesA"]) { lock DesA to 0. } | |
| if (SofiePlaneLibVarLex["DesSlope"]) { lock DesSlope to 0. } | |
| if (SofiePlaneLibVarLex["Runways"]) { set Runways to lex(). } | |
| if (SofiePlaneLibVarLex["ClimbDescentAngle"]) { lock ClimbDescentAngle to 5. } | |
| if (SofiePlaneLibVarLex["TerrainAvoidance"]) { lock TerrainAvoidance to true. } | |
| if (SofiePlaneLibVarLex["TurnRadiusMultiplier"]) { lock TurnRadiusMultiplier to 1. } | |
| if (SofiePlaneLibVarLex["SofiePlaneLibTA_Alt"]) { lock SofiePlaneLibTA_Alt to 75. } | |
| if (SofiePlaneLibVarLex["SofiePlaneLibDirection"]) { lock SofiePlaneLibDirection to srfprograde * R(-DesP, DesS, -DesR). } | |
| if (SofiePlaneLibVarLex["SofiePlaneLibAttitudeLimits"]) { set SofiePlaneLibAttitudeLimits to lex("Upper_Pitch_Limit", 15, "Lower_Pitch_Limit", -3, "Sideslip_Limit", 2, "Roll_Limit", 25). } | |
| if (SofiePlaneLibVarLex["SofiePlaneLibPIDloops"]) { set SofiePlaneLibPIDloops to lex("DesP_PID", PIDloop(1.75, 0.18, 0.75), "DesS_PID", PIDloop(0.45, 0, 0.48), "DesR_PID", PIDloop(1.8, 0, 1.2), "DesA_PID", PIDloop(0.20, 0, 0.28), "DesSp_PID", PIDloop(0.1,0.002,0.2)). } | |
| if (SofiePlaneLibVarLex["SofiePlaneLibDisplayLex"]) { set SofiePlaneLibDisplayLex to lex("Title", true, "Attitude Target", true, "Attitude", true, "Velocity Target", true, "Velocity", true). } | |
| if (SofiePlaneLibVarLex["SofiePlaneLibInternalVariablesLex"]) { set SofiePlaneLibInternalVariablesLex to lex("SofiePlaneLibVelocityMode", "GND", "SofiePlaneLibDisplayKeep", false, "SofiePlaneLibVelocityLex", lex("T_GND", 0, "T_IAS", 0, "T_Mach", 0, "GND", 0, "IAS", 0, "Mach", 0), "SofiePlaneLibTA_Alt", 0, "SofiePlaneLibTA_Climb", 0, "SofiePlaneLibTA_AngVelList", list(ship:angularvel), "SofiePlaneLibBounds", ship:bounds, "SofiePlaneLibDisplayStart", 0, "SofiePlaneLibFlyRunwayApproach", false, "SofiePlaneLibGoAround", false, "SofiePlaneLibDebug", false). } | |
| // Print a warning incase of clashing variables | |
| if (SofiePlaneLibVarClash) { SofiePlaneLibPrint("Warning", "Clashing Variable(s) Detected. Proceed with caution!"). } | |
| // Allow the steering manager to apply roll torque to the craft, no matter its orientation | |
| set steeringManager:rollcontrolanglerange to 180. | |
| // Allow the steering manager to use more of the available pitch torque | |
| set steeringManager:pitchtorquefactor to 0.2. // 5 helps for some vehicles. odd | |
| //------------------------------------------------------------------------------------------------- | |
| // Internal Control Functions | |
| function SofiePlaneLibSteering { | |
| // Lock steering to the vector if it is not already | |
| if not(steeringManager:target = SofiePlaneLibDirection) { lock steering to SofiePlaneLibDirection. } | |
| } | |
| function SofiePlaneLibUpdatePIDs { | |
| // If pidloops are not the same as given pidloops, Update them. Also re-set the minimum and maximum limits of the PID's | |
| parameter DesP_PID is SofiePlaneLibPIDloops["DesP_PID"]. | |
| parameter DesS_PID is SofiePlaneLibPIDloops["DesS_PID"]. | |
| parameter DesR_PID is SofiePlaneLibPIDloops["DesR_PID"]. | |
| parameter DesA_PID is SofiePlaneLibPIDloops["DesA_PID"]. | |
| parameter DesSp_PID is SofiePlaneLibPIDloops["DesSp_PID"]. | |
| if not(DesP_PID:Kp = SofiePlaneLibPIDloops["DesP_PID"]:Kp AND DesP_PID:Ki = SofiePlaneLibPIDloops["DesP_PID"]:Ki AND DesP_PID:Kd = SofiePlaneLibPIDloops["DesP_PID"]:Kd) { set SofiePlaneLibPIDloops["DesP_PID"] to DesP_PID. } | |
| if not(DesS_PID:Kp = SofiePlaneLibPIDloops["DesS_PID"]:Kp AND DesS_PID:Ki = SofiePlaneLibPIDloops["DesS_PID"]:Ki AND DesS_PID:Kd = SofiePlaneLibPIDloops["DesS_PID"]:Kd) { set SofiePlaneLibPIDloops["DesS_PID"] to DesS_PID. } | |
| if not(DesR_PID:Kp = SofiePlaneLibPIDloops["DesR_PID"]:Kp AND DesR_PID:Ki = SofiePlaneLibPIDloops["DesR_PID"]:Ki AND DesR_PID:Kd = SofiePlaneLibPIDloops["DesR_PID"]:Kd) { set SofiePlaneLibPIDloops["DesR_PID"] to DesR_PID. } | |
| if not(DesA_PID:Kp = SofiePlaneLibPIDloops["DesA_PID"]:Kp AND DesA_PID:Ki = SofiePlaneLibPIDloops["DesA_PID"]:Ki AND DesA_PID:Kd = SofiePlaneLibPIDloops["DesA_PID"]:Kd) { set SofiePlaneLibPIDloops["DesA_PID"] to DesA_PID. } | |
| if not(DesSp_PID:Kp = SofiePlaneLibPIDloops["DesSp_PID"]:Kp AND DesSp_PID:Ki = SofiePlaneLibPIDloops["DesSp_PID"]:Ki AND DesSp_PID:Kd = SofiePlaneLibPIDloops["DesSp_PID"]:Kd) { set SofiePlaneLibPIDloops["DesSp_PID"] to DesSp_PID. } | |
| if not(SofiePlaneLibAttitudeLimits["Lower_Pitch_Limit"] = SofiePlaneLibPIDloops["DesP_PID"]:minoutput AND SofiePlaneLibAttitudeLimits["Upper_Pitch_Limit"] = SofiePlaneLibPIDloops["DesP_PID"]:maxoutput) { | |
| set SofiePlaneLibPIDloops["DesP_PID"]:minoutput to SofiePlaneLibAttitudeLimits["Lower_Pitch_Limit"]. | |
| set SofiePlaneLibPIDloops["DesP_PID"]:maxoutput to SofiePlaneLibAttitudeLimits["Upper_Pitch_Limit"]. | |
| } | |
| if not(-SofiePlaneLibAttitudeLimits["Sideslip_Limit"] = SofiePlaneLibPIDloops["DesS_PID"]:minoutput AND SofiePlaneLibAttitudeLimits["Sideslip_Limit"] = SofiePlaneLibPIDloops["DesS_PID"]:maxoutput) { | |
| set SofiePlaneLibPIDloops["DesS_PID"]:minoutput to -SofiePlaneLibAttitudeLimits["Sideslip_Limit"]. | |
| set SofiePlaneLibPIDloops["DesS_PID"]:maxoutput to SofiePlaneLibAttitudeLimits["Sideslip_Limit"]. | |
| } | |
| if not(-SofiePlaneLibAttitudeLimits["Roll_Limit"] = SofiePlaneLibPIDloops["DesR_PID"]:minoutput AND SofiePlaneLibAttitudeLimits["Roll_Limit"] = SofiePlaneLibPIDloops["DesR_PID"]:maxoutput) { | |
| set SofiePlaneLibPIDloops["DesR_PID"]:minoutput to -SofiePlaneLibAttitudeLimits["Roll_Limit"]. | |
| set SofiePlaneLibPIDloops["DesR_PID"]:maxoutput to SofiePlaneLibAttitudeLimits["Roll_Limit"]. | |
| } | |
| if not (0 = SofiePlaneLibPIDloops["DesSp_PID"]:minoutput AND 1 = SofiePlaneLibPIDloops["DesSp_PID"]:maxoutput) { | |
| set SofiePlaneLibPIDloops["DesSp_PID"]:minoutput to 0. | |
| set SofiePlaneLibPIDloops["DesSp_PID"]:maxoutput to 1. | |
| } | |
| } | |
| function SofiePlaneLibBearingCalculator { | |
| // Function to determine a signed bearing between 2 headings. | |
| parameter ref, tgt. | |
| set rel to tgt - ref. | |
| if (rel > 180) { | |
| set rel to rel - 360. | |
| } | |
| else if (rel < -180) { | |
| set rel to rel + 360. | |
| } | |
| return rel. | |
| } | |
| function SofiePlaneLibTurningCircleCalculator { | |
| // Function to determine the distance the plane will fly around a HAC, | |
| // given the current speed setpoint and bank angle | |
| parameter deg. | |
| local turnRadius is SofiePlaneLibTurningRadiusCalculator(). | |
| local turnCircle is (turnRadius * constant:pi * 2) / 360 * deg. | |
| return turnCircle. | |
| } | |
| function SofiePlaneLibTurningRadiusCalculator { | |
| // Function to determine the radius of a plane's turning circle. | |
| return ((airspeed^2) / (gAt(altitude) * tan(SofiePlaneLibAttitudeLimits["Roll_Limit"]))) * TurnRadiusMultiplier. | |
| } | |
| function gAt { | |
| // Function to return the acceleration due to gravity at an altitude. | |
| parameter aslPlus is 0. | |
| return ship:body:mu/((ship:body:radius + aslPlus)^2). | |
| } | |
| function circle_bearing { | |
| // Function to return the heading between to positions. | |
| parameter p1, p2. | |
| return mod(360+arctan2(sin(p2:lng-p1:lng)*cos(p2:lat),cos(p1:lat)*sin(p2:lat)-sin(p1:lat)*cos(p2:lat)*cos(p2:lng-p1:lng)),360). | |
| } | |
| function geoPosition_plus { | |
| // Function to return a geoposition a certain distance and heading from another. | |
| parameter geopos, dist, angle. | |
| local newLat is geopos:lat + (dist / (body:radius * 2 * constant:pi / 360)) * cos(angle). | |
| local newLng is geopos:lng + (dist / ((body:radius * 2 * constant:pi / 360) * cos(geopos:lat)) * (sin(angle))). | |
| return latlng(newLat, newLng). | |
| } | |
| function UpdateAttitudeLimits { | |
| // Update the attitude limits imposed on the functions provided by the script | |
| parameter Upper_Pitch_Limit, Lower_Pitch_Limit, Sideslip_Limit, Roll_Limit. | |
| set SofiePlaneLibAttitudeLimits["Upper_Pitch_Limit"] to Upper_Pitch_Limit. | |
| set SofiePlaneLibAttitudeLimits["Lower_Pitch_Limit"] to Lower_Pitch_Limit. | |
| set SofiePlaneLibAttitudeLimits["Sideslip_Limit"] to abs(Sideslip_Limit). | |
| set SofiePlaneLibAttitudeLimits["Roll_Limit"] to abs(Roll_Limit). | |
| SofiePlaneLibUpdatePIDs(). | |
| } | |
| function SofiePlaneLibDisplay { | |
| // Display some useful data in the terminal | |
| parameter startline is SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayStart"]. | |
| if not(startline = SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayStart"]) { | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayStart"] to startline. | |
| } | |
| if not (SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayKeep"]) { | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayKeep"] on. | |
| SofiePlaneLibPrint("", "Starting Flight Information Display, Call again to disable."). | |
| local linebreak is "". | |
| local i is 0. | |
| until i >= terminal:width { | |
| set i to i + 1. | |
| set linebreak to linebreak + "-". | |
| } | |
| on round(time:seconds * 15) { | |
| local line is SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayStart"]. | |
| if (SofiePlaneLibDisplayLex["Title"]) { | |
| // Print Title | |
| print (" Sofie's Plane Lib Display"):padright(terminal:width) at (0, line). set line to line + 1. | |
| print (linebreak):padright(terminal:width) at (0, line). set line to line + 1. | |
| } | |
| if (SofiePlaneLibDisplayLex["Attitude Target"]) { | |
| // Print target attitude data | |
| print (" DesP: " + round(DesP, 1) + " | " + "DesS: " + round(DesS, 1) + " | " + "DesR: " + round(DesR, 1)):padright(terminal:width) at (0, line). set line to line + 1. | |
| print (linebreak):padright(terminal:width) at (0, line). set line to line + 1. | |
| } | |
| if (SofiePlaneLibDisplayLex["Attitude"]) { | |
| // Print attitude Data | |
| local pitch is 0. | |
| local sideslip is 0. | |
| local roll is 0. | |
| if (ship:velocity:surface:mag >= 1) { | |
| set pitch to round(vang(up:vector, ship:facing:vector) - vang(up:vector, ship:velocity:surface), 1). | |
| // set sideslip to round(choose -vang(vxcl(up:vector, ship:facing:vector), vxcl(up:vector, ship:velocity:surface)) if vang(vxcl(up:vector, ship:facing:starvector), vxcl(up:vector, ship:velocity:surface)) < 90 else vang(vxcl(up:vector, ship:facing:vector), vxcl(up:vector, ship:velocity:surface)), 1). | |
| set sideslip to round(vang(vxcl(up:vector, ship:facing:starvector), vxcl(up:vector, ship:velocity:surface)) - 90, 1). | |
| if vang(vxcl(ship:facing:vector, up:vector), ship:facing:topvector) < 90 AND vang(vcrs(vxcl(up:vector, ship:facing:vector), up:vector), ship:facing:topvector) < 90 { | |
| set roll to round(-vang(vxcl(ship:facing:vector, up:vector), ship:facing:topvector), 1). | |
| } | |
| else { | |
| set roll to round(vang(vxcl(ship:facing:vector, up:vector), ship:facing:topvector), 1). | |
| } | |
| } | |
| print (" Pitch: " + -pitch + " | " + "Sideslip: " + sideslip + " | " + "Roll: " + roll):padright(terminal:width) at (0, line). set line to line + 1. | |
| print (linebreak):padright(terminal:width) at (0, line). set line to line + 1. | |
| } | |
| if (SofiePlaneLibDisplayLex["Velocity Target"]) { | |
| // Print target velocity data | |
| print (" Velocity Mode: " + SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"]):padright(terminal:width) at (0, line). set line to line + 1. | |
| print (" GND : " + round(SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_GND"], 1) + " | " + "IAS: " + round(SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_IAS"], 1) + " | " + "Mach: " + round(SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_Mach"], 2)):padright(terminal:width) at (0, line). set line to line + 1. | |
| print (linebreak):padright(terminal:width) at (0, line). set line to line + 1. | |
| } | |
| if (SofiePlaneLibDisplayLex["Velocity"]) { | |
| // Print velocity data | |
| print (" GND : " + round(SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["GND"], 1) + " | " + "IAS: " + round(SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["IAS"], 1) + " | " + "Mach: " + round(SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["Mach"], 2)):padright(terminal:width) at (0, line). set line to line + 1. | |
| print (linebreak):padright(terminal:width) at (0, line). set line to line + 1. | |
| } | |
| return SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayKeep"]. | |
| } | |
| } | |
| else { | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibDisplayKeep"] off. | |
| } | |
| } | |
| // Flight Control Functions | |
| function HoldVerticalSpeed { | |
| // Update DesVV with a given parameter, lock DesP to the desired pitch as given by the SofiePlaneLibPitchPID, Call SofiePlaneLibSteering. | |
| parameter dverticalSpeed is DesVV. | |
| if not(dverticalSpeed = DesVV) { | |
| set DesVV to dverticalSpeed. | |
| } | |
| lock DesP to SofiePlaneLibPIDloops["DesP_PID"]:update(time:seconds, ship:verticalspeed - DesVV). | |
| SofiePlaneLibSteering(). | |
| } | |
| function HoldVelocity { | |
| // Lock the throttle to hold a desired DesSp, dependant on the current velocity mode. | |
| parameter vel is DesSp. | |
| parameter mode is SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"]. | |
| if not (mode = SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"]) { | |
| ChangeVelocityMode(mode). | |
| } | |
| if not(vel = DesSp) { | |
| set DesSp to vel. | |
| } | |
| lock throttle to SofiePlaneLibPIDloops["DesSp_PID"]:update(time:seconds, ship:airspeed - SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_GND"]). | |
| } | |
| function HoldHeading { | |
| // Lock roll and sidelip to desired values to keep the desired heading. | |
| parameter dHeading is DesH. | |
| if not (dHeading = DesH) { | |
| set DesH to dHeading. | |
| } | |
| lock DesR to -SofiePlaneLibPIDloops["DesR_PID"]:update(time:seconds, SofiePlaneLibBearingCalculator(body:geopositionof(ship:velocity:surface):heading, DesH)). | |
| lock DesS to -SofiePlaneLibPIDloops["DesS_PID"]:update(time:seconds, SofiePlaneLibBearingCalculator(body:geopositionof(ship:velocity:surface):heading, DesH)). | |
| SofiePlaneLibSteering(). | |
| } | |
| function HoldAltitude { | |
| // lock DesVV to a value that will be defined by DesA and ClimbDescentAngle | |
| parameter dAltitude is DesA. | |
| parameter dClimbDescentAngle is ClimbDescentAngle. | |
| if not (dAltitude = DesA) { | |
| set DesA to dAltitude. | |
| } | |
| if not (dClimbDescentAngle = ClimbDescentAngle) { | |
| set ClimbDescentAngle to dClimbDescentAngle. | |
| } | |
| lock DesVV to choose airspeed * sin(SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"]) | |
| if SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] <> 0 AND SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] > min(airspeed * sin(abs(ClimbDescentAngle)), max(airspeed * -sin(abs(ClimbDescentAngle)), SofiePlaneLibPIDloops["DesA_PID"]:update(time:seconds, ship:altitude - DesA))) | |
| else min(airspeed * sin(abs(ClimbDescentAngle)), max(airspeed * -sin(abs(ClimbDescentAngle)), SofiePlaneLibPIDloops["DesA_PID"]:update(time:seconds, ship:altitude - DesA))). | |
| HoldVerticalSpeed(). | |
| } | |
| function HoldSlope { | |
| // lock DesVV to a value that will be defined by DesSlope and ClimbDescentAngle | |
| parameter dDesSlope is DesSlope. | |
| parameter dClimbDescentAngle is ClimbDescentAngle. | |
| if not (dDesSlope = DesSlope) { | |
| set DesSlope to dDesSlope. | |
| } | |
| if not (dClimbDescentAngle = ClimbDescentAngle) { | |
| set ClimbDescentAngle to dClimbDescentAngle. | |
| } | |
| lock DesVV to airspeed * (choose sin(SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"]) | |
| if SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] <> 0 AND SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] > DesSlope | |
| else (choose sin(min(DesSlope, ClimbDescentAngle)) | |
| if DesSlope > 0 | |
| else sin(max(DesSlope, -ClimbDescentAngle)))). | |
| HoldVerticalSpeed(). | |
| } | |
| function Takeoff { | |
| // Function to do takeoff duties. | |
| parameter dDesSp to DesSp. | |
| parameter dMode is "Slope". | |
| parameter dTarget is choose 3 if dMode = "Slope" else ship:altitude + 150. | |
| parameter dTA is TerrainAvoidance. | |
| local onRunway is false. | |
| local isAirborne is false. | |
| if (dMode <> "Slope" AND dMode <> "Alt" AND dMode <> "Altitude") { | |
| SofiePlaneLibPrint("Error", "Invalid Mode: '" + dMode + "' Entered, please retry with a valid mode: Slope, Alt or Altitude"). | |
| return. | |
| } | |
| for runway in Runways:keys { | |
| if (abs(SofiePlaneLibBearingCalculator(body:geoPositionof(ship:facing:vector):heading, Runways[runway][1]:heading)) < 25 AND GetGroundDistanceToRunway(Runways[runway]) < 35) { | |
| set onRunway to runway. | |
| SofiePlaneLibPrint("", "On Runway: '" + runway + "', proceeding with takeoff."). | |
| break. | |
| } | |
| } | |
| if (onRunway = false) SofiePlaneLibPrint("", "Not on a runway, proceeding with takeoff."). | |
| if not(dDesSp = DesSp) { | |
| set DesSp to dDesSp. | |
| } | |
| if (DesSp <= 0) { | |
| SofiePlaneLibPrint("Warning", "Takeoff speed is 0m/s, defaulting to 65m/s!"). | |
| set DesSp to 65. | |
| } | |
| TerrainAvoidance off. | |
| brakes off. | |
| HoldVelocity(DesSp, "GND"). | |
| HoldHeading(). | |
| lock DesS to -SofiePlaneLibPIDloops["DesS_PID"]:update(time:seconds, SofiePlaneLibBearingCalculator(body:geopositionof(ship:facing:vector):heading, DesH)). | |
| set DesVV to 0. | |
| set DesP to 0. | |
| set DesR to 0. | |
| if (onRunway <> false) { | |
| lock DesH to Runways[onRunway][1]:heading. | |
| when (vxcl(up:vector, Runways[onRunway][1]:position):mag < 150 | |
| OR ship:airspeed > 0.85 * DesSp | |
| OR (ship:airspeed > 0.6 * DesSp AND vang(Runways[onRunway][1]:position, ship:velocity:surface) > 10)) | |
| then { | |
| // SofiePlaneLibPrint("Debug", "Broke out of DesH hold!"). | |
| set DesH to DesH. | |
| } | |
| } | |
| else { | |
| set DesH to body:geopositionof(ship:facing:vector):heading. | |
| } | |
| when (airspeed > 0.6 * DesSp) then { | |
| // SofiePlaneLibPrint("Debug", "V1, starting rotation."). | |
| if (dMode = "Slope") { | |
| HoldSlope(dTarget). | |
| } | |
| else { | |
| HoldAltitude(dTarget). | |
| } | |
| } | |
| on round (time:seconds * 15) { | |
| if not(isAirborne) { | |
| if (airspeed < min(20, DesSp*0.35)) set ship:control:wheelsteer to -DesS/2. else set ship:control:wheelsteer to 0. | |
| if (ship:status <> "LANDED" AND ship:status <> "PRELAUNCH" AND ship:status <> "SPLASHED" AND SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomaltradar > 10) set isAirborne to true. | |
| } | |
| else { | |
| // SofiePlaneLibPrint("Debug", "airborne else"). | |
| } | |
| return not(isAirborne). | |
| } | |
| on (isAirborne) { | |
| wait 0. | |
| // SofiePlaneLibPrint("Debug", "airborne on trigger"). | |
| steeringManager:resetpids(). | |
| HoldHeading(). | |
| gear off. | |
| set ship:control:wheelsteer to 0. | |
| if (dTA) { | |
| when SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomaltradar > SofiePlaneLibTA_Alt + SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:relmax:mag then | |
| { | |
| TerrainAvoidance on. | |
| SofiePlaneLibPrint("", "Takeoff Complete, re-engaging terrain avoidance"). | |
| } | |
| } | |
| else { | |
| SofiePlaneLibPrint("", "Takeoff Complete"). | |
| } | |
| } | |
| } | |
| function FlyRunwayApproach { | |
| // Function to automatically fly an approach to a runway | |
| parameter targetRunway. | |
| parameter glideSlope is -3. | |
| parameter finalDistance is 5000. | |
| parameter dTA is TerrainAvoidance. | |
| if (not(glideSlope < 0)) { | |
| SofiePlaneLibPrint("Error", "Cannot land with a non-negative glideslope!"). | |
| return. | |
| } | |
| if (not(glideSlope >= -45)) { | |
| SofiePlaneLibPrint("Error", "Cannot land with a glideslope greater than 45 degrees!"). | |
| return. | |
| } | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"] off. | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibGoAround"] off. | |
| wait 0. | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"] on. | |
| local finalPosHeading is circle_bearing(targetRunway[1], targetRunway[0]). | |
| local FlareHeight is (4.6*abs(GlideSlope)). | |
| local timeStep is 0.1. | |
| local height is FlareHeight. | |
| local curTime is 0. | |
| local Dist is 0. | |
| until height < 0 { | |
| local angle is MAX(MIN(MIN(1, arcsin(2 / MAX(airspeed, 2))), ABS(glideSlope)), height^.92 / 4.6). | |
| local vert is airspeed * sin(angle). | |
| local hor is airspeed * cos(angle). | |
| set Dist to dist + hor * timeStep. | |
| set height to height - vert * timeStep. | |
| set curTime to curTime + timeStep. | |
| } | |
| local glideSlopeAimpoint is geoPosition_plus(targetRunway[0], dist, finalPosHeading). | |
| local finalPosition is geoPosition_plus(glideSlopeAimpoint, finalDistance, finalPosHeading). | |
| SofiePlaneLibPrint("Debug", "ToFinalPointHeading: " + finalPosHeading). | |
| SofiePlaneLibPrint("Debug", "FinalPointPosition: " + finalPosition). | |
| SofiePlaneLibPrint("Debug", "final position heading: " + finalPosition:heading). | |
| local turnRadius is SofiePlaneLibTurningRadiusCalculator(). | |
| local rwUp is (targetRunway[0]:position - body:position):normalized. | |
| local refvec is vcrs(targetRunway[1]:position - targetRunway[0]:position, rwUp). | |
| // local centerPointHeading is choose finalPosHeading + 270 if vdot(targetRunway[0]:position, refvec) > 0 else finalPosHeading + 90. | |
| // if centerPointHeading > 360 set alignHeading to alignHeading - 360. | |
| local rwRight is body:geopositionof(-refvec):heading. | |
| local rwLeft is body:geopositionof(refvec):heading. | |
| local isRight is vdot(targetRunway[0]:position, refvec) > 0. | |
| local centerPointHeading is choose rwRight if(isRight) else rwLeft. | |
| local turningCenterPoint is geoPosition_plus(finalPosition, turnRadius, centerPointHeading). | |
| SofiePlaneLibPrint("Debug", "CenterPointHeading: " + centerPointHeading). | |
| SofiePlaneLibPrint("Debug", "turningcenterpoint: " + turningCenterPoint). | |
| SofiePlaneLibPrint("Debug", "heading center point: " + turningCenterPoint:heading). | |
| local c is vang(turningCenterPoint:position - body:position, -body:position)*constant:degtorad*(body:radius). | |
| if (c <= turnRadius) { | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"] off. | |
| SofiePlaneLibPrint("Error", "Cannot find a valid approach from this position and speed." + (choose "Try reducing your speed!" if airspeed > 120 else "")). | |
| return. | |
| } | |
| local theta is arcsin(turnRadius / c). | |
| local beta is 90 - theta. | |
| local headingtoapproachPoint is mod((choose turningCenterPoint:heading + 180 + beta if isRight else turningCenterPoint:heading - 180 - beta) + 360, 360). | |
| local approachPoint is geoPosition_plus(turningCenterPoint, turnRadius, headingtoapproachPoint). | |
| SofiePlaneLibPrint("Debug", "bangle: " + beta). | |
| SofiePlaneLibPrint("Debug", "toapproachpoint: " + headingtoapproachPoint). | |
| SofiePlaneLibPrint("Debug", "app point: " + approachPoint). | |
| SofiePlaneLibPrint("Debug", "heading app point: " + approachPoint:heading). | |
| HoldHeading(). | |
| lock DesH to approachPoint:heading. | |
| local rwHdg is circle_bearing(targetRunway[0], targetRunway[1]). | |
| SofiePlaneLibPrint("", "Flying to runway approach point."). | |
| on round (time:seconds * 2) { | |
| if (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"]) OR vxcl(up:vector, approachPoint:position):mag < airspeed) { SofiePlaneLibPrint("Debug", "Disposing FlyRunwayApproach trigger 0"). return. } | |
| set turnRadius to SofiePlaneLibTurningRadiusCalculator(). | |
| set refvec to vcrs(targetRunway[1]:position - targetRunway[0]:position, rwUp). | |
| set rwRight to body:geopositionof(-refvec):heading. | |
| set rwLeft to body:geopositionof(refvec):heading. | |
| set isRight to vdot(targetRunway[0]:position, refvec) > 0. | |
| set centerPointHeading to choose rwRight if(isRight) else rwLeft. | |
| set finalPosition to geoPosition_plus(glideSlopeAimpoint, finalDistance, finalPosHeading). | |
| set turningCenterPoint to geoPosition_plus(finalPosition, turnRadius, centerPointHeading). | |
| set c to vang(turningCenterPoint:position - body:position, -body:position)*constant:degtorad*(body:radius). | |
| if (c <= turnRadius) { | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"] off. | |
| SofiePlaneLibPrint("Error", "Entered approachpoint turning circle at an incorrect point, breaking off approach."). | |
| set Desh to DesH. | |
| return. | |
| } | |
| set theta to arcsin(turnRadius / c). | |
| set beta to 90 - theta. | |
| set headingtoapproachPoint to mod((choose turningCenterPoint:heading + 180 + beta if isRight else turningCenterPoint:heading - 180 - beta) + 360, 360). | |
| set approachPoint to geoPosition_plus(turningCenterPoint, turnRadius, headingtoapproachPoint). | |
| return SofiePlaneLibPresent. | |
| } | |
| on round(time:seconds * 2) { | |
| if (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"]) OR vxcl(up:vector, glideSlopeAimpoint:position):mag < airspeed) { SofiePlaneLibPrint("Debug", "Disposing FlyRunwayApproach trigger 0_0"). return. } | |
| set height to FlareHeight. | |
| set curTime to 0. | |
| set Dist to 0. | |
| until height < 0 { | |
| local angle is MAX(MIN(MIN(1, arcsin(2 / MAX(airspeed, 2))), ABS(glideSlope)), height^.92 / 4.6). | |
| local vert is airspeed * sin(angle). | |
| local hor is airspeed * cos(angle). | |
| set Dist to dist + hor * timeStep. | |
| set height to height - vert * timeStep. | |
| set curTime to curTime + timeStep. | |
| } | |
| set glideSlopeAimpoint to geoPosition_plus(targetRunway[0], dist/2, finalPosHeading). | |
| return SofiePlaneLibPresent. | |
| } | |
| when (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"]) | |
| OR vxcl(up:vector, approachPoint:position):mag < airspeed) then { | |
| if (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"])) { set TerrainAvoidance to dTA. SofiePlaneLibPrint("Debug", "Disposing FlyRunwayApproach trigger 1"). return. } | |
| if (vxcl(up:vector, approachPoint:position):mag < airspeed) { | |
| SofiePlaneLibPrint("", "Aligning to runway heading"). | |
| lock DesH to GetRunwayAlignHeading(targetRunway). | |
| // lock reqslope to -arctan((SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomalt - targetRunway[0]:terrainheight) / (vang(targetRunway[0]:position - body:position, -body:position)*constant:degtorad * body:radius)). | |
| // lock tmp1 to glideSlope - min(max((glideSlope - reqslope) * 9, glideSlope), -glideSlope). | |
| // lock tmp2 to (abs(rwHdg - targetRunway[0]:heading) < 90 AND vxcl(up:vector, targetRunway[0]:position):mag > DesSp). | |
| // lock tmp4 to vxcl(up:vector, targetRunway[0]:position):mag. | |
| // lock tmp5 to (airspeed * 10 + (SofiePlaneLibTA_Alt * tan(90 - abs(glideSlope)))) * 1.05. | |
| // lock tmp3_0 to MAX(MIN((SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomalt - targetRunway[0]:terrainheight), SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomaltradar), 0)^0.92. | |
| // lock tmp3 to -MAX(MIN(MIN(1, arcsin(2 / MAX(airspeed, 2))), abs(glideSlope)), tmp3_0 / 4.6). | |
| // when not(tmp2 and tmp1 > tmp3) then { | |
| // set FlareStartPosition to ship:geoposition. | |
| // print "flarestart". | |
| // } | |
| // lock DesVV to airspeed * sin(choose tmp1 IF (tmp2 and tmp1 > tmp3) ELSE MAX(glideSlope, tmp3)). | |
| lock DesVV to ({ | |
| local RequiredSlope is -arctan((SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomalt - targetRunway[0]:terrainheight) / (vang(glideSlopeAimpoint:position - body:position, -body:position)*constant:degtorad * body:radius)). | |
| local CorrectionGlideslope is glideSlope - MIN(MAX((glideSlope - RequiredSlope) * 9, glideSlope), -glideSlope). | |
| local FlareCorrectedAlt is MAX(MIN((SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomalt - targetRunway[0]:terrainheight), SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:bottomaltradar), 0)^0.92. | |
| local FlareAngle is -MAX(MIN(MIN(1, arcsin(2 / MAX(airspeed, 2))), ABS(glideSlope)), FlareCorrectedAlt / 4.6). | |
| local InFlare is (ABS(rwHdg - glideSlopeAimpoint:heading) < 90 AND vxcl(up:vector, glideSlopeAimpoint:position):mag > airspeed). | |
| return airspeed * SIN(CHOOSE CorrectionGlideslope IF InFlare AND CorrectionGlideslope > FlareAngle ELSE MAX(glideSlope, FlareAngle)). | |
| }):call(). | |
| // keep on. | |
| // on time:seconds { | |
| // local index is 0. | |
| // print ("DesH: " + round(DesH, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("DesSlope: " + round(DesSlope, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("reqslope: " + round(reqslope, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("tmp1: " + round(tmp1, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("tmp2: " + tmp2):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("tmp3_0: " + round(tmp3_0, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("tmp3: " + round(tmp3, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("tmp4: " + round(tmp4, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("tmp5: " + round(tmp5, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("DesVV: " + round(DesVV, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // print ("NewDesVV: " + round(NewDesVV, 1)):padright(terminal:width) at (0,index). set index to index + 1. | |
| // return keep AND SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"]. | |
| // } | |
| HoldVerticalSpeed(). | |
| TerrainAvoidance On. | |
| when (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"]) OR abs(rwHdg - targetRunway[1]:heading) > 45 OR ship:status = "LANDED" OR ship:status = "SPLASHED" OR SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] <> 0 OR SofiePlaneLibInternalVariablesLex["SofiePlaneLibGoAround"]) then { | |
| if (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"])) { set TerrainAvoidance to dTA. SofiePlaneLibPrint("Debug", "Disposing FlyRunwayApproach trigger 2"). return. } | |
| if (SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] <> 0 AND vxcl(up:vector, glideSlopeAimpoint:position):mag < (airspeed * 10 + (SofiePlaneLibTA_Alt * tan(90 - abs(glideSlope)))) * 1.05 AND not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibGoAround"])) // 10 is TA ahead check, if we hit this and are within 10 seconds of landing + margin disable TA, else go around. | |
| { | |
| if (dTA) SofiePlaneLibPrint("", "Disabling terrain avoidance for landing"). | |
| gear on. | |
| TerrainAvoidance off. | |
| return true. | |
| } | |
| if ((abs(rwHdg - targetRunway[1]:heading) > 45 OR (SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] <> 0 AND TerrainAvoidance)) OR SofiePlaneLibInternalVariablesLex["SofiePlaneLibGoAround"]) { | |
| SofiePlaneLibPrint("", "Missed approach, Going around"). | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibGoAround"] on. | |
| if (abs(rwHdg - targetRunway[0]:heading) > 90) { | |
| // fly remainder of runway before turning | |
| set DesH to DesH. | |
| } | |
| HoldSlope(-glideSlope * 2). | |
| when (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"]) OR (abs(rwHdg - targetRunway[1]:heading) > 90) OR abs(rwHdg - targetRunway[0]:heading) < 90) then { | |
| if (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"])) { set TerrainAvoidance to dTA. SofiePlaneLibPrint("Debug", "Disposing FlyRunwayApproach trigger 3_0"). return. } | |
| set DesH to finalPosHeading. | |
| if (gear) gear off. | |
| when (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"]) OR abs(finalPosHeading - body:geopositionof(ship:velocity:surface):heading) < 3) then { | |
| if (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"])) { set TerrainAvoidance to dTA. SofiePlaneLibPrint("Debug", "Disposing FlyRunwayApproach trigger 3"). return. } | |
| set TerrainAvoidance to dTA. | |
| FlyRunwayApproach(targetRunway, glideSlope, finalDistance, dTA). | |
| } | |
| } | |
| } | |
| else { | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"] off. | |
| SofiePlaneLibPrint("", "Landing confirmed, engaging brakes."). | |
| set TerrainAvoidance to dTA. | |
| brakes on. | |
| lock throttle to 0. | |
| set DesSp to 0. | |
| set DesR to 0. | |
| set DesH to targetRunway[1]:heading. | |
| when (airspeed < 1 OR DesSp <> 0) then { | |
| if (DesSp = 0) { | |
| SofiePlaneLibPrint("", "Wheelstop, unlocking flightcontrols"). | |
| unlock steering. | |
| } | |
| } | |
| } | |
| } | |
| } | |
| } | |
| } | |
| function GoAround { | |
| // Function to command a go around, aborting the landing procedure. | |
| if (not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibFlyRunwayApproach"])) { SofiePlaneLibPrint("Error", "Not on a runway approach, cannot initiate go-around!"). return. } | |
| if (SofiePlaneLibInternalVariablesLex["SofiePlaneLibGoAround"]) { SofiePlaneLibPrint("Error", "Already in a go-around!"). return. } | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibGoAround"] on. | |
| } | |
| function ChangeVelocityMode { | |
| // Function to change the current velocity mode, while keeping DesSp and targets. | |
| parameter mode. | |
| if (mode = "GND" AND SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] <> "GND") { | |
| set DesSp to SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_GND"]. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] to "GND". | |
| } | |
| else if (mode = "IAS" AND SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] <> "IAS") { | |
| set DesSp to SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_IAS"]. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] to "IAS". | |
| } | |
| else if (mode = "Mach" AND SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] <> "Mach") { | |
| set DesSp to SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_Mach"]. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] to "Mach". | |
| } | |
| } | |
| function SofiePlaneLibTerrainAvoidance { | |
| // Function to calculate the closest terrain the script should avoid and a climbrate override to avoid it. | |
| local stepSize is 0.2. | |
| local travel is ship:velocity:surface. | |
| local angVel is ({ local tmp is v(0,0,0). for Item in SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_AngVelList"] set tmp to tmp+Item. return tmp / SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_AngVelList"]:length. }):call. | |
| set angVel to angleAxis(angVel:mag*constant:radtodeg*stepSize, angVel). | |
| local highestterr is 0. | |
| local highestclimb is 0. | |
| local lastVec is v(0,0,0). | |
| for i in range(0, 50) { | |
| // Look 10 seconds into the future, checking for terrain every 0.2 seconds. resulting in 50 checks. | |
| set travel to travel * angVel. | |
| local pos is lastVec + (travel*stepSize). | |
| set lastVec to pos. | |
| local height is max(0, body:geopositionof(pos):terrainheight) + SofiePlaneLibTA_Alt + SofiePlaneLibInternalVariablesLex["SofiePlaneLibBounds"]:relmax:mag. | |
| local dist is vang(pos - body:position, -body:position)*constant:degtorad*(body:radius + altitude). | |
| local climb is choose 0 if dist <= 0 else arcTan((height- altitude)/dist). | |
| if climb > highestclimb { | |
| set highestclimb to climb. | |
| set highestterr to height. | |
| } | |
| } | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Alt"] to highestterr. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] to min(85, highestclimb). | |
| } | |
| function GetRunways { | |
| // Function to fill the Runways lexicon with the runways that have been found for the current body or the body supplied in the parameter. | |
| parameter bd is body. | |
| local path is "0:/json/" + bd:name + ".json". | |
| if (not(exists(path))) { | |
| set Runways to lex(). | |
| SofiePlaneLibPrint("Error", "No json found for body: " + bd:name). | |
| return. | |
| }. | |
| local SofiePlaneLibPlaces is readJson(path). | |
| local SofiePlaneLibNumberedPlaces is ({local tmp is lex(). for k in SofiePlaneLibPlaces:keys if k:substring(k:length-3, 2):tonumber(-100) <> -100 AND k[k:length-3] = " " tmp:add(k, SofiePlaneLibPlaces[k]). return tmp.}):call. | |
| local SofiePlaneLibRunwaySets is ({local tmp is lex(). for k in SofiePlaneLibNumberedPlaces:keys if tmp:haskey(k:substring(0, k:length-3)) set tmp[k:substring(0, k:length-3)] to tmp[k:substring(0, k:length-3)] + 1. else set tmp[k:substring(0, k:length-3)] to 1. return tmp.}):call. | |
| set Runways to lex(). for k in SofiePlaneLibRunwaySets:keys if SofiePlaneLibRunwaySets[k] = 2 { local fittingloclist is ({ local tmp is lex(). for kn in SofiePlaneLibNumberedPlaces:keys if kn:contains(k) tmp:add(kn, SofiePlaneLibNumberedPlaces[kn]). return tmp.}):call. runways:add(fittingloclist:keys[0], list(fittingloclist[fittingloclist:keys[0]], fittingloclist[fittingloclist:keys[1]])). runways:add(fittingloclist:keys[1], list(fittingloclist[fittingloclist:keys[1]], fittingloclist[fittingloclist:keys[0]])).} | |
| } | |
| function GetGroundDistanceToRunway { | |
| // Function to return the closest distance to the centerline of a runway, Full credit to Sug4rj for writing this function for me! | |
| parameter rw. | |
| local rwLength to (rw[1]:position-rw[0]:position):mag. | |
| local localUp to (rw[0]:position+rw[1]:position)/2-body:position. | |
| if vdot(-rw[0]:position, (rw[1]:position-rw[0]:position):normalized)>rwLength return abs(vxcl(localup, rw[1]:position):mag). | |
| if vdot(-rw[1]:position, (rw[0]:position-rw[1]:position):normalized)>rwLength return abs(vxcl(localup, rw[0]:position):mag). | |
| return abs(vdot(vcrs(rw[1]:position-rw[0]:position, localup):normalized, rw[0]:position)). | |
| } | |
| function GetRunwayAlignHeading { | |
| // Function to return the heading required to line up with a runway. | |
| parameter rwy. | |
| local rwyhdg is circle_bearing(rwy[0], rwy[1]). | |
| local rwyendthreshhold is rwy[1]:heading. | |
| local hdgoffset is min(max(SofiePlaneLibBearingCalculator(rwyendthreshhold, rwyhdg) * 3, -180),180). //steer to heading - offset | |
| local steerhdg is 0. | |
| set steerhdg to rwyendthreshhold - hdgoffset. | |
| if steerhdg < 0 { | |
| set steerhdg to steerhdg + 360. | |
| } | |
| else if steerhdg > 360 { | |
| set steerhdg to steerhdg - 360. | |
| } | |
| return steerhdg. | |
| } | |
| function SofiePlaneLibLoop { | |
| // Script Main 15hz loop | |
| on round (time:seconds * 15) { | |
| // Loop that runs 15 times per second to do critical flight control functions | |
| local calcalt is altitude. | |
| local mach1 is (sqrt(body:atm:adbidx*constant:idealgas*body:atm:altitudetemperature(calcalt)/body:atm:molarmass)). | |
| local iasmod is choose 0 if calcalt >= body:atm:height else sqrt((body:atm:altitudepressure(calcalt) / body:atm:altitudetemperature(calcalt))/(kerbin:atm:altitudepressure(0) / kerbin:atm:altitudetemperature(0))). | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["GND"] to airspeed. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["IAS"] to choose 0 if calcalt >= body:atm:height else airspeed * iasmod. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["Mach"] to choose 0 if calcalt >= body:atm:height else airspeed / mach1. | |
| if (SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] = "GND") { | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_GND"] to DesSp. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_IAS"] to choose 0 if calcalt >= body:atm:height else DesSp * iasmod. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_Mach"] to choose 0 if calcalt >= body:atm:height else DesSp / mach1. | |
| } | |
| else if (SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] = "IAS") { | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_GND"] to choose 0 if calcalt >= body:atm:height else DesSp / iasmod. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_IAS"] to DesSp. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_Mach"] to choose 0 if calcalt >= body:atm:height else (DesSp / iasmod) / mach1. | |
| } | |
| else if (SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityMode"] = "Mach") { | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_GND"] to DesSp * mach1. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_IAS"] to (DesSp * mach1) * iasmod. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibVelocityLex"]["T_Mach"] to DesSp. | |
| } | |
| if not(TerrainAvoidance) and not(SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Alt"] = 0 and SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] = 0) { | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Alt"] to 0. | |
| set SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_Climb"] to 0. | |
| } | |
| if (SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_AngVelList"]:length >= 15) SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_AngVelList"]:remove(0). | |
| SofiePlaneLibInternalVariablesLex["SofiePlaneLibTA_AngVelList"]:add(ship:angularvel). | |
| return SofiePlaneLibPresent. | |
| } | |
| // Script Secondary 2hz loop | |
| on round (time:seconds * 2) { | |
| // Secondary loop that runs less critical flight control functions. | |
| if (TerrainAvoidance) { | |
| SofiePlaneLibTerrainAvoidance(). | |
| } | |
| return SofiePlaneLibPresent. | |
| } | |
| } | |
| //------------------------------------------------------------------------------------------------- | |
| // Function Calls Post-Loading | |
| SofiePlaneLibUpdatePIDs(). | |
| SofiePlaneLibLoop(). | |
| GetRunways(). |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment