Project Launch and Rendezvous Autopilot for Greg Burch's LSTS in Lua

indy91 · Oct 28, 2012

Hey everyone,

this is something I wanted to do for a long time and finally finished (kind of). I am quite new to the forum but a long time user of Orbiter. I also am from Germany and I hope my english is ok.

This is a launch and rendezvous autopilot for Greg Burch's Medium Utility Lander (LSTS2204). The Scenario is a launch into a lunar orbit and Rendezvous with the Gilgamesh transporter (LSTS2012). Everything is very much inspired by the Apollo 11 Lunar Ascent Profile.
At the moment the working features are:

-Waiting for the right launch time (Based on overflight and time at the moment)
-Launch the Lander into a 60 kft x 45 NM Orbit (I am using the Saturn V Launch Vehicle Guidance)
-Doing a slight adjustment of the Apolune
-Coelliptic Sequence Initiation (CSI): Raising the Perilune to 45 NM
-Planar Correction
-Constant Delta Height (CDH): Adjustment of the Apolune; getting the eccentricity to a minimum
-Waiting for the Terminal Phase Initiation (TPI)
-Controlling the attitude of the spacecraft so that it points to the target
-doing a 25 ft/sec burn along the line-of-sight
-performing two Mid Course Corrections (MCC1 and MCC2). I am using Lambert targeting for this.
-The Lander should be now on a collision course to the transporter

Missing features:
-Better attitude control of the spacecraft. At the moment I am using modified versions of the attctrl.lua that is coming with Orbiter. It doesn't work yet at high pitch angles, so I only let the spacecraft hold the line-of-sight until the TPI. I will have a look at control laws and quaternions and all that stuff later.
-Braking: It actually is already implemented, but it is only working with the line-of-sight attitude control.
-Many minor things: Launch time calculation, better control over the RCS...

Future plans:
-Cleaning up the code
-Landing Autopilot

I will release this asap, but first I want to write some kind of documentation.

Have a good day,

Niklas

Interceptor · Oct 28, 2012

Looks Like a great idea,will this be compatable with Greg Burch's Heavy lander,also?

indy91 · Oct 30, 2012

Interceptor said:
Looks Like a great idea,will this be compatable with Greg Burch's Heavy lander,also?

Shouldn't make to much work. Just some minor modifications and it should work for both.

I uploaded the Autopilot to Orbithangar yesterday, but it is still not published.

Mandella · Oct 30, 2012

Interesting.. I'll be checking this one out.

Interceptor · Oct 30, 2012

Thanks, Looking foward to this.

indy91 · Oct 31, 2012

Here it is: [ame="http://orbithangar.com/searchid.php?ID=5899"]Launch and Rendezvous Autopilot for Greg Burch's LSTS in Lua[/ame]. I would love to hear some feedback and if it is working at all for you.

If you look at the scripts, keep in mind I'm an aerospace engineering student and not a programmer. So it probably looks like a mess and there are no comments^^.

I also am a bit lost in angles and coordinate systems. There is no good way to get the orientation of the spacecraft as shown in the Orbit HUD in Lua. I can only get the direction with v:get_progradedir(). The functions like v:get_pitch() return the angles with respect to the local horizon.

indy91 · Nov 6, 2012

I updated the Autopilot. It is now working for the Heavy Lander, too. Also I added Braking and holding the Line-of-Sight while MCC and Braking.

Interceptor · Nov 6, 2012

indy91,thanks,your one of the good ones, love your AP,I use it all the time for cargo operations back,and forth between the lunar bases,and orbit,can't wait for the new version. :cheers:

indy91 · Nov 6, 2012

That is really great to hear

So you used the autopilot with other scenarios? I didn't really try that. How does it work? And what would you like to be added or improved?

Interceptor · Nov 6, 2012

all I did was use the scenario you supplied,and change LSTS2012,to the Arrow Freighter,or what ever ship you want to dock with in the scenario. Scenario

Code:

BEGIN_DESC

END_DESC

BEGIN_ENVIRONMENT
  System Sol
  Date MJD 78994.4134120933
END_ENVIRONMENT

BEGIN_FOCUS
  Ship LSTS2204
END_FOCUS

BEGIN_CAMERA
  TARGET LSTS2204
  MODE Cockpit
  FOV 50.00
END_CAMERA

BEGIN_HUD
  TYPE Surface
END_HUD

BEGIN_MFD Left
  TYPE Orbit
  PROJ Ship
  FRAME Ecliptic
  ALT
  REF Moon
END_MFD

BEGIN_MFD Right
  TYPE Map
  REF Moon
  POS 0.00 0.00
END_MFD

BEGIN_SHIPS
Arrow-01:UCGO\UCGOArrowFreighter
  STATUS Orbiting Moon
  RPOS -182843.84 0.79 -1840158.65
  RVEL 1620.303 0.002 -161.003
  AROT -0.00 84.12 -0.00
  AFCMODE 7
  PRPLEVEL 0:0.999875 1:1.000000
  IDS 0:320 100 1:540 100
  NAVFREQ 0 0
  XPDR 300
  O2_RESERVE 100.00
  SAVEVAR00 0§0,1§0.00,2§0,3§0.00,4§0,5§0.00,6§0,7§1,8§0.00,9§1,10§0.44,11§11.00,12§1,13§2,14§2,15§0.00,16§0,17§0,18§0,19§1,20§4,21§0,22§1,
  UMMUCREW Capt-Peter_Falcon-41-65-74
  UMMUCREW Eng-Fanny_Gorgeous-27-67-55
  UMMUCREW Doc-George_HealGood-15-70-45
  UMMUCREW -Albert_Jr_Falcon-15-70-45
  UCGO @@0,1,0,0,@@1,1,0,0,@@2,1,0,0,@@3,1,0,0,@@4,1,0,0,@@5,1,0,0,@@6,1,0,0,@@7,1,0,0,
  UCGO @@8,1,0,0,@@9,1,0,0,@@10,1,0,0,@@11,1,0,0,@@12,1,0,0,@@13,1,0,0,@@14,1,0,0,
  UCGO @@15,1,0,0,@@16,1,0,0,@@17,1,0,0,@@18,1,0,0,@@19,1,0,0,@@20,1,0,0,@@21,1,0,0,
  UCGO @@22,1,0,0,@@23,1,0,0,@@24,1,0,0,@@25,1,0,0,@@26,1,0,0,@@27,1,0,0,@@28,1,0,0,
  UCGO @@29,1,0,0,@@30,1,0,0,@@31,1,0,0,@@32,1,0,0,@@33,1,0,0,@@34,1,0,0,@@35,1,0,0,
  UCGO @@36,1,0,0,@@37,1,0,0,@@38,1,0,0,@@39,1,0,0,
