VTOL hover experimental hathi

//VTOL hover script with side translation - Ozin
// !runscript xeNy4xmv
clearscreen.
core:doaction("open terminal",true).
clearvecdraws().
clearguis().
sas off.
HUDTEXT("Running script: VTOL hover for jets & propellers",20,2,22,white,false).

//===[Setup shitload of vars]=== A lot of these get updated in triggers and functions but need to be delcared here first with initial values.
if not(defined places) and exists("0:/json/" + body:name + ".json") set places to readjson("0:/json/" + body:name + ".json").

if vang(up:vector,facing:vector) > 60 toggle ag2.

set props to false.


set estNeutral to 0.
set tarAOA to 0.

set mathThingy to 2 * constant:pi * 5.8 * 460/60.
function bladeAOA { parameter ang.
    set tarAOA to ang.
    for rotor in rotors {
        set relSpd to vdot(rotor[0]:facing:vector * rotor[1], vel).
        set estNeutral to arctan(abs(relSpd)/mathThingy) * relSpd/abs(relSpd).
        for i in range(2,rotor:length) {
            rotor[i]:setfield("deploy angle",estNeutral + ang).
        }
    }
}

set vtol to true.
set steeringmanager:rollcontrolanglerange to 180.
set steeringmanager:rollpid:kp to 0.4.
set steeringmanager:rollpid:ki to 0.04.
set steeringmanager:rollpid:kd to 0.25.
set steeringmanager:yawpid:kd to 0.15.
set steeringmanager:maxstoppingtime to 6.

set flipSteering to false.
set face to facing.
set bnds to ship:bounds.
set st to lookdirup(up:vector, face:topvector).
lock hdg to 90. lock sl to 0.
set relVelVec to v(0,0,0).
lock tAltOverride to 0.
set terSlope to 0.
set twr to 1.
set grav to 9.
set acc to 10.
lock height to 20.
set hvel to v(0,0,0).
set vel to velocity:surface.
set velOld to vel.
lock terAlt to max(0,ship:geoposition:terrainheight).
lock talt to choose tAltOverride if tAltOverride <> 0 else height + terAlt.
set altErr to altitude - talt.
set tiltOutwards to 0.
lock vsOverride to 0.
set vsMin to -100.
set vsMax to 200.
set vs to 0.
set vsErr to 0.
set stTopVector to face:topvector.
set terI to 1.
set terSteps to 15.
set terStepDist to 0.25.
set terSlope2 to 0.
set terJ to 0.
set mul to 0.
set followDist to 0.
set hdgLag to hdg.
lock tv to heading(hdgLag,0):vector * sl.
set tvCopy to tv.
set tvCopyMag to 0.
set pitchoffset to 0.

if ship:partsnamedpattern("grap"):length > 0 {
    set localport to ship:partsnamedpattern("grap")[0].
} else if ship:dockingports:length > 0 {
    set localport to ship:dockingports[0].
} else {
    set localport to false.
}
set mode to "Heading".
set tgt to false.
set alignTerrainHeight to 10.
set race to false.
when alt:radar > 10 then gear off.


set menu to gui(260, 30). Set menu:x to -30. set menu:y to 300. menu:show.
menu:addlabel("<b><color=yellow>VTOL 2-axis script</color></b>").
set m0 to menu:addlabel("Mode:").
set m1 to menu:addlabel("SL:").
set m2 to menu:addlabel("HDG:").
set m3 to menu:addlabel("Height:").
set m4 to menu:addlabel("tAltOverride:").
set m5 to menu:addlabel("vsOverride:").
set m6 to menu:addlabel("alt:radar:").
set m7 to menu:addlabel("trackCam:").
set m8 to menu:addlabel("alignTerrainHeight:").
set m9 to menu:addlabel("tiltOutwards:").
set m10 to menu:addlabel("Localport:").
set mLast to menu:addlabel("Functions: <color=white>goToTarget(), goToPosition(<geopos or position>), goTo(<Name>), resetSteering()</color>").
set mLast2 to menu:addlabel("Additional vars: <color=white>vsMin, vsMax, sideTiltMult</color>").
set mIPU to menu:addlabel("IPU: ").
set mLast3 to menu:addlabel(".").
set mLast4 to menu:addlabel(".").
set mAOA to menu:addlabel(".").


