SN9 5KM Hop

clearscreen.
on ag10 {
    aborttest().
}

prep().
fuel().
hop().
detanking().


function prep {
    on ag10 {
       aborttest().
    }

    print "Welcome to Starship Test Software V4!".
    print "Running hardware, preparings for software...".
    wait 1.
    set teststatus to "Preparations".
    wait until teststatus = "Preparations".
    set goal to "5KM Hop".
    lock throttle to 0.
    set h to alt:radar.
    lock altit to alt:radar-h.
    lock myvel to ship:velocity:surface:mag.
    set targetalt to 5000.
    set landingZone to latlng(-5.4655,-84.0333).
    set errorScaling to 1.
    lock steering to heading(90,90).
    set hts to ship:partsnamed("externalTankRound").
    set r1 to hts[0].
    set r2 to hts[1].
    set r3 to r1:resources.
    set r4 to r2:resources.
    set r5 to r3[0].
    set r6 to r3[1].
    set r7 to r4[0].
    set r8 to r4[1].
    set r5:enabled to true.
    set r6:enabled to true.
    set r7:enabled to true.
    set r8:enabled to true.
    set partlist to ship:partsnamed("hinge.01").
    set eng1 to ship:partstagged("Vector1")[0].
    set eng2 to ship:partstagged("Vector2")[0].
    set eng3 to ship:partstagged("Vector3")[0].
    eng1:activate.
    eng2:activate.
    eng3:activate.
    lock g to constant:g * body:mass / body:radius^2.
    set thr to 1040.
    clearVecDraws().
    print "Goal: " + goal.
    print "Press 9".
    wait 1.
    wait until ag9.
    set now to time:seconds.
    lock testtime to time:seconds - now.
    wait 1.
    toggle ag9.
    set teststatus to "Fueling".
    print "Go for test".
}

function fuel {
    on ag10 {
       aborttest().
    }

    print "Beginning fueling".
    wait 1.
    print "Awaiting for fueling end, press 9 to end".
    until ag9 {
        printing().
    }

    set teststatus to goal + " preparing".
    wait 1.
    toggle ag1.
    toggle ag2.
    toggle ag3.
    wait 1.
    toggle ag9.
    print "Fueling finished, press 9".
    wait 1.
    until ag9 {
        printing().
    }

    set teststatus to goal + " go".
    toggle ag1.
    toggle ag2.
    toggle ag3.
    set r5:enabled to false.
    set r6:enabled to false.
    set r7:enabled to false.
    set r8:enabled to false.
    wait 4.
}

function hop {
    on ag10 {
       aborttest().
    }

    set l to 1.
    lock curthr to eng1:thrust + eng2:thrust + eng3:thrust.
    set east to vcrs(ship:up:vector, ship:north:vector).
    toggle ag3.
    wait 2.
    rcs off.
    sas off.
    lock throttle to 0.05.
    toggle ag3.
    eng1:activate.
    eng2:activate.
    eng3:activate.
    lock maxDecel to (thr / ship:mass) - g.
    lock stopDist to ship:verticalspeed^2 / (2 * maxDecel).
    lock idealThrottle to stopDist / altit.
    lock impactTime to altit / abs(ship:verticalspeed).
    set maxthr to ship:maxthrust.
    set g to body:mu/(body:radius)^2.
    set thr to 1559.
    set twr to thr/(ship:mass*g).
    wait 1.
    set n to time:seconds.
    lock t to time:seconds - n.
    set desiredtwr to 1.15.
    set thrvalue to desiredtwr*ship:mass*g/thr.
    lock throttle to min(thrvalue, t/3).
    print twr.
    print "Test began".
    lock steering to up + r(0,0,270).
    wait until altit>4.
    lock latoff to (landingZone:lat-addons:tr:impactpos:lat)*10472.
    lock lngoff to (landingZone:lng-addons:tr:impactpos:lng)*10472.
    set desiredtwr to 1.08.
    lock throttle to thrvalue.
    until altit > 140 {
        hopprinting().
    }

    set desiredtwr to 1.07.
    until altit > 270 {
        hopprinting().
    }

    set desiredtwr to 1.06.
    until altit > 420 {
        hopprinting().
    }

    set desiredtwr to 1.05.
    until altit > 615 {
        hopprinting().
    }

    set desiredtwr to 1.03.
    lock steering to up + r(0,-0.2,270).
    until altit > 1780 {
        hopprinting().
    }

    eng2:shutdown.
    set thr to 1559*0.33.
    set twr to thr/(ship:mass*g).
    set desiredtwr to 0.96.
    set vec1 to eng1:facing:vector+eng3:facing:vector.
    set vec2 to vec1:normalized*(eng1:thrust+eng2:thrust).
    set thrvec to v(0,0,0)+vec2.
    set tvec to vdot(thrvec,up:vector)*up:vector-vxcl(up:vector,thrvec).
    lock steering to lookdirup(tvec,east).
    wait 2.
    set desiredtwr to 1.06.
    until altit > 4450 {
        hopprinting().
    }

    eng3:shutdown.
    set vec1 to eng1:facing:vector.
    set vec2 to vec1:normalized*eng1:thrust.
    set thrvec to v(0,0,0)+vec2.
    set tvec to vdot(thrvec,up:vector)*up:vector-vxcl(up:vector,thrvec).
    lock steering to lookdirup(tvec,east).
    set thr to 1559*0.66.
    set twr to eng1:thrust/(ship:mass*g).
    set desiredtwr to 0.97.
    until ship:verticalspeed < 10 {
        hopprinting().
    }

    set desiredtwr to 1.
    until altit > targetalt {
        hopprinting().
    }

    set desiredtwr to 0.92.
    until ship:verticalspeed < 5 {
        hopprinting().
    }

    set desiredtwr to 0.88.
    print "SN9 is in target altitude".
    toggle ag6.
    until ship:verticalspeed < 0.1 {
        hopprinting().
    }

    rcs on.
    set l to 2.
    set n to time:seconds.
    lock t to time:seconds - n.
    set desiredtwr to 1.08.
    lock throttle to max(0, thrvalue-t/2).
    lock steering to up + r(0,-90,270).
    toggle ag4.
    toggle ag5.
    wait 2.
    partlist[3]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -20).
    partlist[2]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -20).
    partlist[0]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", 15).
    partlist[1]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", 15).
    wait 3.
    toggle ag6.
    eng1:shutdown.
    partlist[3]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -35).
    partlist[2]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -35).
    partlist[0]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -35).
    partlist[1]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -35).
    wait 4.
    toggle ag4.
    toggle ag5.
    lock steering to up + r(0,steerroll1,270+steerpitch1).
    until altit < stopDist+100 {
        hopprinting().
    }

    set r5:enabled to true.
    set r6:enabled to true.
    set r7:enabled to true.
    set r8:enabled to true.
    set l to 4.
    toggle ag5.
    toggle ag4.
    wait 0.01.
    partlist[3]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -90).
    partlist[2]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -90).
    partlist[0]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", 15).
    partlist[1]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", 15).
    eng2:activate.
    lock throttle to min(0.55, t/0.9).
    wait 0.7.
    eng3:activate.
    lock steering to srfretrograde.
    wait 0.3.
    lock aoa to -10.
    wait 0.5.
    eng3:shutdown.
    wait 0.4.
    eng3:activate.
    wait 0.5.
    eng3:shutdown.
    until vang(ship:facing:vector, -ship:velocity:surface)<15 {
        hopprinting().
    }

    partlist[3]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -15).
    partlist[2]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -15).
    partlist[0]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -15).
    partlist[1]:getmodule("ModuleRoboticServoHinge"):setfield("Target Angle", -15).
    lock throttle to idealThrottle.
    lock steering to getSteering().
    until altit < 350 {
        hopprinting().
    }

    lock steering to up + r(0.5,36,285).
    wait 3.
    unlock steering.
    until ship:verticalspeed > -12 {
        hopprinting().
    }

    gear on.
    until ship:verticalspeed < -6 {
        hopprinting().
    }

    lock steering to up + r(0,0,270).
    until ship:verticalspeed > -1 {
        hopprinting().
    }

    set latoff to 0.
    set lngoff to 0.
    lock throttle to 0.1.
    print "Test completed".
    wait 3.5.
    lock throttle to 0.
    eng1:shutdown.
    eng2:shutdown.
    eng3:shutdown.
    set teststatus to "Completed".
    detanking().
}