END
LSTS2204:Spacecraft/spacecraft3
  STATUS Landed Moon
  POS 0.4926574 0.6963968
  HEADING 90.00
  RCSMODE 2
  PRPLEVEL 0:1.000
  NAVFREQ 0 0
  RCS 1
  CTRL_SURFACE 1
  CONFIGURATION 1
  CURRENT_PAYLOAD 0
  SEQ 0 -2 0.000800
  SEQ 1 -2 0.000000
  SEQ 2 -2 0.000000
END
LSTS2213:Spacecraft/spacecraft3
  STATUS Landed Moon
  POS 0.4926574 0.6963968
  HEADING 90.00
  ATTACHED 0:0,LSTS2204
  PRPLEVEL 0:1.000
  NAVFREQ 0 0
  RCS 1
END
END_SHIPS

BEGIN_ExtMFD
END

Script

Code:

-- Load attitude functions
run('attctrl3')

pitch_a =  1.1
pitch_b =  2.6

term.out('LSTS2204 ascent autopilot')
term.out('Run function \'autopilot()\' to start')


-- Reset some of the attitude parameters


vif = V -- target vessel interface
if vif == nil then
   vif = vessel.get_focusinterface()
end

target = vessel.get_interface('Arrow-01')
--Arrow-01

gravref = vif:get_gravityref()


Rm = 1737.1e3
h_t = Rm+84.25e3


mu = GGRAV*oapi.get_mass(gravref)

	
azimuth = PI/2      -- launch azimuth

comment = nil       -- running comments
last_comment = nil  -- previous comment
comment_timer = 0



-- return periapsis radius
function ped (v)
    local el,prm
	el,prm = v:get_elementsex()
	return prm.PeD
end

function alt (v)
    altitude = v:get_altitude()
	return altitude
end

function empty (v)
    local e
	e = v:get_emptymass()
	return e
end

function prop (v)
    local propl
	propl = v:get_totalpropellantmass()
	return propl
end

-- return apoapsis radius
function apd (v)
	el,prm = v:get_elementsex()
	return prm.ApD
end

function trl (v)
	el,prm = v:get_elementsex()
	return prm.TrL
end

function tra (v)
	el,prm = v:get_elementsex()
	return prm.TrA
end

function inc (v)
	el = v:get_elements()
	return el.i
end

function LAN (v)
	el = v:get_elements()
	return el.theta
end

function SMA (v)
	el = v:get_elements()
	return el.a
end

function ecc (v)
	el = v:get_elements()
	return el.e
end

function LPe (v)
	el = v:get_elements()
	return el.omegab
end

function apt (v)
	el,prm = vif:get_elementsex()
	return prm.ApT
end

function pet (v)
	el,prm = vif:get_elementsex()
	return prm.PeT
end

function cross (a,b)
	local c = {x,y,z}
	c.x = a.y*b.z-a.z*b.y
	c.y = a.z*b.x-a.x*b.z
	c.z = a.x*b.y-a.y*b.x
	return c
end

function normalize (vec)
	local laenge = math.sqrt(vec.x*vec.x+vec.y*vec.y+vec.z*vec.z)
	vec.x = vec.x/laenge
	vec.y = vec.y/laenge
	vec.z = vec.z/laenge
	return vec
end

function vspd (v)
  local vel
  vel = v:get_horizonairspeedvector()
 return vel.y
end

function hspd (v)
  vel = v:get_horizonairspeedvector()
 return vel.x
end

function matrixmult(a,b)
	local c = {	m11=a.m11*b.m11+a.m12*b.m21+a.m13*b.m31, m12=a.m11*b.m12+a.m12*b.m22+a.m13*b.m32, m13=a.m11*b.m13+a.m12*b.m23+a.m13*b.m33,
				m21=a.m21*b.m11+a.m22*b.m21+a.m23*b.m31, m22=a.m21*b.m12+a.m22*b.m22+a.m23*b.m32, m23=a.m21*b.m13+a.m22*b.m23+a.m23*b.m33,
				m31=a.m31*b.m11+a.m32*b.m21+a.m33*b.m31, m32=a.m31*b.m12+a.m32*b.m22+a.m33*b.m32, m33=a.m31*b.m13+a.m32*b.m23+a.m33*b.m33}
	return c
end

function transpose_matrix(a)
	local b = {m11=a.m11,m12=a.m21,m13=a.m31,m21=a.m12,m22=a.m22,m23=a.m32,m31=a.m13,m32=a.m23,m33=a.m33}
	return b
end

function matrix_mal_vektor(a,b)
	local c = {x,y,z}
	c.x = a.m11*b.x+a.m12*b.y+a.m13*b.z
	c.y = a.m21*b.x+a.m22*b.y+a.m23*b.z
	c.z = a.m31*b.x+a.m32*b.y+a.m33*b.z
	return c
end

function dot(a,b)
	local c = a.x*b.x+a.y*b.y+a.z*b.z
	return c
end

function sv_from_coe_r (v)
	local e = ecc(v)
	local a = SMA(v)
	local r_p = (1-e)*a
	local h = math.sqrt(r_p*mu*(1+e))
	local RA = LAN(v)
	local incl = inc(v)
	local TA = tra(v)
	local w = LPe(v) - LAN(v)
	local rp_factor = h*h/(mu*(1+e*math.cos(TA)))
	local rp = {}; rp.x = rp_factor*math.cos(TA); rp.y = rp_factor*math.sin(TA); rp.z = 0
	local R3_W = {m11=math.cos(RA),m12=math.sin(RA),m13=0,m21=-math.sin(RA),m22=math.cos(RA),m23=0,m31=0,m32=0,m33=1}
	local R1_i = {m11=1,m12=0,m13=0,m21=0,m22=math.cos(incl),m23=math.sin(incl),m31=0,m32=-math.sin(incl),m33=math.cos(incl)}
	local R3_w = {m11=math.cos(w),m12=math.sin(w),m13=0,m21=-math.sin(w),m22=math.cos(w),m23=0,m31=0,m32=0,m33=1}
	local R2 = matrixmult(R3_w,R1_i)
	local Q_Xp = matrixmult(R2,R3_W)
	local Q_pX = transpose_matrix(Q_Xp)
	local r = mat.mul(Q_pX,rp)
	return r
