Plane auto-pitch & roll

//auto pitch & roll for planes. Lock steering as usual, will roll and pitch for you.
// !runscript DbxqrVs5
HUDTEXT("Running script: plane auto-pitch & roll",20,2,22,white,false).
set autoPitch to true.
set autopilot to true.
set ppid to pidloop(2.0,0.7,0,-1,1).
set rpid to pidloop(0.5,0.05,0,-1,1).
set aoalimit to 6.
set maxclimb to 15.
set mul to 3. set limit to 50.
set talt to round(altitude).
set takeoffspeed to 50.
set altNoBank to 20.
set toggledAutopilot to true.

set heightMargin to 100. //minimum altitude above terrain when using keepalt()
set dst to 1.
set terSlope to 0.
set terSlope2 to 0.
set terTime to time:seconds.
set terMax to 12. //amount of seconds ahead to sample terrain.

function disableYaw {
    if ship:status = "flying" {
         set ship:control:yaw to 0.0001.
    } else {
        set ship:control:yaw to 0.
         when ship:status = "flying" then set ship:control:yaw to 0.0001.
    }
}
disableYaw().
when true then {
    if autopilot {
    //pitch
    set vp to vang(up:vector, velocity:surface).
    set sp to max(vp - aoalimit,min(vp + aoalimit,vang(up:vector,steeringmanager:target:vector))).
    set ppid:setpoint to max(-0.1,(vp - sp) / (40 + airspeed / 30)).
    set ship:control:pitch to ppid:update(time:seconds, -vdot(vxcl(up:vector,facing:starvector),ship:angularvel)).

    //roll
    set stv to steeringmanager:target:vector.
    set angE to max(0,vang(vxcl(up:vector,stv),vxcl(up:vector, velocity:surface)) - 0) ^ 0.85.
    if vdot(vxcl(velocity:surface,facing:starvector),stv) < 0 set angE to -angE.
    set tr to choose 0 if alt:radar < altNoBank or (vp - sp < -2) else min(limit,max(-limit,angE * mul)).
    set rpid:setpoint to (tr - (vang(facing:starvector,up:vector) - 90)) / 90.
    set ship:control:roll to rpid:update(time:seconds, vdot(-facing:vector, ship:angularvel)).

    if not(toggledAutopilot)  {
        toggledAutopilot on.
        set ship:control:yaw to 0.0001.
    }
    }
    else if toggledAutopilot {
        set ship:control:pitch to 0. set ship:control:roll to 0. set ship:control:yaw to 0. toggledAutopilot off. steeringmanager:resetpids().
    }
    return autoPitch.
}


function autoThrottle {
    if not(defined sl) set sl to round(airspeed).
        set tpid to pidloop(0.1,0.002,0.2,0,1).
    lock throttle to tpid:update(time:seconds,vdot(facing:vector,ship:velocity:surface) - sl).
        HUDTEXT("Auto-throttle enabled, change 'SL'",15,2,18,yellow,false).
}
function takeoff {
    lock x to round(body:geopositionof(facing:vector * 1000):heading).
    lock p to choose 8 if airspeed > takeoffspeed else (90-vang(up:vector, velocity:surface)). lock steering to heading(x,p).
    lock throttle to 1. if ship:maxthrust = 0 stage.
    disableYaw().
    brakes off.
}
function keepAlt {
    parameter taltIn is round(altitude).
    set talt to taltIn.
    lock p to max(-25,min(maxClimb, (talt-altitude) / (airspeed/8))).
    lock steering to heading(x,max(p, avoidTerrain())).
}

function avoidTerrain {
    set dst to choose 1 if dst > terMax else dst + 0.1.
    set gi to body:geopositionof(velocity:surface * dst).
    set pi to choose gi:position if gi:terrainheight > 0 else gi:altitudeposition(0).

    set tempSlope to 90 - vang(up:vector, pi + up:vector * heightMargin).
    if tempSlope > terSlope {
        set terSlope to tempSlope.
        set terTime to time:seconds.
    }
    else {
        set terSlope2 to max(tempSlope, terSlope2 * 0.999 + 0.001 * tempSlope).
        if time:seconds > terTime + 3 {
            set terTime to time:seconds. set terSlope to terSlope2. set terSlope2 to max(-30, terSlope2 - 25).
        }
    }
    return terSlope.
}

set vesselSettings to lexicon().
if homeconnection:isconnected and exists("0:/json/vesselsettings.json") {
    set vesselSettings to readjson("0:/json/vesselsettings.json").
}

if vesselSettings:haskey(ship:name) {
    print "Found vessel settings in json:".
    set cfg to vesselSettings[ship:name].
    print cfg.
    set ppid:kp to cfg["ppid_kp"].
    set ppid:ki to cfg["ppid_ki"].
    set ppid:kd to cfg["ppid_kd"].
    set rpid:kp to cfg["rpid_kp"].
    set rpid:ki to cfg["rpid_ki"].
    set rpid:kd to cfg["rpid_kd"].
    set maxclimb to cfg["maxclimb"].
    set takeoffspeed to cfg["takeoffspeed"].
    set mul to cfg["mul"].
    set limit to cfg["limit"].
}
function saveSettings {
    set vesselSettings[ship:name] to lexicon(
        "ppid_kp", ppid:kp, "ppid_ki", ppid:ki, "ppid_kd", ppid:kd,
        "rpid_kp", rpid:kp, "rpid_ki", rpid:ki, "rpid_kd", rpid:kd,
        "maxclimb", maxclimb,
        "takeoffspeed", takeoffspeed,
        "mul", mul,
        "limit", limit
    ).
    if (homeconnection:isconnected) {
        print "Saving vessel settings to 0:/json/vesselsettings.json".
        print vesselSettings[ship:name].
        writejson(vesselSettings, "0:/json/vesselsettings.json").
    }
    else {
        HUDTEXT("Error saving: no connection",20,2,22,red,true).
    }
}



set campitch to 8.
set trackCam to false.
set cam to addons:camera:flightcamera.
set shipsize to ship:bounds:size:mag.
when trackCam then {
    set cam:position to angleaxis(campitch, facing:starvector) * (facing:vector * (-2 - shipsize  * 1.25)).
    return true.
}


set menu to gui(180, 30). Set menu:x to -30. set menu:y to 400. menu:show.
menu:addlabel("<b><color=yellow>Auto pitch & roll</color></b>").
set m1 to menu:addlabel("SL (speedlimit):").
set m2 to menu:addlabel("TAlt:").
set m3 to menu:addlabel("Limit:").
set m4 to menu:addlabel("Mul:").
set m5 to menu:addlabel("MaxClimb:").
set m6 to menu:addlabel("Trackcam:").
set mlast to menu:addlabel("Functions: autothrottle(), takeoff(), keepAlt(altitude), savesettings()").

menu:addlabel("<i>'menu:hide'</i> to close").
for txt in menu:widgets { set txt:style:fontsize to 10. set txt:style:padding:v to 1. }

on time:second {
    if (defined sl)
        set m1:text to "SL (speedlimit): <color=white>" + sl + "</color>m/s".
    set m2:text to "TAlt: <color=white>" + talt + "</color>m".
    set m3:text to "Limit: <color=white>" + limit + "</color> max bank".
    set m4:text to "Mul: <color=white>" + mul + "</color>x (roll sensitivity)".
    set m5:text to "MaxClimb: <color=white>" + maxclimb + "</color>" + char(176).
    set m6:text to "Trackcam: <color=white>" + trackcam + "</color>".
    return menu:visible.
}