VTOL Experimental Clone for Props

//VTOL hover script with side translation - Ozin
//hacked and likely broken - sumguy
// !runscript N8eMd50c
switch to 0.
clearvecdraws().
HUDTEXT("Running script: VTOL hover for props",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.


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 bnds to ship:bounds.
set st to  lookdirup(up:vector, facing:topvector).
lock steering to st.
lock hdg to 90.
lock sl to 0.
set relVelVec to v(0,0,0).
lock tAltOverride to 0.
set terSlope to 0.

set grav to 9.
set acc to 10.
lock height to 20.
set hvel to v(0,0,0).
set vel to velocity:surface.
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 facing: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 1.
lock tv to heading(hdg,0):vector * sl.
set tvCopy to tv.

lock deploy to max(3,airspeed^0.7).
lock rpmlimit to max(0,(-2*verticalspeed - altitude + talt -altErr*0.6)*4.6*2).
set blades to ship:partsdubbedpattern("blade").

set rotors to ship:partsdubbedpattern("rotor").

for rotor in rotors rotor:getmodulebyindex(1):setfield("torque limit(%)",100).

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.


//===[Functions]===

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 + 5).
    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).
}
function showVD { toggle terDisplay. set vdTer to vecdraw(v(0,0,0), up:vector * 1, red, "", 1, true, 0.9, false). set vdSlope to vecdraw(v(0,0,0), up:vector * 1, yellow, "", 1, true, 0.4, true). set vdVel to vecdraw(v(0,0,0), up:vector * 1, blue, "", 1, true, 0.4, true). }
set terDisplay to false.
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.9) / 80.

    set vdSlope:vec to angleaxis(terSlope, vcrs(tvCopy, up:vector)) * tvCopy / 5.
    set vdVel:vec to vel / 5.
}
function tiltStar {
    parameter tlt is 0.
    if vsErr < -1 set tlt to tlt + tlt * max(-1, vsErr / 15).
    set tlt to tlt + vang(up:vector,  facing:starvector) - 90.
    set rollError to steeringmanager:rollpid:output.
    for hinge in allHinges hinge["s"]:setfield("target angle", tlt * hinge["right"] + tiltOutwards + min(15, max(-15,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) set tlt to tlt + tlt * max(-1, vsErr / 20).
    set tlt to tlt + vang(up:vector,  facing:topvector) - 90.
    for s in servos s:setfield("target angle", tlt).
}
function goToTarget {
    if localport:istype("part") {
        lock tv to choose (vxcl(up:vector,target:position - localport:position):normalized * min(sl,(vxcl(up:vector,target:position - localport:position):mag/3)^0.70) + (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):normalized * min(sl,(vxcl(up:vector,target:position):mag/3)^0.70) + (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 resetSteering { lock tv to heading(hdg,0):vector * sl. lock steering to st. set mode to "Heading". }


//===[engine & servos setup]===

set allServos to ship:modulesnamed("ModuleRoboticRotationServo").
set servos to list().
set yawServos to list().
for s in allServos {
    if s:part:tag <> "yaw" {
        s:setfield("damping", 80). s:setfield("traverse rate", 80). servos:add(s).
    } else {
        yawServos:add(s).
    }
}
set allHinges to list().
for m in ship:modulesnamed("ModuleRoboticServoHinge") {
    m:setfield("damping", 80). m:setfield("traverse rate", 80).
    local pPos is m:part:position.
    local l is lexicon("right", choose 1 if vdot(facing:starvector, pPos) > 0 else -1, "front", choose 1 if vdot(facing:topvector, pPos) < 0 else -1, "s", m).

    allHinges:add(l).
}
set twoaxistilt to (allHinges: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.

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


//===[TRIGGER WARNING!]===
//5hz trigger
on round(time:seconds * 5) {


    return vtol.
}
//fast (50hz) trigger
when true then {
    set vel to velocity:surface.
    set hvel to vxcl(up:vector,vel).
    set tvCopy to tv.
    set tvCopyMag to tvCopy:mag.
    set altErr to altitude - talt.

    //[Terrain detection and minimum climb angle]
    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 0 if tAltOverride else (tan(terSlope) * (groundspeed + (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 * (1 * 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.

    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).


    For b in blades b:getmodule("ModuleControlSurface"):setfield("deploy angle", deploy).
    for rotor in rotors rotor:getmodulebyindex(1):setfield("rpm limit", rpmlimit).

    return vtol.
}

//12hz trigger - steering and engine tilt
on round(time:seconds * 12) {
    tilt(tltpid:update(time:seconds, vdot(facing:topvector, relVelVec))).

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


//===[Camera, GUI, and other]===
if vang(up:vector,facing:vector) > 60 toggle ag2.

if not(lights) lights on.
set trackCam to true.
set pitch to 12.
lock co to v(0,0,0).
set cam to addons:camera:flightcamera.
when trackCam then {
    set cam:position to cam:position * 0.9 + 0.1 * (co + angleaxis(pitch, facing:starvector) * (-vel / 50 + (-relVelVec/10 + facing:topvector):normalized * bnds:size:mag * 1.2)).
    return vtol.
}

clearguis().
set menu to gui(200, 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>), resetSteering()</color>").
set mLast2 to menu:addlabel("Additional vars: <color=white>vsMin, vsMax, sideTiltMult</color>").
set mLast3 to menu:addlabel("").
set mLast4 to menu:addlabel("").

for txt in menu:widgets { set txt:style:fontsize to 10. set txt:style:padding:v to 1. }
set m3:style:margin:top to 15.
set m5:style:margin:bottom to 15.
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>".
    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).
    return menu:visible.
}
on vtol menu:dispose.
clearscreen.
print "VTOL hover script running. See GUI for available variables and functions.".

set places to readjson("kerbin.json").
function gotoplace { parameter placename is "helipad". set dest to places[placename]. lock taltoverride to max(dest:terrainheight + 15, ship:geoposition:terrainheight + 15). gotoposition(dest). }.