end

function sv_from_coe_v (v)
	local e = ecc(v)
	local a = SMA(v)
	local r_p = (1-e)*a
	local h = math.sqrt(r_p*mu*(1+e))
	local RA = LAN(v)
	local incl = inc(v)
	local TA = tra(v)
	local w = LPe(v) - LAN(v)
	local vp_factor = mu/h
	local vp = {}; vp.x = -vp_factor*math.sin(TA); vp.y = vp_factor*(e+math.cos(TA)); vp.z = 0
	local R3_W = {m11=math.cos(RA),m12=math.sin(RA),m13=0,m21=-math.sin(RA),m22=math.cos(RA),m23=0,m31=0,m32=0,m33=1}
	local R1_i = {m11=1,m12=0,m13=0,m21=0,m22=math.cos(incl),m23=math.sin(incl),m31=0,m32=-math.sin(incl),m33=math.cos(incl)}
	local R3_w = {m11=math.cos(w),m12=math.sin(w),m13=0,m21=-math.sin(w),m22=math.cos(w),m23=0,m31=0,m32=0,m33=1}
	local R2 = matrixmult(R3_w,R1_i)
	local Q_Xp = matrixmult(R2,R3_W)
	local Q_pX = transpose_matrix(Q_Xp)
	local v = mat.mul(Q_pX,vp)
	return v
end

function rva_relative_r(rA,vA,rB,vB)
	local hA = vec.crossp(rA,vA)
	local i = vec.mul(rA,1/vec.length(rA))
	local k = vec.mul(hA,1/vec.length(hA))
	local j = vec.crossp(k,i)
	local QXx = {m11=i.x, m12=i.y, m13=i.z, m21=j.x, m22=j.y, m23=j.z, m31=k.x, m32=k.y, m33=k.z}
	local r_rel = vec.sub(rB,rA)
	local r_rel_x = mat.mul(QXx,r_rel)
	return r_rel_x
end

function rva_relative_v(rA,vA,rB,vB)
	local hA = vec.crossp(rA,vA)
	local i = vec.mul(rA,1/vec.length(rA))
	local k = vec.mul(hA,1/vec.length(hA))
	local j = vec.crossp(k,i)
	local QXx = {m11=i.x, m12=i.y, m13=i.z, m21=j.x, m22=j.y, m23=j.z, m31=k.x, m32=k.y, m33=k.z}
	local Omega = vec.mul(hA,1/(vec.length(rA)*vec.length(rA)))
	local r_rel = vec.sub(rB,rA)
	local v_rel = vec.sub(vec.sub(vB,vA),vec.crossp(Omega,r_rel))
	local v_rel_x = mat.mul(QXx,v_rel)
	return v_rel_x
end

function rva_relative_a(rA,vA,rB,vB)
	local hA = vec.crossp(rA,vA)
	local i = vec.mul(rA,1/vec.length(rA))
	local k = vec.mul(hA,1/vec.length(hA))
	local j = vec.crossp(k,i)
	local QXx = {m11=i.x, m12=i.y, m13=i.z, m21=j.x, m22=j.y, m23=j.z, m31=k.x, m32=k.y, m33=k.z}
	local Omega = vec.mul(hA,1/(vec.length(rA)*vec.length(rA)))
	local Omega_dot = vec.mul(-2*dot(rA,vA)/(vec.length(rA)*vec.length(rA)),Omega)
	local aA = vec.mul(rA,-mu/math.pow(vec.length(rA),3))
	local aB = vec.mul(rB,-mu/math.pow(vec.length(rB),3))
	local r_rel = vec.sub(rB,rA)
	local v_rel = vec.sub(vec.sub(vB,vA),vec.crossp(Omega,r_rel))
	local a_rel = vec.sub(vec.sub(vec.sub(vec.sub(aB,aA),vec.crossp(Omega_dot,r_rel)),vec.crossp(Omega,vec.crossp(Omega,r_rel))),vec.mul(2,vec.crossp(Omega,v_rel)))
	local a_rel_x = mat.mul(QXx,a_rel)
	return a_rel_x
end

function met_counter ()
  local t0 = oapi.get_simtime()+7
  local met, metstr
  for i=-7,10000 do
    met = oapi.get_simtime()-t0
    metstr = 'MET = '..string.format('%0.0f',met)
	if i==126 then comment = 'Solid rocket booster separation' end
	if comment ~= last_comment then
	  comment_timer = 0
	else
	  comment_timer = comment_timer+1
	  if comment_timer > 10 then comment = nil; comment_timer = 0 end
	end
	last_comment = comment
	if comment ~= nil then
	  metstr = metstr..'\n'..comment
	end
    note.set_text(metstr)
	proc.wait_simtime(t0+i+1)
  end
  note.set_text('')
end

function spdpitch (v)
  -- pitch angle of velocity marker
  -- this only works for bank=0 or bank=pi
  local p=v:get_pitch()
  local a=v:get_aoa()
  if math.abs(v:get_bank()) < PI05 then
    return p-a
  else
    return p+a
  end
end

function prograde(v)
	 v:send_bufferedkey(0x1A)
end

function retrograde(v)
	 v:send_bufferedkey(0x1B)
end

function normal(v)
	 v:send_bufferedkey(0x27)
end

function antinormal(v)
	 v:send_bufferedkey(0x28)
end

function killrot(v)
	 v:send_bufferedkey(0x4C)
end

function RInc (v,target)
  inc_i = inc(v)
  inc_f = inc(target)
  dinc = inc_f - inc_i
  LAN_i = LAN(v)
  LAN_f = LAN(target)
  a1 = math.sin(inc_i)*math.cos(LAN_i)
  a2 = math.sin(inc_i)*math.sin(LAN_i)
  a3 = math.cos(inc_i)
  b1 = math.sin(inc_f)*math.cos(LAN_f)
  b2 = math.sin(inc_f)*math.sin(LAN_f)
  b3 = math.cos(inc_f)
  dangle = math.acos(a1*b1+a2*b2+a3*b3)
  return dangle
end
	

function timetonode(v,TrL_Node)
	local e = ecc(v)
	local TrA_Node = TrL_Node - LPe(v)
	local h = math.sqrt(ped(v)*mu*(1+e))
	local T = 2*PI/mu/mu*math.pow(h/math.sqrt(1-e*e),3)
	local TrA = tra(v)
	local E = 2*math.atan(math.sqrt((1-e)/(1+e))*math.tan(TrA/2))
	local E_Node = 2*math.atan(math.sqrt((1-e)/(1+e))*math.tan(TrA_Node/2))
	local M_e = E-e*math.sin(E)
	local M_e_Node = E_Node-e*math.sin(E_Node)
	local t = M_e/2/PI*T
	local t_Node = M_e_Node/2/PI*T
	local dt = t_Node-t
	return dt