for txt in menu:widgets { set txt:style:fontsize to 14. set txt:style:padding:v to 1. }
set m3:style:margin:top to 15.
set m5:style:margin:bottom to 15.

wait 1.
//===[Functions]===


function vertThrust {
    if props return max(1, vdot(rotor1:facing:vector, up:vector) * 20).

    set vth to 0.
    for e in engs if e:ignition {
        set vth to vth + vdot(up:vector, e:facing:vector * (choose -1 if e:tag = "reverse" else 1)) * e:availablethrust.
    }
    return max(1,vth).
}
function norm {
    parameter p0 is v(0,0,0).
    set u to (p0-body:position):normalized.
    set p1 to vxcl(u,facing:vector):normalized * (bnds:size:mag).
    set g2 to body:geopositionof(angleaxis(120, u) * p1 + p0). set p2 to choose g2:position if g2:terrainheight > 0 else g2:altitudeposition(0).
    set g3 to body:geopositionof(angleaxis(-120, u) * p1 + p0). set p3 to choose g3:position if g3:terrainheight > 0 else g3:altitudeposition(0).
    set g1 to body:geopositionof(p1 + p0). set p1 to choose g1:position if g1:terrainheight > 0 else g1:altitudeposition(0).
    return vcrs(p1-p2,p1-p3):normalized.
}
function getSlope {
    parameter pos, vec.
    local g1 is body:geopositionof(pos). local p1 is choose g1:position if g1:terrainheight > 0 else g1:altitudeposition(0).
    local g2 is body:geopositionof(pos+vec). local p2 is choose g2:position if g2:terrainheight > 0 else g2:altitudeposition(0).
    return 90 - vang(up:vector, p2-p1).
}

set terDisplay to false.

function showVD {
    toggle terDisplay.
    set vdTer to vecdraw(v(0,0,0), up:vector * 1, red, "", 1, true, 0.9).
    set vdSlope to vecdraw(v(0,0,0), up:vector * 1, yellow, "", 1, true, 0.4).
    set vdVel to vecdraw(v(0,0,0), up:vector * 1, blue, "", 1, true, 0.4).
    set vdAcc to vecdraw(v(0,0,0), up:vector * 1, green, "", 1, true, 0.3).
}
function terDisplayFunc {
    set vdTer:start to pi.
    set vdTer:vec to (angleaxis(terSlope, vcrs(hvel, up:vector)) * up:vector) * height.
    set vdTer:width to 1 + (pi:mag^0.75) / 80.

    set vdSlope:vec to angleaxis(terSlope, vcrs(tvCopy, up:vector)) * tvCopy / 5.
    set vdVel:vec to vel / 5.

    set velDiff to vel-velOld.
    set vdAcc:vec to (velDiff:normalized * (velDiff:mag/0.02)) / 10.
    set velOld to vel.
}


