In [1]:
import krpc
import numpy as np
import time
import math
In [2]:
linkup = krpc.connect('192.168.1.2', name='flight control')
In [3]:
ksc = linkup.space_center
vessel = ksc.active_vessel
In [4]:
def cross_product(x, y):
return (x[1]*y[2] - x[2]*y[1], x[2]*y[0] - x[0]*y[2], x[0]*y[1] - x[1]*y[0])
def dot_product(x, y):
return x[0]*y[0] + x[1]*y[1] + x[2]*y[2]
def magnitude(x):
return math.sqrt(x[0]**2 + x[1]**2 + x[2]**2)
def vector_proection(vec, norm):
return dot_product(vec,norm) / magnitude(norm)
def vector_projection_on_plane(vec, plane_norm):
pass
def angle_between_vectors(x, y):
""" Compute the angle between vector x and y """
dp = dot_product(x, y)
if dp == 0:
return 0
xm = magnitude(x)
ym = magnitude(y)
return math.acos(dp / (xm*ym)) * (180. / math.pi)
def angle_between_vector_and_plane(x, n):
""" Compute the angle between a vector x and plane with normal vector n """
dp = dot_product(x,n)
if dp == 0:
return 0
xm = magnitude(x)
nm = magnitude(n)
return math.asin(dp / (xm*nm)) * (180. / math.pi)
In [ ]:
def vertical_launch_orientation_initial(ksc, vessel):
'''Orientation for vertical launch of roundish vehicles. Initial launch.
good while the vessel pointed mostly straight up
pitch: angle between forward and horizon due-starboard
roll: angle between starboard and east
'''
up = (1,0,0)
east = (0,0,1)
forward = ksc.transform_direction((0,1,0), vessel.reference_frame, vessel.surface_reference_frame)
starboard = ksc.transform_direction((1,0,0), vessel.reference_frame, vessel.surface_reference_frame)
B = A - (A.dot.N)N
starboard_horizon = tuple(s-u for s,u in zip(starboard,up))
pitch = angle_between_vectors(forward, starboard_horizon)
roll = angle_between_vectors(starboard, east)
return pitch, roll
def vertical_launch_orientation_low(ksc, vessel):
'''Orientation for vertical launch of roundish vehicles. Low-altitude.
good once vessel has a definite heading
good until vessel is pointed about 45 deg above horizon
pitch: angle between forward and up
roll: angle between starboard and east
heading:
'''
east = (0,0,1)
flight = vessel.flight(vessel.surface_reference_frame)
starboard = ksc.transform_direction((1,0,0), vessel.reference_frame, vessel.surface_reference_frame)
roll = angle_between_vectors(starboard, east)
return flight.pitch, flight.heading, roll
def vertical_launch_orientation_high(ksc, vessel):
'''Orientation for vertical launch of roundish vehicles. High-altitude to orbit.
good once vessel is pointed to within 45 deg of the horizon
pitch: angle between forward and up
roll: angle between starboard and east
'''
flight = vessel.flight(vessel.surface_reference_frame)
return flight.pitch, flight.heading, flight.roll
In [5]:
def vessel_surface_orientation(ksc, vessel):
vessel_direction = vessel.direction(vessel.surface_reference_frame)
# Get the direction of the vessel in the horizon plane
horizon_direction = (0, vessel_direction[1], vessel_direction[2])
# Compute the pitch - the angle between the vessels direction
# and the direction in the horizon plane
pitch = angle_between_vectors(vessel_direction, horizon_direction)
if vessel_direction[0] < 0:
pitch = -pitch
# Compute the heading - the angle between north
# and the direction in the horizon plane
north = (0,1,0)
heading = angle_between_vectors(north, horizon_direction)
if horizon_direction[2] < 0:
heading = 360 - heading
# Compute the roll
# Compute the plane running through the vessels direction
# and the upwards direction
up = (1,0,0)
plane_normal = cross_product(vessel_direction, up)
# Compute the upwards direction of the vessel
vessel_up = ksc.transform_direction(
(0,0,-1), vessel.reference_frame, vessel.surface_reference_frame)
# Compute the angle between the upwards direction of the vessel and the plane
roll = angle_between_vector_and_plane(vessel_up, plane_normal)
# Adjust so that the angle is between -180 and 180 and
# rolling right is +ve and left is -ve
if vessel_up[0] > 0:
roll *= -1
elif roll < 0:
roll += 180
else:
roll -= 180
return pitch, heading, roll
In [6]:
print(vessel_surface_orientation(ksc,vessel))
In [7]:
import wernher
In [8]:
con = wernher.Controller()
In [9]:
con.kp = .1
con.ki = 0.05
con.kd = .2
con.cmin = -1
con.cmax = 1
In [10]:
tt,roll,conroll = [],[],[]
con.t0 = ksc.ut
con.set_point = vessel_surface_orientation(ksc,vessel)[2]*3.14159/180
while True:
t = ksc.ut
p,h,r = vessel_surface_orientation(ksc,vessel)
c = con(r*3.14159/180,t)
tt.append(t)
roll.append(r)
conroll.append(c)
vessel.control.roll = c
time.sleep(1/100)
In [13]:
tt = np.array(tt)
roll = np.array(roll)
conroll = np.array(conroll)
In [24]:
%matplotlib inline
from matplotlib import pyplot
fig,ax = pyplot.subplots(2)
ax[0].plot(tt[:10],roll[:10])
ax[1].plot(tt[:10],conroll[:10])
Out[24]:
In [21]:
print(tt)
In [22]:
print(roll)
In [23]:
print(conroll)