end

function ascent_pitch_prog (v)

m0 = empty(v)
v_ex = 5500
Thrust = 50000
y_t = 60000*0.3048
y_t_dot = 19.5*0.3048
x_t_dot = 5509.5*0.3048
m_dot = Thrust/v_ex
  T = 420
  apo = apd(v)
  height = Rm + 83.35e3
  m1 = 14500

  while apo < height do
  t1 = oapi.get_simtime()
  y_dot = vspd(v)
  local Y = alt(v)
  x_dot = hspd(v)
  local met = t1-t0
  m = empty(v) + prop(v) + 1500
  dv = x_t_dot-x_dot
  tau = m/m_dot
  T = tau*(1-math.exp(-dv/v_ex))
  g = mu/((Rm+alt(v))*(Rm+alt(v)))
  if T>9 then
  tt = (10000-prop(v))/m_dot
  chi_d = math.atan((y_t_dot-y_dot+g*T)/(x_t_dot-x_dot))
  A1 = math.cos(chi_d)*math.log(tau/(tau-T))
  B1 = math.cos(chi_d)*(tau*math.log(tau/(tau-T))-T)
  A2 = v_ex*math.cos(chi_d)*(T-(tau-T)*math.log(tau/(tau-T)))
  B2 = v_ex*math.cos(chi_d)*(-T*T/2+tau*(T-(tau-T)*math.log(tau/(tau-T))))
  C2 = Y-y_t-0.5*g*T*T+y_dot*T+v_ex*math.sin(chi_d)*(T-(tau-T)*math.log(tau/(tau-T)))
  K1 = B1*C2/(A2*B1-A1*B2)
  K2 = A1*K1/B1
  chi = chi_d - K1 - K2*tt - PI/2
  else
  chi = -80*RAD
  end
  if chi<-90*RAD then
  chi = -89.99*RAD
  end
  setpitch (v,chi,0.1,0)
	-- note that we set a tolerance of 0 to prevent the
	-- pitch programme from terminating prematurely
  end
end


-- ------------------------------------------------------
-- The complete autopilot
-- ------------------------------------------------------

function autopilot()
local l_i = trl(vif)
local l_f = trl(target)
while l_f<l_i do
proc.wait_simdt(1)
l_i = trl(vif)
l_f = trl(target)
end
term.out('overflight, launch in 105 seconds')
proc.wait_simdt(105)
launch()
end


function launch ()
  term.out('Ascent autopilot initiated.')
  term.out('Attached to: '..vif:get_name())
  term.out('Launch azimuth: '..azimuth*DEG..'deg.')
  ascent(vif)
  --oms_burn1 (vif)
  --Plane(vif)
  CSI (vif)
  if RInc(vif,target)>0.0005 then
  Plane(vif)
  end
  CDH(vif)
  TPI(vif)
  MCC(vif)
  --Braking(vif)
  term.out('Exit launch autopilot.')
end


-- ------------------------------------------------------
-- First part of ascent autopilot.
-- Convers launch to ET separation
-- ------------------------------------------------------

function ascent (v)
  
  proc.bg(met_counter)  -- launch MET counter

  proc.wait_simdt(3);
  v:set_thrustergrouplevel (THGROUP.HOVER, 1)
  comment = 'Hover engine ignition'
  proc.wait_simdt(4) -- SSME ignition

  t0 = oapi.get_simtime()         -- set MET reference
  comment = 'We have liftoff!'




  proc.wait_ge(alt, 100, v)
  term.out('initial pitch')
  SetRCS(v, RCSMODE.PITCH, -1) -- intital pitch to avoid gimbal lock
  proc.wait_simdt(1)
  SetRCS(v, RCSMODE.PITCH, 0)
  v:send_bufferedkey(ktable.G)

  proc.wait_simtime(t0+8)
  comment = 'initiate roll and yaw stabilisation'
  bank_prog = proc.bg(setbank,v,0,500,0)
  yaw_prog  = proc.bg(setyaw,v,azimuth,500,0)
  pitch_prog = proc.bg(ascent_pitch_prog, v)
  comment = 'Start pitch programme'
  -- run the roll/yaw stabilisation and the pitch control
  -- in separate program branches

  -- meanwhile, in the main branch we sit back and wait
  -- until MECO conditions are satisfied
  proc.wait_ge(apd, h_t, v)
  -- wait for apogee > 154km
  
  term.out('Hover engine cut off')
  v:set_thrustergrouplevel (THGROUP.HOVER, 0)

  proc.kill(bank_prog)
  proc.kill(yaw_prog)
  proc.kill(pitch_prog)
  proc.wait_simdt(2)
  term.out('roll upright')
  local bank_prog = proc.bg(setbank,v,0,100,0)
  local yaw_prog  = proc.bg(setyaw,v,PI/2,100,0)
  local pitch_prog = proc.bg(setpitch,v,0,100,0)
  proc.wait_simdt(20)
  proc.kill(bank_prog)
  proc.kill(yaw_prog)
  proc.kill(pitch_prog)
  attoff(v)
  killrot(vif)
  proc.wait_simdt(15)
end

function oms_burn1 (v)
  term.out('OMS 1 initialized')
  local bank_prog = proc.bg(setbank,v,0,100,0)
  local yaw_prog  = proc.bg(setyaw,v,PI/2,100,0)
  local pitch_prog = proc.bg(setpitch,v,0,100,0)
  -- end the ascent attitude control programs
  proc.wait_simdt(20)
  dh = apd(v)-h_t
  while math.abs(dh)> 20 do
  if dh<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
  else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
  end
  dh = apd(v)-h_t
  end
  proc.kill(bank_prog)
  proc.kill(yaw_prog)
  proc.kill(pitch_prog)
  setyaw(v,PI/2,5,0)
  setbank(v,0,5,0)
  setpitch(v,0,5,0)
end