function tiltStar {
    parameter tlt is 0.
    if vsErr < -1 set tlt to tlt + tlt * max(-1, vsErr / 8).
    set tlt to tlt + vang(up:vector,  face:starvector) - 90.
    set rollError to steeringmanager:rollpid:output. // rollerror
    for hinge in servosSideTilt hinge["s"]:setfield("target angle", tlt * hinge["direction"] * hinge["right"] + tiltOutwards + min(20, max(-20,(rollError * 40) * hinge["front"] * hinge["right"])) ).
}
function tilt {
    parameter tlt is 0.
    //if vsErr < -1  and (vdot(hvel:normalized, relVelVec) > -2 or vdot(tvCopy, hvel) < 0) or race set tlt to tlt + tlt * max(-1, vsErr / 10).
    if vsErr < -1  set tlt to tlt + tlt * max(-1, vsErr / 10).
    set curPitch to choose (vang(up:vector,  face:topvector) - 90) if vdot(face:vector, up:vector) >= 0 else (90 + vang(up:vector, -facing:topvector)).
    set tlt to tlt + curPitch.
    if tlt > 180 set tlt to tlt - 360.
    set mLast4:text to "pitch tilt: " + round(tlt,2).
    for s in servos s["s"]:setfield("target angle", tlt).
    //if tlt > 45 {
    //  for s in servos s["s"]:setfield("target angle", tlt + s["front"] * steeringmanager:pitcherror / -2).
    //} else {
    //  for s in servos s["s"]:setfield("target angle", tlt).
    //}
}
function goToTarget {
    if localport:istype("part") {
        lock tv to choose (vxcl(up:vector,(target:position + target:facing:vector * -followDist) - localport:position):normalized * min(sl,(vxcl(up:vector,(target:position + target:facing:vector * -followDist) - localport:position):mag/3)^0.75) + (choose target:velocity:surface if target:loaded else v(0,0,0))) if hastarget else v(0,0,0).

    } else {
        lock tv to choose (vxcl(up:vector,target:position + target:facing:vector * -followDist):normalized * min(sl,(vxcl(up:vector,target:position + target:facing:vector * -followDist):mag/3)^0.75) + (choose target:velocity:surface if target:loaded else v(0,0,0))) if hastarget else v(0,0,0).
    }
    set mode to "Target".
    if sl < 1 { set sl to 5. HUDTEXT("SL (speedlimit) is low, setting SL to 5..",16,2,20,yellow,false). }
}
function goToPosition {
    parameter pos.
    if pos:typename = "Vector" set pos to body:geopositionof(pos).
    set tgt to pos.
    lock tv to vxcl(up:vector,tgt:position):normalized * min(sl,(vxcl(up:vector,tgt:position):mag/3)^0.70) - (choose localport:position if localport:istype("part") else v(0,0,0)).
    set mode to "GeoPos".
    if sl < 1 { set sl to 5. HUDTEXT("SL (speedlimit) is low, setting SL to 5..",16,2,20,yellow,false). }
}
function goto {
    parameter where.
    if not(defined places) { print "ERROR: places json not loaded.". }
    else if places:haskey(where) { gotoposition(places[where]). }
    else { print char(7)+ "ERROR: no known place: " + where. }
}
function resetSteering { lock tv to heading(hdgLag,0):vector * sl. lock steering to st. set mode to "Heading". set keepCamSteer to false. }

function powerdown {
    for s in ship:modulesnamed("ModuleRoboticServoRotor") s:setfield("torque limit(%)", 0).
    on height powerup().
}
function powerup {
    if props {
        set t0 to time:seconds.
        when true then {
            set tPercent to min(100,(time:seconds - t0) * 33).
            for s in ship:modulesnamed("ModuleRoboticServoRotor") s:setfield("torque limit(%)", tPercent).
            return tPercent < 100.
        }
    }
}

//===[engine & servos setup]===
set engs to list().
list engines in engs2. for e in engs2 if e:tag <> "aft" engs:add(e).
set allServos to ship:modulesnamed("ModuleRoboticRotationServo").
set servos to list().
set yawServos to list().
set servosSideTilt to list().
for s in allServos {

    if s:part:tag = "yaw" {
        yawServos:add(s).
    } else if abs(vdot(facing:starvector, s:part:facing:vector)) > 0.5 {
        local sLex is lexicon("direction", 1, "right", choose 1 if vdot(face:starvector, s:part:position) > 0 else -1, "front", choose 1 if vdot(face:topvector, s:part:position) < 0 else -1, "s", s).
        servos:add(sLex).
    } else if abs(vdot(facing:topvector, s:part:facing:vector)) > 0.5 {
        local sLex is lexicon("direction", choose -1 if vdot(facing:topvector, s:part:facing:vector) > 0 else 1,"right", choose 1 if vdot(face:starvector, s:part:position) > 0 else -1, "front", choose 1 if vdot(face:topvector, s:part:position) < 0 else -1, "s", s).
        servosSideTilt:add(sLex).
    }
}


