land.ks

@lazyglobal off.

function gravMag {
    parameter alt is altitude,bdy is body.
    return bdy:mu/(bdy:radius+alt)^2.
}

function gravVec {
    parameter self is ship, alt is self:altitude.
    return gravMag(alt,self:body)*-self:up:vector:normalized.
}

function fuelFlow {
    parameter self is ship, a is self:altitude.
    local p is self:body:atm:altitudePressure(a).
    local m is 0.
    local engs is 0.
    list engines in engs.
    for eng in engs {
        set m to m+eng:availableThrust/eng:ispAt(p).
    }
    return m/9.80665.
}

function thrustVec {
    parameter self is ship, a is self:altitude.
    local p is self:body:atm:altitudePressure(a).
    local t is V(0,0,0).
    local engs is 0.
    list engines in engs.
    for eng in engs {
        set t to t+(eng:availableThrustAt(p)*eng:facing:vector:normalized).
    }
    return t.
}

local hVec is vecDraw(V(0,0,0),V(0,0,0),purple,"HOR",1,true,0.2).

function constrainSteering {
    parameter vec, maxGamma is 20.

    local vHat is ship:up:vector:normalized.
    local vAccV is vHat*vDot(vHat,vec).

    local hHat is vCrs(vCrs(vHat,vec),vHat):normalized.
    local hDot is vDot(hHat,vec).
    local hSign is hDot/abs(hDot).
    local hAccV is hSign*hHat*min(abs(hDot),vAccV:mag*tan(maxGamma)).
    print "hDot: "+round(hDot,2)+"    hAcc: "+round(hAccv:mag,2).
    set hVec:vec to hAccV.
    return vAccV+hAccV.
}

function constrainSteering2 {
    parameter vec, maxGamma is 20.

    local vHat is ship:up:vector:normalized.
    local vAccV is vHat*vDot(vHat,vec).

    local hHat is vCrs(vCrs(vHat,vec),vHat):normalized.
    local hDot is vDot(hHat,vec).
    local hSign is hDot/abs(hDot).
    local hAccV is hSign*hHat*vAccV:mag*tan(maxGamma).
    print "hDot: "+round(hDot,2)+"    hAcc: "+round(hAccv:mag,2).
    set hVec:vec to hAccV.
    return vAccV+hAccV.
}

function fallTime {
    parameter r is ship:altitude, x is 0, b is ship:body.
    local xr is (x+b:radius)/(r+b:radius).
    local xr2 is sqrt(xr).
    return (r+b:radius)^1.5*(constant:degToRad*arccos(xr2)+xr2*sqrt(1-xr))/sqrt(2*b:mu).
}

function dragVecInit {
    return Lex("v",ship:velocity:surface,"t",time:seconds,"a",V(0,0,0)).
}

function accelVec {
    parameter args.
    local lastT is args["t"].
    if lastT<>time:seconds{
        //local tV is throttle*thrustVec().
        //local gV is ship:mass*gravVec().
        local aV is ship:mass*(ship:velocity:surface-args["v"])/(time:seconds-lastT).
        set args["v"] to ship:velocity:surface.
        set args["t"] to time:seconds.
        set args["a"] to aV.//-tV-gV.
    }
    return args["a"].
}

function dragVec {
    parameter args.
    local lastT is args["t"].
    if lastT<>time:seconds{
        local tV is throttle*thrustVec().
        local gV is ship:mass*gravVec().
        local aV is ship:mass*(ship:velocity:surface-args["v"])/(time:seconds-lastT).
        set args["v"] to ship:velocity:surface.
        set args["t"] to time:seconds.
        set args["d"] to aV-tV-gV.
    }
    return args["d"].
}

function sbHeight {
    parameter dragVecArgs.
    local m is mass.
    local h is ship:altitude.
    local v is airSpeed.
    local drag is dragVec(dragVecArgs):mag.
    if 1{
        local n is 10.
        local deccelUnit is airSpeed/n.
        from{local i is 0.}until i=n step{set i to i+1.}do{
            local th is thrustVec(ship,h):mag/m.
            local d is drag/m.
            local g is gravMag(h).
            local a is th-g+d.
            local t is deccelUnit/a.
            local s is v*t-a*t^2/2.
            //print i+"    h: "+round(h,1)+"    t: "+round(t,1)+"    s: " + round(s,1)+"    m: "+round(m,1).
            set m to m-t*fuelFlow(ship,h).
            set h to h-s.
            set v to v-deccelUnit.
        }
    }else{
        until v<0{
            local tVec is thrustVec(ship,h).
            local g is gravMag(h).
            local a is tVec:mag/m-g.
            local s is v-a/2.
            //print "    h: "+round(h,1)+"    s: " + round(s,1)+"    m: "+round(m,1).
            set m to m-fuelFlow(ship,h).
            set h to h-s.
            set v to v-a.
        }
    }
    print round(drag/mass/9.80665,2)+"    "+round(h).
    return h-ship:geoPosition:terrainHeight.
}