function Plane (v)
  term.out('Align Plane engaged')
  inc_i = inc(v)
  inc_f = inc(target)
  dinc = inc_f - inc_i
  LAN_i = LAN(v)
  LAN_f = LAN(target)
  a1 = math.sin(inc_i)*math.cos(LAN_i)
  a2 = math.sin(inc_i)*math.sin(LAN_i)
  a3 = math.cos(inc_i)
  b1 = math.sin(inc_f)*math.cos(LAN_f)
  b2 = math.sin(inc_f)*math.sin(LAN_f)
  b3 = math.cos(inc_f)
  c1 = a2*b3-a3*b2
  c2 = a3*b1-a1*b3
  c3 = a1*b2-a2*b1
  if c1<0 then
  long1 = math.atan(c2/c1)+PI/2
  else
  long1 = math.atan(c2/c1)+3*PI/2
  end
  if long1 > PI then
  long2 = long1
  long1 = long1-PI
  else
  long2 = long1+PI
  end
  if trl(v)>long2 then
  long = long1
  else
  if trl(v)>long1 then
  long = long2
  else
  long = long1
  end
  end
  waiting = 0
  if trl(v) > long-6*RAD then
  proc.wait_le(trl, 1*RAD, v)
  end
  proc.wait_ge(trl, long-6*RAD, v)
  r_A = SMA(v)*(1-ecc(v)*ecc(v))/(1+ecc(v)*math.cos(tra(v)))
  v_A = math.sqrt(mu/r_A)
  local dangle = math.acos(a1*b1+a2*b2+a3*b3)
  dv = 2*v_A*math.sin(dangle/2)
  burnt = burntime (v,dv)
  vif:set_navmode(NAVMODE.ANTINORMAL)
  while timetonode(vif,long)>burnt/2 do
	proc.skip()
  end
  -- end the ascent attitude control programs
  while math.abs(dangle)> 0.1*RAD do
  if dinc<0 then
    v:set_thrustergrouplevel (THGROUP.MAIN,1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.MAIN,0)
  else
    v:set_thrustergrouplevel (THGROUP.RETRO,1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.RETRO,0) 
  end
  dangle = RInc(v,target)
  end
  while math.abs(dangle)> 0.001*RAD do
  if dinc<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.5)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
  else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.5)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
  end
  dangle = RInc(v,target)
  end
  prograde(vif)
  proc.wait_simdt(20)
  killrot(vif)
end

function CSI (v)
  term.out('CSI engaged')
  ped_t = 45*1852+Rm
  while apt(v)>60 do
  proc.skip()
  end
  vif:set_navmode(NAVMODE.PROGRADE)
  -- end the ascent attitude control programs
  while apt(v)>5 do
  proc.skip()
  end
  local dh = ped(v)-ped_t
  while math.abs(dh)> 1000 do
  if dh<0 then
    v:set_thrustergrouplevel (THGROUP.MAIN,1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.MAIN,0)
  else
    v:set_thrustergrouplevel (THGROUP.RETRO,1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.RETRO,0) 
  end
  dh = ped(v)- ped_t
  end
  while math.abs(dh)> 20 do
  if dh<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.1)
	proc.wait_simdt(0.05)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
  else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.1)
	proc.wait_simdt(0.05)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
  end
  dh = ped(v)-ped_t
  end
  proc.wait_simdt(10)
  vif:set_navmode(NAVMODE.KILLROT)
  proc.wait_simdt(10)
end

function CDH(v)
  term.out('CDH engaged')
  apd_t = 45*1852.9+Rm
  proc.wait_le(pet, 60, v)
  vif:set_navmode(NAVMODE.PROGRADE)
  proc.wait_le(pet, 4, v)
  -- end the ascent attitude control programs
  local e = ecc(v)
  local dh = apd(v) - apd_t
  while math.abs(e)> 0.001 do
  if dh<0 then
    v:set_thrustergrouplevel (THGROUP.MAIN,1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.MAIN,0)
  else
    v:set_thrustergrouplevel (THGROUP.RETRO,1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.RETRO,0) 
  end
  e = ecc(v)
  dh = apd(v) - apd_t
  end
  while math.abs(e)> 0.00005 do
  if dh<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.1)
	proc.wait_simdt(0.05)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
  else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.1)
	proc.wait_simdt(0.05)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
  end
  e = ecc(v)
  dh = apd(v) - apd_t
  end
  vif:set_navmode(NAVMODE.KILLROT)
end

function TPI (v)
	--state vector
	term.out('TPI engaged')
	r_i = sv_from_coe_r(v)
	v_i = sv_from_coe_v(v)
	r_f = sv_from_coe_r(target)
	v_f = sv_from_coe_v(target)
	r_relativex=rva_relative_r(r_i,v_i,r_f,v_f)
	v_relativex=rva_relative_v(r_i,v_i,r_f,v_f)
	a_relativex=rva_relative_a(r_i,v_i,r_f,v_f)
	term.out(r_relativex)
	term.out(v_relativex)
	term.out(a_relativex)
	term.out('Distance:')
	term.out(vec.length(r_relativex))
	term.out('Angle:')
	term.out(math.atan2(r_relativex.x,r_relativex.y)*DEG)
	DST = vec.length(r_relativex)
	--wait until angle = 5 deg
	while DST>100*1000 do
	r_i = sv_from_coe_r(v)
	v_i = sv_from_coe_v(v)
	r_f = sv_from_coe_r(target)
	v_f = sv_from_coe_v(target)
	r_relativex=rva_relative_r(r_i,v_i,r_f,v_f)
	DST = vec.length(r_relativex)
	proc.skip()
	end
	--point to target
	term.out('Point to Target')
	local bank_prog = proc.bg(target_bank_prog,v)
	local yaw_prog  = proc.bg(target_yaw_prog,v)
    local pitch_prog = proc.bg(target_pitch_prog,v)
	proc.wait_ge(elev, 26.5*RAD, v)
	t_TPI = oapi.get_simtime()
	term.out('25fps burn')
	--at 26.5 deg burn of 25fps
	local dv = 24.8*0.3048
	local burnt = burntime(v,dv)
	v:set_thrustergrouplevel (THGROUP.MAIN,1)
	proc.wait_simdt (burnt)
	v:set_thrustergrouplevel (THGROUP.MAIN,0)
	proc.kill(bank_prog)
	proc.kill(yaw_prog)
	proc.kill(pitch_prog)
	attoff(v)
end