for m in ship:modulesnamed("ModuleRoboticServoHinge") {
    if m:part:tag = "enginePitch" {
        local pitchEngLex is lexicon("right", 0, "front", 0, "s", m).
        servos:add(pitchEngLex).
    }
    else if m:part:tag <> "ignore" {
        local pPos is m:part:position.
        local l is lexicon("direction", 1, "right", choose 1 if vdot(face:starvector, pPos) > 0 else -1, "front", choose 1 if vdot(face:topvector, pPos) < 0 else -1, "s", m).
        if not (props) and engs:length <= 2 set l["front"] to 0.
        servosSideTilt:add(l).
    }
}
for l in servos { l["s"]:setfield("damping", 100). l["s"]:setfield("traverse rate", 120). }
for l in servosSideTilt { l["s"]:setfield("damping", 50). l["s"]:setfield("traverse rate", 120). }

set twoaxistilt to (servosSideTilt:length > 1).
if twoaxistilt HUDTEXT("2-axis engine tilting enabled. Toggle 'twoaxistilt' to disable",20,2,22,yellow,false).
set sideTiltMult to choose 0.50 if twoaxistilt else 1.
set jets to false.
for e in engs if e:sealevelisp > 3000 jets on.

//===[PID controllers]===
if props {
    set aoaPID to pidloop(1.5, 0, 0, -2, 5).
} else if jets {
    set tpid to pidloop(8,0.2, 9, -0.5, 4).
} else {
    set tpid to pidloop(2,0.2, 0.0, -1, 4).
}
set vsPID to pidloop(1,1,1,-50,100).
set tltpid to pidloop(20, 2, 2, -35, 75).
set tltStarpid to pidloop(20, 2, 2, -35, 35).

when sl >= 200 then if ship:availablethrust = 0 stage.


wait 1.

//===[TRIGGER WARNING!]===
//5hz trigger

if not props {
    set curThrust to vertThrust().
    on round(time:seconds * 5) {
        set grav to body:mu/body:position:sqrmagnitude.
        set curThrust to min(curThrust * 1.05,vertThrust()).
        set twr to (curThrust/mass)/grav.
        set acc to max(0.1, curThrust/mass - grav).
        set tpid:maxoutput to twr - 1.
        set tpid:minoutput to (choose -0.5 if altErr > -1 else -1).
        return vtol.
    }
}
//fast (50hz) trigger
when true then {
    set face to choose facing + r(0,0,180) if flipSteering else facing.
    set vel to velocity:surface.
    set hvel to vxcl(up:vector,vel).
    //if hdgLag <> hdg {
    //  set hdgLag to choose round(hdgLag-1) if sin(hdg-hdgLag)<0 else round(hdgLag+1).
    //}
    set hdgLag to hdg.

    set tvCopy to tv.
    set tvCopyMag to tvCopy:mag.
    set altErr to altitude - (choose talt if (mode <> "Target") else target:altitude).

    //[Terrain detection and minimum climb angle]
    if tAltOverride = 0 and vsOverride = 0 {
        set gi to body:geopositionof((hvel * (1.5 - terI/terSteps) + tvCopy * terI/terSteps):normalized * (groundspeed + (tvCopyMag + groundspeed)/2 * (terI^1.4) * terStepDist)).
        set pi to choose gi:position if gi:terrainheight > 0 else gi:altitudeposition(0).
        set tempSlope to max(-60,90 - vang(up:vector, pi + (angleaxis(terSlope, vcrs(hvel, up:vector)) * up:vector) * min(alt:radar + height * terI/terSteps,height))).
        if tempSlope > terSlope {
            set terSlope to tempSlope.
            set terJ to 0.
        } else {
            set terJ to terJ + 1.
            set terSlope2 to max(tempSlope, terSlope2 * 0.999 + 0.001 * tempSlope).
            if terJ > terSteps * 3 + 30 { set terJ to 0. set terSlope to terSlope2. set terSlope2 to max(-30, terSlope2 - 25).  }
        }
        if terDisplay terDisplayFunc().
        set terI to terI + terStepDist.
        if terI > terSteps {
            set terI to terStepDist.
            //if terSlope > 0 set terSlope to max(terSlope, getSlope(vel:normalized * (sl * terSteps), vel:normalized * (sl * 2))).
        }
    }

    //[Target vertical speed & throttle]
    set slopeVS to choose vsMin if (tAltOverride <> 0 or groundspeed < 3) else (tan(terSlope) * (groundspeed * 1.1 + (choose 0 if altErr < 0 else max(0,20 - groundspeed^1.5)))).
    set vs to choose vsOverride if vsOverride <> 0 else max(max(vsMin,slopeVS)  ,min(vsMax,       choose (-1 * (0.7 * acc * max(altErr + min(0, verticalspeed * 1) ,0.0001))^0.4) if altErr > 0 else sqrt(2 * (grav/2) * max(-altErr - verticalspeed * 0.2,0.0001))         )).
    set vsErr to verticalspeed - vs.
    if props {
        bladeAOA(aoaPID:update(time:seconds, vsErr)).
    } else {
        set mul to 1 + tpid:update(time:seconds,vsErr).
        if vsErr < -10 tPid:reset().
    }
    set relVelVec to vxcl(up:vector, min(1,  max(0, 1 + (2 + vsErr) / 20)) * tvCopy - vel) / 4.
    set relVelVec:mag to min(5,relVelVec:mag).

    for s in yawServos s:setfield("target angle", steeringmanager:yawpid:output * 20).
    return vtol.
}

