def compute_thrust(self, vessel, t):
v = np.array(vessel.flight(vessel.orbit.body.reference_frame).velocity)
if vessel.flight().surface_altitude > 20000:
dt = 5
else:
dt = 1
if self.traj.compute_coasting(self.conn.space_center,
vessel,
t=t,
dt=dt,
tgt_alt=self.tgt_alt + self.aim_height):
# Find impact position on surface
t0, r0, v0, a0 = self.traj.path[0]
tT, rT, vF, aT = self.traj.path[-1]
# Compute target position in the future - all in rotating reference frame now
rTgt, rotm = utils.target_position_and_rotation(
vessel, self.tgt_lat, self.tgt_lng,
self.tgt_alt + self.aim_height,
vessel.orbit.body.reference_frame)
r = np.linalg.norm(rT)
dE = rT - rTgt
# convert to local surface reference frame
# Convert from non_rotating co-ords into surface/target relative co-ords
F = -dE
print "Boostback prediction dist:", np.linalg.norm(dE)
throttle = np.linalg.norm(F) * self.throttle_Kp
# compute thrust
att = np.array(
vessel.flight(vessel.orbit.body.reference_frame).direction)
ddot = F.dot(att) / np.linalg.norm(F)
print "ddot:", ddot
if ddot < 0.9:
throttle = 0.01 # enough to steer
if np.linalg.norm(F) < self.deadzone:
if not self.in_deadzone:
print "Prediction within deadzone of %dm" % self.deadzone
self.in_deadzone = True
throttle = 0
# Steer retrograde
F = -v
else:
self.in_deadzone = False
return throttle, F
maxT_angle=args.maxT_angle,
maxLand_angle=args.maxLand_angle,
pos_P=args.pos_P,
vel_P=args.vel_P)
soft_landing.draw(vessel)
final_landing = FinalLanding(surface_g=vessel.orbit.body.surface_gravity,
cog_height=args.cog_height,
land_factor=args.land_factor,
throttle_Kp=args.throttle_Kp,
conn=conn)
# returns position in given reference frame
# and rotation matrix that converts from vessel.orbit.body.reference_frame to
# X(Up), Y(North), Z(East) in local target co-ordinates
pos_tgt, rotm = utils.target_position_and_rotation(vessel, tgt_lat, tgt_lng,
tgt_alt)
irotm = np.transpose(rotm)
t0 = conn.space_center.ut
t_draw = 0
latt = None
##################################### MAIN LOOP ########################################
while True:
# Main instrument/guidance data
t = conn.space_center.ut - t0
r = np.array(vessel.position(vessel.orbit.body.reference_frame))
v = np.array(vessel.flight(vessel.orbit.body.reference_frame).velocity)
att = np.array(vessel.flight(vessel.orbit.body.reference_frame).direction)
h = vessel.flight(vessel.surface_velocity_reference_frame).surface_altitude
if vessel.available_thrust > 0:
twr = vessel.available_thrust / vessel.mass
else:
def compute_soft_landing(self,
space_center,
vessel,
tgt_lat,
tgt_lng,
tgt_alt,
dt=1,
compute_t=0.5,
t0=0,
logfile=None,
maxT_angle=90,
maxLand_angle=15,
min_throttle=0.05,
max_throttle=0.9):
"""Compute trajectory for soft-landing phase"""
r = np.array(vessel.position(vessel.orbit.body.reference_frame))
v = np.array(vessel.flight(vessel.orbit.body.reference_frame).velocity)
rTgt, rotm = utils.target_position_and_rotation(
vessel, tgt_lat, tgt_lng, tgt_alt)
# vars
twr = vessel.max_thrust / vessel.mass
t_per_N = 5
max_N = 8
r2 = rotm.dot(r - rTgt)
v2 = rotm.dot(v)
att = rotm.dot(
np.array(
vessel.flight(vessel.orbit.body.reference_frame).direction))
g = np.array([-vessel.orbit.body.surface_gravity, 0., 0.])
results = {}
# progress time forward to compute_t to start trajectory in future, allowing for real time to compute it
r2 = r2 + v2 * compute_t + 0.5 * g * compute_t * compute_t
v2 = v2 + g * compute_t
# Function solver called from golden_search()
def f(T):
num_thrust = int(T / t_per_N)
num_thrust = max(3, min(num_thrust, max_N))
print "Evaluating f(T=%.3f)" % T, "num_thrust=", num_thrust
fuel, accels = gfold.solve(r2,
v2,
att,
T=T,
N=num_thrust,
g=g,
max_thrust=twr * max_throttle,
min_thrust=twr * min_throttle,
maxT_angle=maxT_angle,
maxLand_angle=maxLand_angle,
dt=T / 100.0,
min_height=40)
if accels != None:
results[fuel] = (T, accels)
print "Saving fuel=", fuel, "T=", T, "Accels=", len(accels)
print "Solving for T=%.2f N=%d fuel=%.1f" % (T, num_thrust, fuel)
return fuel
# Convert co-ords to target surface reference frame
min_T = utils.golden_search(f, 1, 120, tol=0.1)
# Find minimum T from results (as above might return an infeasible region)
if results:
fuels = sorted(list(results.keys()))
for f in fuels:
print "Fuel:", f, "T:", results[f][0]
min_fuel = fuels[0]
min_T = results[min_fuel][0]
accels = results[min_fuel][1]
self.path = utils.compute_trajectory(r2,
v2,
accels,
min_T,
g,
dt=dt,
t0=t0 + compute_t)
# make sure trajectory ends at (0,0,0) - bit of a cheat
#self.path = utils.correct_trajectory(self.path)
print "logfile", logfile
if logfile:
self.save(logfile)
# Convert co-ords back to rotating reference frame - for drawing
self.path = self.convert_local_to_rotating_reference_frame(
rTgt, rotm)
return True
self.path = []
return False
def compute_thrust(self, vessel, t):
h = vessel.flight().surface_altitude
r = np.array(vessel.position(vessel.orbit.body.reference_frame))
v = np.array(vessel.flight(vessel.orbit.body.reference_frame).velocity)
twr = vessel.max_thrust / vessel.mass
# Get target altitude between tgt_alt and tgt_alt+aim_height aimed current_alt-aim_below
aim_height = utils.clamp(
vessel.flight().surface_altitude - self.aim_below, self.tgt_alt,
self.tgt_alt + self.aim_height)
# Compute target position in the future - all in rotating reference frame now
rTgt, rotm = utils.target_position_and_rotation(
vessel, self.tgt_lat, self.tgt_lng, aim_height,
vessel.orbit.body.reference_frame)
if h > 20000:
dt = 5
else:
dt = 1
# Compute trajectory in rotating co-ords
if self.traj.compute_coasting(self.conn.space_center,
vessel,
t=t,
dt=dt,
tgt_alt=aim_height):
# Find impact position on surface
t0, r0, v0, a0 = self.traj.path[0]
tT, rT, vF, aT = self.traj.path[-1]
dE = rT - rTgt
print "Coasting predict err:", dE, "aim alt:", aim_height
vst = dE / np.linalg.norm(dE) # prediction direction
mag = min(1, np.linalg.norm(dE) * 0.006) # 200m = maximum steering
F = -v / np.linalg.norm(v) + 0.3 * vst * mag
# Use thrust to stop horizontal motion?
# convert into horizontal co-ords
dF = r - rTgt
hdF = rotm.dot(dF)
hdF[0] = 0
hv = rotm.dot(v)
hv[0] = 0
#print "Horizontal distance:",hdF[1],"N",hdF[2],"E (m)"
#print "Horizontal speed:",np.linalg.norm(hv)
dhd = np.linalg.norm(hdF + hv) - np.linalg.norm(hdF)
#print "Change in hor. distance:",dhd
# Find point of closest approach (ignoring altitude)
rClose, u = utils.closest_point_on_line(hdF, hv, np.zeros(3))
if u < 0: # behind current position, so current position is closest
rClose = hdF
#print "Closest approach dist:",np.linalg.norm(rClose),"at:",u
d = np.linalg.norm(rClose) # dist to closest point
dz = rClose[2] - hdF[2] # dist in z direction
print "Dist to closest point (East):", dz
throttle = 0
if np.linalg.norm(hv) > 10 and np.linalg.norm(
hdF) < 20000 and h > 10000:
factor = 0.0015
Kp = 0.5
v_wanted = twr * factor * dz
err = hv[2] - v_wanted
print "wanted:", v_wanted, "actual:", hv[2]
throttle = err * Kp # last component to hover
print
return throttle, F
return 0, -v
def compute_thrust(self, vessel, t):
"""Follow the computed trajectory - if there is one"""
# Set Force/direction based on closest trajectory point in speed/position
r = np.array(vessel.position(vessel.orbit.body.reference_frame))
v = np.array(vessel.flight(vessel.orbit.body.reference_frame).velocity)
g = np.array([-vessel.orbit.body.surface_gravity, 0.,
0.]) # in local target ref frame
twr = vessel.max_thrust / vessel.mass
# Convert to local co-ords for target,rotm converting from rotating ref to local, with X=up
# r_Tgt is target position in rotating ref frame
r_Tgt, rotm = utils.target_position_and_rotation(
vessel, self.tgt_lat, self.tgt_lng, self.tgt_alt)
irotm = np.transpose(rotm)
# Gains of 1.0, 0.0 means find closest position only (ignore velocity)
dr, dv, F = self.traj.closest_to_trajectory(r, v, 1.0, 1.0)
if dr == None:
return 0, None # not on trajectory
F2 = rotm.dot(F)
r2 = rotm.dot(r - r_Tgt)
v2 = rotm.dot(v)
dr2 = rotm.dot(dr - r_Tgt)
dv2 = rotm.dot(dv)
self.PID_x.setPoint(dr2[0])
self.PID_y.setPoint(dr2[1])
self.PID_z.setPoint(dr2[2])
self.PID_vx.setPoint(dv2[0])
self.PID_vy.setPoint(dv2[1])
self.PID_vz.setPoint(dv2[2])
# Update PID controllers
px = self.PID_x.update(r2[0])
py = self.PID_y.update(r2[1])
pz = self.PID_z.update(r2[2])
pvx = self.PID_vx.update(v2[0])
pvy = self.PID_vy.update(v2[1])
pvz = self.PID_vz.update(v2[2])
#print "px:",px,"py:",py,"pz:",pz
#print "pvx:",pvx,"pvy:",pvy,"pvz:",pvz
print >> self.fpid, "%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f" % (
t, r2[0], r2[1], r2[2], v2[0], v2[1], v2[2], dr2[0], dr2[1],
dr2[2], dv2[0], dv2[1], dv2[2], px, py, pz, pvx, pvy, pvz)
self.fpid.flush()
# Correct force vector
F2 = F2 + np.array([px, py, pz]) + np.array([pvx, pvy, pvz])
#F2 = F2 + np.array([px,py,pz]) # aim only for position
# Don't thrust down
if F2[0] < 0.1:
throttle = self.steer_throttle
F2 = np.array([0.1, F2[1], F2[2]])
throttle = np.linalg.norm(F2) / twr
F = irotm.dot(F2)
# Shut-off throttle if pointing away from desired direction
att = np.array(
vessel.flight(vessel.orbit.body.reference_frame).direction)
ddot = np.dot(F / np.linalg.norm(F), att / np.linalg.norm(att))
if (ddot < math.cos(math.radians(70))):
throttle = self.steer_throttle # enough to steer
return throttle, F