function MCC (v)
	local bank_prog = proc.bg(target_bank_prog,v)
	local yaw_prog  = proc.bg(target_yaw_0,v)
    local pitch_prog = proc.bg(target_pitch_0,v)
	--Waiting for MCC1
	proc.wait_ge(oapi.get_simtime,t_TPI+10*60,v)
	term.out('5 minutes to MCC1')
	proc.wait_ge(oapi.get_simtime,t_TPI+14*60,v)
	term.out('1 minutes to MCC1')
	proc.wait_ge(oapi.get_simtime,t_TPI+15*60,v)
	term.out('MCC1')
	--43*60-(oapi.get_simtime()-t_TPI)
	local dv = lambert_targeting(v,43*60-(oapi.get_simtime()-t_TPI))
	proc.kill(bank_prog)
	proc.kill(yaw_prog)
	proc.kill(pitch_prog)
	attoff(v)
	while vec.length(dv)>0.01 do
	term.out(dv)
	--dv_maneuver(v,dv)
	if dv.y > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_DOWN,0)
	v:set_thrustergrouplevel (THGROUP.ATT_UP, 0.1)
	elseif dv.y < 0 then
	v:set_thrustergrouplevel (THGROUP.ATT_UP, 0)
	v:set_thrustergrouplevel(THGROUP.ATT_DOWN,0.1)
	end
	if dv.z > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_RIGHT,0)
	v:set_thrustergrouplevel(THGROUP.ATT_LEFT,0.1)
	elseif dv.z < 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_LEFT,0)
	v:set_thrustergrouplevel(THGROUP.ATT_RIGHT,0.1)
	end
	if dv.x < 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_BACK,0)
	v:set_thrustergrouplevel(THGROUP.ATT_FORWARD,0.1)
	elseif dv.x > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_FORWARD,0)
	v:set_thrustergrouplevel(THGROUP.ATT_BACK,0.1)
	end
	--proc.wait_simdt(2)
	dv = lambert_targeting(v,43*60-(oapi.get_simtime()-t_TPI))
	end
	attoff(v)
	bank_prog = proc.bg(target_bank_prog,v)
	yaw_prog  = proc.bg(target_yaw_0,v)
    pitch_prog = proc.bg(target_pitch_0,v)
	proc.wait_ge(oapi.get_simtime,t_TPI+25*60,v)
	term.out('5 minutes to MCC2')
	proc.wait_ge(oapi.get_simtime,t_TPI+29*60,v)
	term.out('1 minutes to MCC2')
	proc.wait_ge(oapi.get_simtime,t_TPI+30*60,v)
	term.out('MCC2')
	dv = lambert_targeting(v,43*60-(oapi.get_simtime()-t_TPI))
	term.out(dv)
	--Waiting for MCC2
	proc.kill(bank_prog)
	proc.kill(yaw_prog)
	proc.kill(pitch_prog)
	attoff(v)
	while vec.length(dv)>0.005 do
	term.out(dv)
	if dv.y > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_DOWN,0)
	v:set_thrustergrouplevel (THGROUP.ATT_UP, 0.1)
	elseif dv.y < 0 then
	v:set_thrustergrouplevel (THGROUP.ATT_UP, 0)
	v:set_thrustergrouplevel(THGROUP.ATT_DOWN,0.1)
	end
	if dv.z > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_RIGHT,0)
	v:set_thrustergrouplevel(THGROUP.ATT_LEFT,0.1)
	elseif dv.z < 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_LEFT,0)
	v:set_thrustergrouplevel(THGROUP.ATT_RIGHT,0.1)
	end
	if dv.x < 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_BACK,0)
	v:set_thrustergrouplevel(THGROUP.ATT_FORWARD,0.1)
	elseif dv.x > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_FORWARD,0)
	v:set_thrustergrouplevel(THGROUP.ATT_BACK,0.1)
	end
	dv = lambert_targeting(v,43*60-(oapi.get_simtime()-t_TPI))
	end
	attoff(v)
	dv = lambert_targeting(v,43*60-(oapi.get_simtime()-t_TPI))
	term.out(dv)
end

function lambert_targeting(v,t)
	local R1,V1 = sv_from_coe (v)
	local R1_T,V1_T = sv_from_coe (target)
	local R2_T,V2_T = rv_from_r0v0(R1_T,V1_T,t)
	local V_i = lambert(R1,R2_T,t)
	local V_m = vec.sub(V_i,V1)
	local V_mE = localize(v,V_m)
	return V_mE
	
end

function localize (v,Vm)
	local rA,vA = sv_from_coe (v)
	local hA = vec.crossp(rA,vA)
	local i = vec.mul(rA,1/vec.length(rA))
	local k = vec.mul(hA,1/vec.length(hA))
	local j = vec.crossp(k,i)
	local QXx = {m11=i.x, m12=i.y, m13=i.z, m21=j.x, m22=j.y, m23=j.z, m31=k.x, m32=k.y, m33=k.z}
	local V_mE = mat.mul(QXx,Vm)
	local en = elev(v)
	local pro = v:get_progradedir()
	local ya = math.atan2(pro.x,pro.z)
	local Rz = {m11=math.cos(en), m12=-math.sin(en), m13=math.sin(en), m21=math.sin(en), m22=math.cos(en), m23=0, m31=0, m32=0, m33=0}
	local Ry = {m11=math.cos(ya), m12=0, m13=math.sin(ya), m21=0, m22=1, m23=0, m31=-math.sin(ya), m32=0, m33=math.cos(ya)}
	local V_mEn = mat.mul(Ry,mat.mul(Rz,V_mE))
	return V_mEn
end

function dv_maneuver(v,dv)
	xburnt = burntime(v,math.abs(dv.x))
	yburnt = burntime(v,math.abs(dv.y))
	zburnt = burntime(v,math.abs(dv.z))
	term.out(xburnt)
	term.out(yburnt)
	term.out(zburnt)
	if dv.z > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_LEFT,1)
	proc.wait_simdt(zburnt)
	v:set_thrustergrouplevel(THGROUP.ATT_LEFT,0)
	elseif dv.z < 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_RIGHT,1)
	proc.wait_simdt(zburnt)
	v:set_thrustergrouplevel(THGROUP.ATT_RIGHT,0)
	end
	if dv.y < 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_UP,1)
	proc.wait_simdt(yburnt)
	v:set_thrustergrouplevel(THGROUP.ATT_UP,0)
	elseif dv.y > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_DOWN,1)
	proc.wait_simdt(yburnt)
	v:set_thrustergrouplevel(THGROUP.ATT_DOWN,0)
	end
	if dv.x > 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_FORWARD,1)
	proc.wait_simdt(xburnt)
	v:set_thrustergrouplevel(THGROUP.ATT_FORWARD,0)
	elseif dv.x < 0 then
	v:set_thrustergrouplevel(THGROUP.ATT_BACK,1)
	proc.wait_simdt(xburnt)
	v:set_thrustergrouplevel(THGROUP.ATT_BACK,0)
	end