//12hz trigger - steering and engine tilt
on round(time:seconds * 12) {
    set backDir to vxcl(up:vector, choose (face:topvector) if vdot(up:vector, face:vector) >= 0 else (-face:topvector)):normalized.
    tilt(tltpid:update(time:seconds, vdot(backDir, relVelVec))).

    set stTopVector to choose tvCopy * (choose 1 if flipSteering else -1) if tvCopyMag > 2 else (choose -target:facing:topvector if hastarget else face:topvector).
    if twoaxistilt {
        tiltStar(tltStarpid:update(time:seconds, vdot(face:starvector, relVelVec))).
        set st to lookdirup(angleaxis(choose 0 if alt:radar < alignTerrainHeight or race else (min(tvCopyMag/2,max(-tvCopyMag/2,terSlope - vsErr/6))), -face:starvector) * (vel / -500 + (choose norm() if alt:radar < alignTerrainHeight else up:vector) * 10 + vxcl(vxcl(up:vector,face:topvector),relVelVec * sideTiltMult )), stTopVector) * angleaxis(pitchoffset, face:starvector).
    } else {
        set st to lookdirup( (vel / -500 + vxcl(vcrs(face:topvector, up:vector), (choose norm() if alt:radar < alignTerrainHeight else up:vector)):normalized * 10 + vxcl(vxcl(up:vector,face:topvector),relVelVec * sideTiltMult )), stTopVector).
    }

    set tltpid:minoutput to min(-10,-55 + airspeed / 3).
    //set tltStarpid:minoutput to min(-10,-45 + airspeed / 3).
    //set tltStarpid:maxoutput to max(10,45 - airspeed / 3).
    return vtol.
}

// Aft engine(s) trigger
set aftEngs to ship:partstagged("aft").
if aftEngs:length > 0 {
    on round(time:seconds * 13) {
        for eng in aftEngs {
            set eng:thrustlimit to choose 0 if tvCopyMag < 200 or vsErr < -20 or vdot(eng:facing:vector,relVelVec) < -2 else vdot(eng:facing:vector, tvCopy-vel) * 30.
        }
        return vtol.
    }
}
wait 1.1.