function hopprinting {
    clearscreen.
    if l = 2 or l = 4 {
        set thr to 1040.
    } else if l = 1 {
        set thr to ship:availablethrust.
    }

    set thrvalue to desiredtwr*ship:mass*g/thr.
    set twr to thr / (ship:mass*g).
    print "Test lenght: " + t + " Seconds".
    print "Current throttle: " + curthr + "kn".
    print "Maximum throttle: " + maxthr + "kn".
    print "TWR: " + twr.
    print "Mass: " + ship:mass + "tons".
    print "Gravity: " + g.
    if l=2 {
      steerroll1().
      steerpitch1().
    }

    print "Latoff: " + latoff + "M".
    print "Lngoff: " + lngoff + "M".
    print "Landing Burn Alt: " + stopDist + "M".
    log t + ";" + maxthr + ";" + curthr + ";" + altit + ";" + latoff + ";" + lngoff to starhopper75hop.txt.
}

function detanking {
    print "Test status: " + teststatus.
    ag2 on.
    ag3 on.
    ag1 on.
    wait 1.
    toggle ag9.
    unlock steering.
    unlock throttle.
    wait 300.
    ag1 off.
    ag2 off.
    ag3 off.
    print 3/0.
}

function printing {
    clearscreen.
    print "Time from test beginning: " + testtime + " Seconds".
    print "Test status: " + teststatus.
    print "Goal: " + goal.
    print "Time(KSC): " + time:clock.
    wait 0.1.
}

function aborttest {
    lock latoff to 0.
    lock lngoff to 0.
    print "Abort".
    eng1:shutdown.
    eng2:shutdown.
    eng3:shutdown.
    ag1 on.
    lock throttle to 0.
    wait 1.
    unlock throttle.
    unlock steering.
    wait 1.
    set teststatus to "Detanking".
    detanking().
}

function steerroll1 {
    wait 0.
    set pid to pidloop(0.08, 0.08, 0.08, -13,13).
    set pid:setpoint to 0.
    set ang to pid:update(time:seconds,lngoff).
    if ang < 0 {
        set y to ang+(-90).
    } else if ang > 0 {
        set y to ang-90.
    }

    return y.
}

function steerpitch1 {
    wait 0.
    set pid to pidloop(0.05,0.06,0.03, -18,18).
    set pid:setpoint to 0.
    set pit to pid:update(time:seconds,latoff).
    if pit < 0 {
        set p to abs(pit).
    } else if pit > 0 {
        set p to -pit.
    }

    return p.
}

function errorVector {
 return getImpact():position - landingZone:position.
}

function getSteering {
 local errorVector is errorVector().
 local velVector is -ship:velocity:surface.
 local result is velVector + errorVector*errorScaling.

 if vang(result, velVector) > aoa
 {
   set result to velVector:normalized + tan(aoa)*errorVector:normalized.
 }

 return lookdirup(result, facing:topvector).
}

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