Starhopper 10M Hop

clearscreen.
on ag10 {
    aborttest().
}

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


function prep {
    print "Welcome to Starhopper Test Software V4!".
    wait 2.
    print "Running hardware, preparings for software...".
    wait 5.
    set teststatus to "Preparations".
    wait until teststatus = "Preparations".
    wait 1.
    set goal to "Hop".
    set h to alt:radar.
    lock altit to alt:radar-h.
    set targetalt to 10+altit.
    set northPole to latlng(90,0).
    set head to mod(360-northpole:bearing,360).
    set landingZone to latlng(-5.46634004,-54.0389159).
    lock steering to heading(0,90).
    set lenght to 2.
    set eng to ship:partstagged("Vector1")[0].
    eng:activate.
    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 {
    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 ag3.
    toggle ag2.
    toggle ag1.
    wait 1.
    toggle ag9.
    print "Fueling finished, press 9".
    wait 1.
    until ag9 {
        printing().
    }

    set teststatus to goal + " go".
}

function hop {
    set maxthr to ship:maxthrust.
    set g to body:mu/(body:radius)^2.
    set twr to ship:mass * g / ship:availablethrust.
    lock curthr to eng:thrust.
    wait 1.
    rcs on.
    sas off.
    lock steering to heading(0,90).
    lock throttle to 0.1.
    toggle ag2.
    toggle ag3.
    toggle ag1.
    wait 1.5.
    set n to time:seconds.
    lock t to time:seconds - n.
    eng:activate.
    lock throttle to min(1, t/2.5).
    print "Test began".
    wait until altit>0.8.
    lock latoff to (landingZone:lat-addons:tr:impactpos:lat)*10472.
    until ship:verticalspeed > 1.25 {
        hopprinting().
    }

    lock throttle to twr * 0.994.
    lock steering to heading(0,p).
    until latoff < 3 {
        hopprinting().
    }

    lock steering to heading(0,90).
    until altit >= targetalt {
        hopprinting().
    }

    lock throttle to twr * 0.92.
    lock steering to heading(0,p).
    until ship:verticalspeed < -1.2 {
        hopprinting().
    }

    lock throttle to twr * 0.996.
    until altit < 3 {
        hopprinting().
    }

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

    lock throttle to twr*0.996.
    until altit<0.15 {
        hopprinting().
    }

    set latoff to 0.
    lock throttle to 0.1.
    print "Test completed".
    wait 4.
    lock throttle to 0.
    eng:shutdown.
    set teststatus to "Completed".
}

function hopprinting {
    clearscreen.
    set twr to ship:mass * g / ship:availablethrust.
    print "Test lenght: " + t + " Seconds".
    print "Current throttle: " + curthr + "kn".
    print "Maximum throttle: " + maxthr + "kn".
    steer().
    print ang.
    print latoff.
    log t + ";" + maxthr + ";" + curthr + ";" + altit + ";" + latoff + ";" + p to starhopper10hop.txt.
    wait 0.01.
}

function detanking {
    print "Test status: " + teststatus.
    ag2 on.
    ag3 on.
    ag1 on.
    wait 1.
    toggle ag9.
    unlock steering.
    unlock throttle.
    wait 300.
    toggle ag2.
    toggle ag3.
    toggle ag1.
    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.
    print "Abort".
    eng:shutdown.
    lock throttle to 0.
    wait 1.
    unlock throttle.
    unlock steering.
    set eng2 to ship:partstagged("s")[0].
    eng2:activate.
    set t to 5.
    set s to 0.
    until s>t {
      lock throttle to 1.
      wait 1.
      lock throttle to 0.
      set s to s+1.
      wait 1.
    }

    eng2:shutdown.
    wait 1.
    set teststatus to "Detanking".
    detanking().
}

function steer {
    wait 0.
    set pid to pidloop(1, 1, 1, -1,1).
    set pid:setpoint to 0.
    set ang to pid:update(time:seconds,latoff).
    if ang < 0 {
        set p to 90-abs(ang).
    } else if ang > 0 {
        set p to ang+90.
    }
}