//===[Camera, GUI, and other]===

if not(lights) lights on.
set trackCam to false.
set pitch to 15.
lock co to v(0,0,0).
set cam to addons:camera:flightcamera.
set camDist to bnds:size:mag * 1.0.
when trackCam then {
    set cam:position to cam:position * 0.8 + 0.2 * (co + angleaxis(pitch, face:starvector) * (-vel / 50 + (-relVelVec/10 + face:topvector):normalized * camDist)).
    return vtol.
}


on time:second {
    set m0:text to "Mode: <color=yellow>" + mode + "</color>".
    set m1:text to "SL: <color=white>" + round(sl,2) + "m/s</color> (" + round(tvCopyMag,1) + ")".
    set m2:text to "HDG: <color=white>" + round(hdg,2) + "</color>".

    set m3:text to (choose "<color=#222>Height: </color>" if (vsOverride <> 0 or tAltOverride <> 0) else "Height: ") + "<color=white>" + height + "</color> (priority #3)".
    set m4:text to (choose "<color=#222>tAltOverride: </color>" if (vsOverride <> 0 or tAltOverride = 0) else "tAltOverride: ")+ "<color=white>" + tAltOverride + "</color> (priority #2 if <> 0)".
    set m5:text to (choose "<color=#222>vsOverride: </color>" if (vsOverride = 0)  else "vsOverride: ") + "<color=white>" + vsOverride + "</color> (priority #1 if <> 0)".

    set m7:text to "TrackCam: <color=white>" + trackCam + "</color>".
    set m8:text to "alignTerrainHeight: <color=white>" + alignTerrainHeight + "m</color> (match slope if height below this value)".
    set m9:text to "tiltOutwards: <color=white>" + tiltOutwards + "</color>".
    set m10:text to "Localport: <color=white>" + localport + "</color>".

    set mIPU:text to "IPU: " + round(core:getfield("kOS average power") / 0.0002).
    return menu:visible.
}
when true then {
    set m6:text to "Alt:radar: " + round(alt:radar,2) + "m".
    set mLast3:text to "vs err: " + round(vsErr,3) + ", cur: " + round(verticalspeed,3).
    //set mLast4:text to "mul: " + round(mul,2) + ", tpid:iterm: " + round(tpid:iterm,4).

    if props set mAOA:text to "neutAng: " + round(estNeutral,1) + "  AOA: " + round(tarAOA,1).
    return menu:visible.
}
on vtol menu:dispose.
//clearscreen.
print "VTOL hover script running. See GUI for available variables and functions.".

lock steering to st.
if not (props) lock throttle to mul/twr.

if defined(places) {
    function showPlaces {
        set placesGUI to gui(180,50).
        for p in places:keys {
            set txt to placesgui:addlabel(p + " (<color=white>" + round(places[p]:distance/1000,1) + "km</color>)").
            set txt:style:fontsize to 9. set txt:style:padding:v to 0.
        }
        placesGUI:addlabel("<color=white>placesGUI:dispose</color> to close").
        Set placesGUI:x to -250.
        set placesGUI:y to 10.
        set placesGUI:style:bg to "".
        placesGUI:show.
    }
}

set keepCamSteer to false.
set camDist to ship:bounds:furthestcorner(facing:topvector):mag * 2.
function camSteer {
    set keepCamSteer to true.
    trackcam off.
    set cam:distance to camDist.
    set cam:pitch to 10.
    when true then {
        set hdg to cam:heading.
        set sl to max(0,round(sl + (camDist - cam:distance ) * 10)).
        set cam:distance to camDist.
        return keepCamSteer.
    }
}

function land {
    set keepCamSteer to false. resetsteering.
    set sl to 0. set hdg to round(body:geopositionof(velocity:surface:normalized * 100):heading). set height to min(20, height).
    set vsOverride to 0. set tAltOverride to 0.
    when groundspeed < 2 then {
        set sl to 0.
        set height to 0.6.
        on ship:status powerdown().
    }
}

powerup().