IFT-3 REENTRY AND LANDING(STEERING FUNCTIONS ARE DISABLED CAUSE OF REAL FLIGHT PROFILE)

clearscreen.
set h to 25.
set altit to alt:radar - h.
set now to time:seconds - 1190.
lock testtime to time:seconds - now.
set teststatus to "Transfer back complete, awaiting for PEZ door to close, T+: " + round(testtime,1) + " Seconds".
prep().
reentryprep().
reentry().
descending().
landing().


function prep {
    set h1 to ship:partsdubbed("h1")[0].
    set h2 to ship:partsdubbed("h2")[0].
    set h3 to ship:partsdubbed("h3")[0].
    set h4 to ship:partsdubbed("h4")[0].
    set lockstatus to 1.
    hingeslock().
    wait 1.
    set lockstatus to 0.
    hingeslock().
    set hts to ship:partsnamed("externalTankRound").
    set rs1 to hts[0].
    set rs2 to hts[1].
    set rs3 to rs1:resources.
    set rs4 to rs2:resources.
    set rs5 to rs3[0].
    set rs6 to rs3[1].
    set rs7 to rs4[0].
    set rs8 to rs4[1].
    set rs5:enabled to false.
    set rs6:enabled to false.
    set rs7:enabled to false.
    set rs8:enabled to false.
    set eng1 to ship:partstagged("VectorS1")[0].
    set eng2 to ship:partstagged("VectorS2")[0].
    set eng3 to ship:partstagged("VectorS3")[0].
    set steeringManager:maxstoppingtime to 0.02.
    set landingZone to impactpos().
    set reentered to 0.
    set descend to 0.
    set landingburn to 0.
}


function reentryprep {
    until altit < 75000 {
        printing().
    }

    set tophingeleftang to 15.
    set tophingerightang to 15.
    set afthingesang to -35.
    hingesmove().
    set P1 to 0.15.
    set P2 to 0.1.
    set I to 0.
    set D to 0.05.
    set minpitch to 70.
    set maxpitch to 85.
    set minroll to -30.
    set maxroll to 30.
    descendcontrol().
    set rs5:enabled to true.
    set rs6:enabled to true.
    set rs7:enabled to true.
    set rs8:enabled to true.
    rcs on.
    set teststatus to "Flaps are configured for reentry, orienting for reentry, T+: " + round(testtime,1) + " Seconds".
    until altit < 70000 {
        printing().
    }

    set teststatus to "Starship has reentered the atmosphere, T+: " + round(testtime,1) + " Seconds".
    set reentered to 1.
}


function reentry {
    set oldq to ship:q.
    wait 0.1.
    set newq to ship:q.
    set steeringManager:maxstoppingtime to 0.1.
    set lockstatus to 1.
    hingeslock().
    until newq < oldq and altit < 35000 {
        set oldq to ship:q.
        wait 0.001.
        set newq to ship:q.
        printing().
        descendcontrol().
    }

    set teststatus to "Max-Q, awaiting for landing burn, " + round(newq*constant:atmtokpa,3) + " kPa, T+: " + round(testtime,1) + " Seconds".
}


function descendcontrol {
    pitchpid().
    rollpid().
}


function pitchpid {
    set pid1 to pidloop(P1, I, D, minpitch,maxpitch).
    set pid1:setpoint to 0.
    set pitchang to pid1:update(time:seconds, lngoff).
    return pitchang.
}


function rollpid {
    set pid2 to pidloop(P2, I, D, minroll,maxroll).
    set pid2:setpoint to 0.
    set rollang to pid2:update(time:seconds, latoff).
    return rollang.
}


function descending {
    set minpitch to -50.
    set maxpitch to 50.
    set minroll to -70.
    set maxroll to 70.
    set P1 to 0.03.
    set P2 to 0.12.
    set I to 0.
    set D to 0.01.
    set latoff to 0.
    set lngoff to 0.
    descendcontrol().
    set steeringManager:maxstoppingtime to 1.
    set steeringManager:rollts to 0.5.
    set steeringManager:pitchts to 0.5.
    set descend to 1.
    eng1:activate.
    eng2:activate.
    eng3:activate.
    set rollang to 0.
    set engnum to 3.
    lock steering to heading(90,pitchang,rollang).
    until altit < altitudecalc(engnum) + 250 and altit < 1500 {
        printing().
        descendcontrol().
        altitudecalc(engnum).
    }

    eng1:shutdown.
    eng2:shutdown.
    eng3:shutdown.
}