function steerTo {
    parameter vec.
    local vel is ship:velocity:surface.
    return -(angleAxis(5*vAng(vel,vec),vCrs(vel,vec))*vel).
}

clearVecDraws().

local launchPad is LatLng(-0.097207933678136,-74.557757572037).
local landingPad is LatLng(-0.0967218940930756,-74.6173570891441).
local landingZone is launchPad.

local posVec is vecDraw(V(0,0,0),V(0,0,0),red,"POS",1,true,0.2).
local velVec is vecDraw(V(0,0,0),V(0,0,0),blue,"VEL",1,true,0.2).

function drawVecs {
    set posVec:vec to landingZone:position.
    set velVec:vec to ship:velocity:surface.
    //print vAng(posVec:vec,velVec:vec).
    print posVec:vec:mag*sin(vAng(posVec:vec,velVec:vec)).
    wait 0.
}

wait until verticalSpeed<0.
lock steering to lookDirUp(-ship:velocity:surface,heading(90,-45):vector).
//lock steering to lookDirUp(-ship:velocity:surface,heading(90,-45):vector).
//wait until altitude<69000.
until altitude<25000 drawVecs().
lock throttle to 1.
until airSpeed<500 drawVecs().
lock throttle to 0.
wait 0.1.

//local landingZone is landingPad.

lock steering to lookDirUp(steerTo(landingZone:position),heading(90,-45):vector).
local accVecArgs is dragVecInit().
until vDot(up:vector,accelVec(accVecArgs))>0 drawVecs.

local lock altRadar to altitude-body:geopositionOf(ship:position):terrainHeight.
local lock goal to airSpeed^2/altRadar/2.
local lock grav to body:mu/body:radius^2.
local lock thst to ship:availableThrust/mass.
local lock throttleGoal to(goal+grav)/thst.
//local lock throttleGoal to .
print throttleGoal.
//wait until throttleGoal>0.99.
//until goal*mass/thrustVec():mag>0.99{
until altRadar<5000 and throttleGoal>0.99 drawVecs().
//print 1/0.

//local dragVecArgs is dragVecInit().
//wait until sbHeight(dragVecArgs)<100.

//local pid is PIDLoop(0.8, 0.4, 0.2).
//set pid:minOutput to 0.
//set pid:maxOutput to 100.
//set pid:setPoint to 100.
//lock throttle to pid:output/100.

//until (altitude-ship:geoPosition:terrainHeight)<1250{
//    local tgtPos is landingZone:altitudePosition(landingZone:terrainHeight+1000).
//    //local tgtVel is sqrt(tgtPos:mag)*tgtPos:normalized.
//    local tgtVel is airSpeed/2*tgtPos:normalized.
//    local tgtAcc is tgtVel-ship:velocity:surface.
//    local tAccV is thrustVec()/ship:mass.
//    local gAccV is gravVec().
//    local accV is constrainSteering(tgtAcc-gAccV).
//    lock steering to lookDirUp(accV,heading(270,-45):vector).
//    pid:update(time:seconds,sbHeight(dragVecArgs)).
//    wait 0.
//}

local accVec is vecDraw(V(0,0,0),V(0,0,0),green,"ACC",1,true,0.2).

//wait until altitude/10<airSpeed.
lock throttle to throttleGoal.
until airSpeed<200 drawVecs().

function descentDir {
    local tgt is landingZone:position.
    local vel is ship:velocity:surface.
    return -(angleAxis(2*vAng(vel,tgt),vCrs(tgt,vel))*vel).
}

lock steering to lookDirUp(descentDir(),heading(90,-45):vector).
until altRadar<100 drawVecs().
until status="LANDED"{
    local tgtPos is landingZone:position.
    local tgtVel is sqrt(tgtPos:mag)*tgtPos:normalized.
    local tgtAcc is tgtVel-ship:velocity:surface.
    local tAccV is thrustVec()/ship:mass.
    local gAccV is gravVec().
    local accV is constrainSteering(tgtAcc-gAccV).
    lock steering to lookDirUp(accV,heading(90,-45):vector).
    local x is vDot(tAccV,accV).
    if x<>0 lock throttle to accV:vec:sqrMagnitude/x.
    set accVec:vec to tgtAcc.
    drawVecs().
}

lock throttle to 0.
lock steering to lookDirUp(ship:up:vector,heading(90,-45):vector).
wait 30.
unlock steering.
rcs off.