end

function Rendezvous (v)
TPI(v)
MCC(v)
--Braking(v)
end

function Braking (v)
	local bank_prog = proc.bg(target_bank_prog,v)
	local yaw_prog  = proc.bg(target_yaw_prog,v)
    local pitch_prog = proc.bg(target_pitch_prog,v)
	proc.wait_le(distance, 6000*0.3048, v)
	--Braking Gate 1: 6000 feet, 30 feet/sec
	relv = relvel(v)-30*0.3048
	while math.abs(relv)> 0.01 do
    if relv<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
    else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
    end
    relv = relvel(v)-30*0.3048
    end
	proc.wait_le(distance, 3000*0.3048, v)
	--Braking Gate 2: 3000 feet, 20 feet/sec
	relv = relvel(v)-20*0.3048
	while math.abs(relv)> 0.01 do
    if relv<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
    else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
    end
    relv = relvel(v)-20*0.3048
    end
	proc.wait_le(distance, 1500*0.3048, v)
	--Braking Gate 3: 1500 feet, 10 feet/sec
	relv = relvel(v)-10*0.3048
	while math.abs(relv)> 0.01 do
    if relv<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
    else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
    end
    relv = relvel(v)-10*0.3048
    end
	proc.wait_le(distance, 600*0.3048, v)
	--Braking Gate 4: 600 feet, 5 feet/sec
	relv = relvel(v)-5*0.3048
	while math.abs(relv)> 0.01 do
    if relv<0 then
    v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_FORWARD,0)
    else
    v:set_thrustergrouplevel (THGROUP.ATT_BACK,0.1)
	proc.wait_simdt(0.1)
	v:set_thrustergrouplevel (THGROUP.ATT_BACK,0) 
    end
    relv = relvel(v)-5*0.3048
    end
	proc.kill(bank_prog)
	proc.kill(yaw_prog)
	proc.kill(pitch_prog)
	attoff(v)
end

function target_pitch_prog(v)
	while distance(v) > 150 do
	local pit = elev(v)
	--if elev(v) > 90*RAD then
	--pit = PI - pit
	--end
	setpitch_orb(v,pit,1,0)
	end
end

function target_pitch_0(v)
	while distance(v) > 150 do
	setpitch_orb(v,0,1,0)
	end
end

function target_bank_prog(v)
	while distance(v) > 150 do
	setbank(v,0,1,0)
	end
end

function target_yaw_prog(v)
	while distance(v) > 150 do
	local yawt = yaw_target(v)
	setyaw_orb(v,yawt,1,0)
	end
end

function target_yaw_0(v)
	while distance(v) > 150 do
	setyaw_orb(v,0,1,0)
	end
end

function yaw_target (v)
	local r_i = sv_from_coe_r(v)
	local v_i = sv_from_coe_v(v)
	local r_f = sv_from_coe_r(target)
	local v_f = sv_from_coe_v(target)
	local r_relativex=rva_relative_r(r_i,v_i,r_f,v_f)
	local v_relativex=rva_relative_v(r_i,v_i,r_f,v_f)
	local a_relativex=rva_relative_a(r_i,v_i,r_f,v_f)
	local ya = math.atan2(r_relativex.z,r_relativex.y)
	local yaw_t = -ya
	return yaw_t
end

function elev(v)
	r_i = sv_from_coe_r(v)
	v_i = sv_from_coe_v(v)
	r_f = sv_from_coe_r(target)
	v_f = sv_from_coe_v(target)
	r_relativex=rva_relative_r(r_i,v_i,r_f,v_f)
	v_relativex=rva_relative_v(r_i,v_i,r_f,v_f)
	a_relativex=rva_relative_a(r_i,v_i,r_f,v_f)
	elevation=math.atan2(r_relativex.x,r_relativex.y)
	return elevation
end

function distance (v)
	local r_i = sv_from_coe_r(v)
	local v_i = sv_from_coe_v(v)
	local r_f = sv_from_coe_r(target)
	local v_f = sv_from_coe_v(target)
	local r_relativex=rva_relative_r(r_i,v_i,r_f,v_f)
	local DST = vec.length(r_relativex)
	return DST
end

function relvel2 (v)
	local v_f = sv_from_coe_v(target)
	local r_i = sv_from_coe_r(v)
	local r_f = sv_from_coe_r(target)
	local v_i = sv_from_coe_v(v)
	local v_relativex=rva_relative_v(r_i,v_i,r_f,v_f)
	local relv = vec.length(v_relativex)
	return relv
end


function burntime (v,dv)
local m0 = v:get_mass()
local isp = 5500
local F = 10000
local dt = isp*m0/F*(1-math.exp(-dv/isp))
return dt
end

function burntime_att (v,dv)
local m0 = v:get_mass()
local isp = 5500
local F = 5000
local dt = isp*m0/F*(1-math.exp(-dv/isp))
return dt
end

function relvel (v)
	local v_f = sv_from_coe_v(target)
	local r_i = sv_from_coe_r(v)
	local r_f = sv_from_coe_r(target)
	local v_i = sv_from_coe_v(v)
	local v_relativex=rva_relative_v(r_i,v_i,r_f,v_f)
	local r_relativex=rva_relative_r(r_i,v_i,r_f,v_f)
	local relv = -1*vec.dotp(r_relativex,v_relativex)/vec.length(r_relativex)
	return relv
end

function lambert (R1, R2, t)
r1 = vec.length(R1)
r2 = vec.length(R2)
c12 = vec.crossp(R1,R2)
local theta = math.acos(vec.dotp(R1,R2)/r1/r2)
if c12.z <= 0 then
	theta = 2*PI-theta
end
A = math.sin(theta)*math.sqrt(r1*r2/(1-math.cos(theta)))
z = -100
while F(z,t) < 0 do
	z = z+0.1
end
local tol = 1e-8
local nmax = 1000
local ratio = 1
local n = 0
while math.abs(ratio) > tol and n<=nmax do
	n = n+1
	ratio = F(z,t)/dFdz(z)
	z = z-ratio
	proc.skip()
end
local f = 1- yy(z)/r1
local g = A*math.sqrt(yy(z)/mu)
local gdot = 1- yy(z)/r2
local V1 = vec.mul(1/g,vec.sub(R2,vec.mul(f,R1)))
local V2 = vec.mul(1/g,vec.sub(vec.mul(gdot,R2),R1))
return V1
end