function landing {
    set teststatus to "Landing burn starting, T+: " + round(testtime,1) + " Seconds".
    set steeringManager:maxstoppingtime to 0.15.
    eng3:activate.
    set lockstatus to 0.
    hingeslock().
    set reentered to 0.
    set now1 to time:seconds.
    lock ti1 to time:seconds - now1.
    set afthingesang to -90.
    hingesmove().
    set topvector to facing:topvector.
    lock steering to lookdirup(-ship:velocity:surface,topvector).
    lock throttle to min(0.5, ti1/1).
    wait 0.2.
    eng2:activate.
    wait 0.2.
    eng1:activate.
    set tophingerightang to 15.
    set tophingeleftang to 15.
    hingesmove().
    until vang(ship:facing:vector, -velocity:surface) < 4 {
        printing().
    }

    set aoa to -5.
    set tophingerightang to -35.
    set tophingeleftang to -35.
    set afthingesang to -35.
    hingesmove().
    lock throttle to 1.
    lock steering to Steer().
    set teststatus to "All sea level engines are lit, T+: " + round(testtime,1) + " Seconds".
    set engnum to 1.
    until altitudecalc(engnum) < altit {
        printing().
        altitudecalc(engnum).
    }

    set aoa to -3.
    eng3:shutdown.
    wait 0.2.
    eng2:shutdown.
    thrustcalc().
    set landingburn to 1.
    lock throttle to thrustcalc().
    until altit < 100 {
        printing().
    }

    lock steering to lookdirup(-ship:velocity:surface,topvector).
    until altit < 40 {
        printing().
    }

    lock steering to lookdirup(up:vector,topvector).
    until ship:status = "Landed" or ship:status = "Splashed" {
        printing().
    }

    set teststatus to "Touchdown confirmed".
    set now1 to time:seconds.
    lock ti1 to time:seconds - now1.
    lock throttle to max(0.05, 1-(ti1/0.5*1)).
    set landingburn to 0.
    set descend to 0.
    set reentered to 0.
    until ti1 > 1 {
        printing().
    }

    lock throttle to 0.
    eng1:shutdown.
    eng2:shutdown.
    eng3:shutdown.
    wait 0.1.
    unlock steering.
    unlock throttle.
    set lockstatus to 1.
    hingeslock().
    rcs off.
    set ship:control:pilotmainthrottle to 0.
    set teststatus to "Ship has landed".
    until false {
        printing().
    }
}


function hingeslock {
    h1:getmodule("ModuleRoboticServoHinge"):setfield("Locked", lockstatus).
    h2:getmodule("ModuleRoboticServoHinge"):setfield("Locked", lockstatus).
    h3:getmodule("ModuleRoboticServoHinge"):setfield("Locked", lockstatus).
    h4:getmodule("ModuleRoboticServoHinge"):setfield("Locked", lockstatus).
}


function hingesmove {
    h1:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", tophingeleftang).
    h2:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", tophingerightang).
    h3:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", afthingesang).
    h4:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", afthingesang).
}


function printing {
    clearscreen.
    set altit to alt:radar - h.
    set lngoff to (landingZone:lng - impactpos:lng)*10472.
    set latoff to (landingZone:lat - impactpos:lat)*10472.
    print "Status: " + teststatus.
    print "T+: " + round(testtime,1) + " Seconds".
    print "Longitude difference: " + round(lngoff,2) + " Meters".
    print "Latitude difference: " + round(latoff,2) + " Meters".
    print "Time(KSC): " + time:clock.
    if reentered = 1 {
       print "Targeted pitch is " + round(pitchang,2) + " Degrees".
       print "Targeted roll is " + round(rollang,2) + " Degrees".
    }

    if descend = 1 {
        print "Landing Burn Altitude : " + round(LBAlt,2) + " Meters".
    }

    if landingburn = 1 {
        set shipthrust to throttle.
        print "Current thrust percentage: " + round(shipthrust*100,2) + "%".
        print "Targeted thrust percentage: " + round(Thr*100,2) + "%".
    }


    print "Press 10 to abort program".
    wait 0.001.
}


function altitudecalc {
    parameter engnum.
    set grav to constant:g * body:mass / body:radius^2.
    set maxAccel to (engnum*555.35 / ship:mass) - grav.
    set LBAlt to ship:verticalspeed^2 / (2 * maxAccel).
    return abs(LBAlt).
}


function thrustcalc {
    set Thr to altitudecalc(engnum) / altit.
    return Thr.
}


function errorvector {
    set errorvec to Impactpos():position - landingZone:position.
    return errorvec.
}


function Steer {
 set result to -velocity:surface + errorvector().

 if vang(result, -velocity:surface) > aoa
 {
   set result to -velocity:surface:normalized + tan(aoa)*errorvector():normalized.
 }

 return lookdirup(result, topvector).
}


function impactpos {
    if addons:tr:hasimpact {
    return addons:tr:impactpos.
  } else {
    return ship:geoposition.
  }
}