SN8 6.25KM 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 "6.25KM Hop".
    set h to alt:radar.
    lock altit to alt:radar-h.
    lock myvel to ship:velocity:surface:mag.
    set targetalt to 6250.
    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.
    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).
    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 5.
}

function hop {
    on ag10 {
       aborttest().
    }

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

    lock throttle to twr * 1.16.
    lock steering to up + r(0,0,270).
    until altit > 200 {
        hopprinting().
    }

    lock throttle to twr * 1.1.
    lock steering to up + r(0,0,270).
    until altit > 200 {
        hopprinting().
    }

    lock throttle to twr * 1.06.
    lock steering to up + r(0,0,270).
    until altit > 350 {
        hopprinting().
    }

    lock throttle to twr * 1.04.
    lock steering to up + r(0,0,270).
    until altit > 560 {
        hopprinting().
    }

    lock throttle to twr * 1.03.
    lock steering to up + r(0,-0.2,270).
    until altit > 800 {
        hopprinting().
    }

    lock throttle to twr * 1.02.
    lock steering to up + r(0,-0.5,270).
    until altit > 2200 {
        hopprinting().
    }

    eng2:shutdown.
    set thr to 1559*0.33.
    set twr to ship:mass * g / eng1:availablethrust+eng3:availablethrust.
    lock throttle to twr * 0.96.
    lock steering to up + r(0.7, 0.4, 270).
    wait 2.
    lock throttle to twr * 1.04.
    until altit > 4100 {
        hopprinting().
    }

    lock steering to up + r(0.7,0.4,270).
    until altit > 5450 {
        hopprinting().
    }

    eng3:shutdown.
    set thr to 1559*0.66.
    set twr to ship:mass * g / eng1:thrust.
    lock steering to up + r(-0.1,3,270).
    lock throttle to twr * 0.98.
    until ship:verticalspeed < 10 {
        hopprinting().
    }

    lock throttle to twr * 0.99.
    until altit > targetalt-100 {
        hopprinting().
    }

    lock throttle to twr * 0.9.
    until ship:verticalspeed < 5 {
        hopprinting().
    }

    print "SN8 is in target altitude".
    until ship:verticalspeed < 0.1 {
        hopprinting().
    }

    ag6 on.
    lock throttle to twr * 0.85.
    print "SN8 is in apoapsis".
    until ship:verticalspeed < -24 {
        hopprinting().
    }

    lock throttle to twr * 0.99.
    until altit < 5800 {
        hopprinting().
    }

    rcs on.
    set l to 2.
    lock throttle to max(0, 0.9-t/2).
    lock steering to up + r(0,-90,270).
    toggle ag4.
    toggle ag5.
    wait 3.
    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.
    ag6 off.
    eng1:shutdown.
    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).
    ag6 off.
    lock steering to up + r(0,steerroll1,270+steerpitch1).
    wait 3.
    toggle ag4.
    toggle ag5.
    until altit < stopDist+150 {
        hopprinting().
    }

    set r5:enabled to true.
    set r6:enabled to true.
    set r7:enabled to true.
    set r8:enabled to true.
    eng2:activate.
    eng3:activate.
    lock throttle to min(0.55, t/0.3).
    toggle ag4.
    toggle ag5.
    wait 0.5.
    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).
    wait 0.3.
    lock aoa to -7.
    lock steering to srfretrograde.
    until vang(ship:facing:vector, -ship:velocity:surface)<20 {
        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 < 100 {
        hopprinting().
    }

    eng3:shutdown.
    lock throttle to twr*0.85.
    lock steering to srfretrograde.
    until ship:verticalspeed > -12 {
        hopprinting().
    }

    gears on.
    lock steering to up + r(0,0,270).
     until ship:verticalspeed > -1 {
        hopprinting().
    }
    ag4 off.
    ag5 off.
    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 {
        set thr to 1040.
    } else if l = 3 or l = 1 {
        set thr to ship:availablethrust.
    }

    set twr to ship:mass * g / thr.
    print "Test lenght: " + t + " Seconds".
    print "Current throttle: " + curthr + "kn".
    print "Maximum throttle: " + maxthr + "kn".
    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.2, 0.08, 0.1, -20,20).
    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(80,40,40, -15,15).
    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.
}