function yy(z)
local dum = r1+r2+A*(z*S(z)-1)/math.sqrt(C(z))
return dum
end

function F(z,t)
local dum = power((yy(z)/C(z)),1.5)*S(z)+A*math.sqrt(yy(z))-math.sqrt(mu)*t
return dum
end

function dFdz(z)
local dum = 0
if z==0
then
dum = math.sqrt(2)/40*power(yy(0),1.5)+A/8*(math.sqrt(yy(0))+A*math.sqrt(1/2/yy(0)))
else
dum = power(yy(z)/C(z),1.5)*(1/2/z*(C(z)-3*S(z)/2/C(z))+3*power(S(z),2)/4/C(z))+A/8*(3*S(z)/C(z)*math.sqrt(yy(z))+A*math.sqrt(C(z)/yy(z)))
end
return dum
end

function C(z)
local dum = stumpC(z)
return dum
end

function S(z)
local dum = stumpS(z)
return dum
end

function stumpS(z)
local s = 0
if z>0 then
s = (math.sqrt(z)-math.sin(math.sqrt(z)))/power(math.sqrt(z),3)
elseif z<0 then
s = (math.sinh(math.sqrt(-z))-math.sqrt(-z))/power(math.sqrt(-z),3)
else
s = 1/6
end
return s
end

function stumpC(z)
local c = 0
if z>0 then
c = (1-math.cos(math.sqrt(z)))/z
elseif z<0 then
c = (math.cosh(math.sqrt(-z))-1)/(-z)
else
c = 0.5
end
return c
end

function rv_from_r0v0(R0, V0, t)
local r0 = vec.length(R0)
local v0 = vec.length(V0)
local vr0 = vec.dotp(R0,V0)/r0
local alpha = 2/r0-v0*v0/mu
local x = kepler_U(t,r0,vr0,alpha)
local f,g = f_and_g(x,t,r0,alpha)
local R = vec.add(vec.mul(f,R0),vec.mul(g,V0))
local r = vec.length(R)
local fdot,gdot = fDot_and_gDot(x,r,r0,alpha)
local V = vec.add(vec.mul(fdot,R0),vec.mul(gdot,V0))
return R,V
end

function f_and_g(x,t,ro,a)
local z = a*x*x
local f = 1-x*x/ro*stumpC(z)
local g = t-1/math.sqrt(mu)*power(x,3)*stumpS(z)
return f,g
end

function fDot_and_gDot(x,r,ro,a)
local z = a*x*x
local fdot = math.sqrt(mu)/r/ro*(z*stumpS(z)-1)*x
local gdot = 1-x*x/r*stumpC(z)
return fdot,gdot
end

function kepler_U(dt,ro,vro,a)
local error2 = 1e-8
local nMax = 1000
local x = math.sqrt(mu)*math.abs(a)*dt
local n = 0
local ratio = 1
local C = 0
local S = 0
local F = 0
local dFdx = 0
while math.abs(ratio)>error2 and n<=nMax do
n = n+1
C = stumpC(a*x*x)
S = stumpS(a*x*x)
F = ro*vro/math.sqrt(mu)*x*x*C+(1-a*ro)*power(x,3)*S+ro*x-math.sqrt(mu)*dt
dFdx = ro*vro/math.sqrt(mu)*x*(1-a*x*x*S)+(1-a*ro)*x*x*C+ro
ratio = F/dFdx
x = x-ratio
end
return x
end

function sv_from_coe (v)
	local e = ecc(v)
	local a = SMA(v)
	local r_p = (1-e)*a
	local h = math.sqrt(r_p*mu*(1+e))
	local RA = LAN(v)
	local incl = inc(v)
	local TA = tra(v)
	local w = LPe(v) - LAN(v)
	local rp_factor = h*h/(mu*(1+e*math.cos(TA)))
	local rp = {}; rp.x = rp_factor*math.cos(TA); rp.y = rp_factor*math.sin(TA); rp.z = 0
	local vp_factor = mu/h
	local vp = {}; vp.x = -vp_factor*math.sin(TA); vp.y = vp_factor*(e+math.cos(TA)); vp.z = 0
	local R3_W = {m11=math.cos(RA),m12=math.sin(RA),m13=0,m21=-math.sin(RA),m22=math.cos(RA),m23=0,m31=0,m32=0,m33=1}
	local R1_i = {m11=1,m12=0,m13=0,m21=0,m22=math.cos(incl),m23=math.sin(incl),m31=0,m32=-math.sin(incl),m33=math.cos(incl)}
	local R3_w = {m11=math.cos(w),m12=math.sin(w),m13=0,m21=-math.sin(w),m22=math.cos(w),m23=0,m31=0,m32=0,m33=1}
	local R2 = matrixmult(R3_w,R1_i)
	local Q_Xp = matrixmult(R2,R3_W)
	local Q_pX = transpose_matrix(Q_Xp)
	local r = mat.mul(Q_pX,rp)
	local v = mat.mul(Q_pX,vp)
	return r,v
end

I would have to think about,what could to be added,or improved in a future version,not much it's very precise as is,I think being able to use it with the LSTS231 heavy utility lander is a great option.Thanks,and keep up the good work.

indy91 · Nov 8, 2012

New (minor) update is released

I think i fixed the problem with the heavy lander. The planar correction didn't work as intended, but I never had any problem at the CSI.
I only had a MCC1 execution error sometimes, especially when i used time warp, but i think i fixed that too.

My longterm goals are:

-implementing the direct transfer rendezvous mode (flown on Apollo 14-17)
-Lunar Landing Autopilot
-transfer the autopilot to a C++ MFD. I wanted to program it in C++ at the beginning, but as I am not a programmer it was much easier to experiment with lua at first.

indy91 · Dec 29, 2012

I rewrote the autopilot completetly and release it here first as a kind of beta version. Still missing is the breaking.
No new features yet, but it is working (for me) and I would like to here some feedback. Feel free to suggest features and changes, so I can improve the script, before I release it at Orbithangar.

Orbiter 2024 has been released!

Project Launch and Rendezvous Autopilot for Greg Burch's LSTS in Lua

indy91

Addon Developer

Interceptor

Well-known member

indy91

Addon Developer

Mandella

Space Cultist

Interceptor

Well-known member

indy91

Addon Developer

indy91

Addon Developer

Interceptor

Well-known member

indy91

Addon Developer

Interceptor

Well-known member

indy91

Addon Developer

indy91

Addon Developer

Attachments

Similar threads