def followWpp(self, w, p, newpath):
"""
followWpp implements waypoint following via connected straight-line
paths.
Inputs:
w = 3xn matrix of waypoints in NED (m)
p = position of MAV in NED (m)
newpath = flag to initialize the algorithm or define new waypoints
Outputs
r = origin of straight-line path in NED (m)
q = direction of straight-line path in NED (m)
Example Usage;
r, q = followWpp(w, p, newpath)
Reference: Beard, Small Unmanned Aircraft, Chapter 11, Algorithm 5
Copyright 2018 Utah State University
"""
if self.i is None:
self.i = 0
if newpath:
# initialize index
self.i = 1
# check sizes
m, N = w.shape
assert N >= 3
assert m == 3
# calculate the q vector
r = w[:, self.i - 1]
qi1 = s_norm(w[:, self.i], -w[:, self.i - 1])
# Calculate the origin of the current path
qi = s_norm(w[:, self.i + 1], -w[:, self.i])
# Calculate the unit normal to define the half plane
ni = s_norm(qi1, qi)
# Check if the MAV has crossed the half-plane
if in_half_plane(p, w[:, self.i], ni):
if self.i < (N - 2):
self.i += 1
q = qi1
return r, q
def followWppDubins(self, W, Chi, p, R, newpath):
"""
followWppDubins implements waypoint following via Dubins paths
Inputs:
W = list of waypoints in NED (m)
Chi = list of course angles at waypoints in NED (rad)
p = mav position in NED (m)
R = fillet radius (m)
newpath = flag to initialize the algorithm or define new waypoints
Outputs
flag = flag for straight line path (1) or orbit (2)
r = origin of straight-line path in NED (m)
q = direction of straight-line path in NED (m)
c = center of orbit in NED (m)
rho = radius of orbit (m)
lamb = direction or orbit, 1 clockwise, -1 counter clockwise
self.i = waypoint number
dp = dubins path parameters
Example Usage
flag, r, q, c, rho, lamb = followWppDubins(W, Chi, p, R, newpath)
Reference: Beard, Small Unmanned Aircraft, Chapter 11, Algorithm 8
Copyright 2018 Utah State University
"""
# TODO Algorithm 8 goes here
# state = 0
if not self.i: # checks if i is empty
self.i = 0
self.state = 0
if newpath:
self.i = 1
self.state = 1
m, N = W.shape
assert N >= 3
assert m == 3
else:
m, N = W.shape
assert N >= 3
assert m == 3
dp = self.findDubinsParameters(W[:, self.i - 1], Chi[self.i - 1, :],
W[:, self.i], Chi[self.i, :], R)
if self.state == 1:
# Follow start orbit until on the correct side of H1
flag = 2
c = dp.c_s
rho = R
lamb = dp.lamb_s
r = np.empty((3, 1))
r[:] = np.nan
q = np.empty((3, 1))
q[:] = np.nan
if in_half_plane(p, dp.z_1, -dp.q_12):
self.state = 2
elif self.state == 2:
# Continue following the start orbit until in H1
flag = 2
c = dp.c_s
rho = R
lamb = dp.lamb_s
r = np.empty((3, 1))
r[:] = np.nan # if nothing else works, tryo to put 'zeros' in place of 'nan'
q = np.empty((3, 1))
q[:] = np.nan
if in_half_plane(p, dp.z_1, dp.q_12):
self.state = 3
elif self.state == 3:
# Transition to straight-line path until in H2
flag = 1
r = dp.z_1
q = dp.q_12
c = np.empty((3, 1))
c[:] = np.nan
rho = np.nan
lamb = np.nan
if in_half_plane(p, dp.z_2, dp.q_12):
self.state = 4
elif self.state == 4:
flag = 2
c = dp.c_e
rho = R
lamb = dp.lamb_e
r = np.empty((3, 1))
r[:] = np.nan
q = np.empty((3, 1))
q[:] = np.nan
if in_half_plane(p, dp.z_3, -dp.q_3):
self.state = 5
else: # state == 5
# Continue following the end orbit until in H3
flag = 2
c = dp.c_e
rho = R
lamb = dp.lamb_e
r = np.empty((3, 1))
r[:] = np.nan
q = np.empty((3, 1))
q[:] = np.nan
if in_half_plane(p, dp.z_3, dp.q_3):
self.state = 1
self.i = self.i + 1
if self.i >= N:
self.i = N
return flag, r, q, c, rho, lamb, self.i, dp
def followWppFillet(self, w, p, R, newpath):
"""
followWppFillet implements waypoint following via straightline paths
connected by fillets
Inputs:
W = 3xn matrix of waypoints in NED (m)
p = position of MAV in NED (m)
R = fillet radius (m)
newpath = flag to initialize the algorithm or define new waypoints
Outputs
flag = flag for straight line path (1) or orbit (2)
r = origin of straight-line path in NED (m)
q = direction of straight-line path in NED (m)
c = center of orbit in NED (m)
rho = radius of orbit (m)
lamb = direction or orbit, 1 clockwise, -1 counter clockwise
Example Usage
[flag, r, q, c, rho, lamb] = followWppFillet( w, p, R, newpath )
Reference: Beard, Small Unmanned Aircraft, Chapter 11, Algorithm 6
Copyright 2018 Utah State University
"""
if self.i is None:
self.i = 0
self.state = 0
if newpath:
# Initialize the waypoint index
self.i = 2
self.state = 1
# Check size of waypoints matrix
m, N = W.shape # Where 'N' is the number of waypoints and 'm' dimensions
assert N >= 3
assert m == 3
else:
[m, N] = W.shape
assert N >= 3
assert m == 3
# Calculate the q vector and fillet angle
qi1 = mat(s_norm(w[:, self.i], -w[:, self.i - 1]))
qi = mat(s_norm(w[:, self.i + 1], -w[:, self.i]))
e = acos(-qi1.T * qi)
# Determine if the MAV is on a straight or orbit path
if self.state == 1:
c = mat([0, 0, 0]).T
rho = 0
lamb = 0
flag = 1
r = w[:, self.i - 1]
q = q1
z = w[:, self.i] - (R / (np.tan(e / 2))) * qi1
if in_half_plane(p, z, qi1):
self.state = 2
elif self.state == 2:
r = [0, 0, 0]
q = [0, 0, 0]
flag = 2
c = w[:, self.i] - (R / (np.sin(e / 2))) * s_norm(qi1, -qi)
rho = R
lamb = np.sign(qi1(1) * qi(2) - qi1(2) * qi(1))
z = w[:, self.i] + (R / (np.tan(e / 2))) * qi
if in_half_plane(p, z, qi):
if self.i < (N - 1):
self.i = self.i + 1
self.state = 1
else:
# Fly north as default
flag = -1
r = p
q = mat([1, 0, 0]).T
c = np.nan(3, 1)
rho = np.nan
lamb = np.nan
return flag, r, q, c, rho, lamb
def pathFollower(self, flag, r, q, p, chi, chi_inf, k_path, c, rho, lamb,
k_orbit):
"""
Input:
flag = 1 for straight line, 2 for orbit
r = origin of straight-line path in NED (m)
q = direction of straight-line path in NED (m)
p = current position of uav in NED (m)
chi = course angle of UAV (rad)
chi_inf = straight line path following parameter
k_path = straight line path following parameter
c = center of orbit in NED (m)
rho = radius of orbit (m)
lamb = direction of orbit, 1 clockwise, -1 counter-clockwise
k_orbit = orbit path following parameter
Outputs:
e_crosstrack = crosstrack error (m)
chi_c = commanded course angle (rad)
h_c = commanded altitude (m)
Example Usage
e_crosstrack, chi_c, h_c = pathFollower(path)
Reference: Beard, Small Unmanned Aircraft, Chapter 10, Algorithms 3 and 4
Copyright 2018 Utah State University
"""
# Unpackaging variables
r_n = r[0][0]
r_e = r[1][0]
r_d = r[2][0]
p_n = p[0][0]
p_e = p[1][0]
p_d = p[2][0]
k_i = np.array([0, 0, 1])
e_i_p = np.array([[p_n - r_n], [p_e - r_e], [p_d - r_d]]) # 3x1
normal = (np.cross(q.T, k_i) /
(np.linalg.norm(np.cross(q.T, k_i)))).T # 3x1
s_i = np.subtract(e_i_p, (np.dot(e_i_p.T, normal) * normal)) # 3x1
s_n = s_i[0]
s_e = s_i[1]
# s_d =
q_n = q[0]
q_e = q[1]
q_d = q[2]
chi_q = np.arctan2(q_e, q_n)
chi = chi
c_n = c[0][0]
c_e = c[1][0]
c_d = c[2][0]
lamb = lamb
k_orbit = 3.6
rho = 55
if flag == 1: # straight line
pass
# TODO Algorithm 3 goes here
h_d = -r_d + np.sqrt(s_n**2 + s_e**2) * (
q_d / np.sqrt(q_n**2 + q_e**2)) #equation 10.5
chi_q = np.arctan2(q_e, q_n)
while (chi_q - chi) < -np.pi:
chi_q = chi_q + 2 * np.pi
while (chi_q - chi) > np.pi:
chi_q = chi_q - 2 * np.pi
e_py = -np.sin(chi_q) * (p_n - r_n) + np.cos(chi_q) * (p_e - r_e)
e_crosstrack = e_py
chi_c = chi_q - chi_inf * 2 / np.pi * np.arctan(k_path * e_py)
h_c = h_d
elif flag == 2: # orbit following
pass
# TODO Algorithm 4 goes here
h_c = -c_d
d = np.sqrt((p_n - c_n)**2 + (p_e - c_e)**2)
psi = np.arctan2(p_e - c_e, p_n - c_n)
while (psi - chi) < -np.pi:
psi = psi + 2 * np.pi
while (psi - chi) > np.pi:
psi = psi - 2 * np.pi
chi_c = 0.6 + psi + lamb * (np.pi / 2 + np.arctan(k_orbit * (
(d - rho) / rho)))
e_crosstrack = d - rho
else:
raise Exception("Invalid path type")
return e_crosstrack, chi_c, h_c
# followWpp algorithm left here for reference
# It is not used in the final implementation
# def followWpp(self, w, p, newpath):
"""
followWpp implements waypoint following via connected straight-line
paths.
Inputs:
w = 3xn matrix of waypoints in NED (m)
p = position of MAV in NED (m)
newpath = flag to initialize the algorithm or define new waypoints
Outputs
r = origin of straight-line path in NED (m)
q = direction of straight-line path in NED (m)
Example Usage;
r, q = followWpp(w, p, newpath)
Reference: Beard, Small Unmanned Aircraft, Chapter 11, Algorithm 5
Copyright 2018 Utah State University
"""
if self.i is None:
self.i = 0
if newpath:
# initialize index
self.i = 1
# check sizes
m, N = W.shape
assert N >= 3
assert m == 3
# calculate the q vector
r = w[:, self.i - 1]
qi1 = s_norm(w[:, self.i], -w[:, self.i - 1])
# Calculate the origin of the current path
qi = s_norm(w[:, self.i + 1], -w[:, self.i])
# Calculate the unit normal to define the half plane
ni = s_norm(qi1, qi)
# Check if the MAV has crossed the half-plane
if in_half_plane(p, w[:, self.i], ni):
if self.i < (N - 2):
self.i += 1
q = qi1
return r, q
def followWppDubins(self, W, Chi, p, R, newpath):
"""
followWppDubins implements waypoint following via Dubins paths
Inputs:
W = list of waypoints in NED (m)
Chi = list of course angles at waypoints in NED (rad)
p = mav position in NED (m)
R = fillet radius (m)
newpath = flag to initialize the algorithm or define new waypoints
Outputs
flag = flag for straight line path (1) or orbit (2)
r = origin of straight-line path in NED (m)
q = direction of straight-line path in NED (m)
c = center of orbit in NED (m)
rho = radius of orbit (m)
lamb = direction or orbit, 1 clockwise, -1 counter clockwise
self.i = waypoint number
dp = dubins path parameters
Example Usage
flag, r, q, c, rho, lamb = followWppDubins(W, Chi, p, R, newpath)
Reference: Beard, Small Unmanned Aircraft, Chapter 11, Algorithm 8
Copyright 2018 Utah State University
"""
if self.i is None:
self.i = 0
self.state = 0
if newpath:
# Initialize the waypoint index
self.i = 1
self.state = 1
# Check size of waypoints matrix
m, N = W.shape # 'N' is number of waypoints and 'm' dimensions
assert N >= 3
assert m == 3
else:
[m, N] = W.shape
assert N >= 3
assert m == 3
#print("Calculating Start Position...")
# Determine the Dubins path parameters
p_s = mat([[W[0, self.i - 1]], [W[1, self.i - 1]], [W[2,
self.i - 1]]]).T
#print("Calculating Start Attitude...")
chi_s = Chi[self.i - 1],
#print("Calculating End Position...")
p_e = mat([[W[0, self.i]], [W[1, self.i]], [W[2, self.i]]]).T
#print("Calculating End Attitude...")
chi_e = Chi[self.i]
#print("W = ", W)
#print("Finding Dubins Parameters...\n")
dp = self.findDubinsParameters(p_s, chi_s, p_e, chi_e, R)
#print("Dubins Parameters found\n")
r = mat([[0], [0], [0]]).T
q = mat([[0], [0], [0]]).T
c = mat([[0], [0], [0]]).T
rho = 0
lamb = 0
print("state = ", self.state)
# ... Follow start orbit until on the correct side of H1
if self.state == 1:
flag = 2
c = dp.c_s
rho = R
lamb = dp.lamb_s
if in_half_plane(p, dp.z_1, -dp.q_1):
self.state = 2
# ... Continue following the start orbit until in H1
elif self.state == 2:
flag = 2
c = dp.c_s
rho = R
lamb = dp.lamb_s
if in_half_plane(p, dp.z_1, dp.q_1):
self.state = 3
# ... Transition to straight-line path until H2
elif self.state == 3:
flag = 1
r = dp.z_1
q = dp.q_1
if in_half_plane(p, dp.z_2, dp.q_1):
self.state = 4
# ... Follow the end orbit until on the correct side of H3
elif self.state == 4:
flag = 2
c = dp.c_e
rho = R
lamb = dp.lamb_e
if in_half_plane(p, dp.z_3, -dp.q_3):
self.state = 5
# state = 5
else:
flag = 2
c = dp.c_e
rho = R
lamb = dp.lamb_e
# ... Continue following the end oribit until in H3
if in_half_plane(p, dp.z_3, dp.q_3):
self.state = 1
if (self.i < N):
self.i = self.i + 1
dp = self.findDubinsParameters(p_s, chi_s, p_e, chi_e, R)
return flag, r, q, c, rho, lamb, self.i, dp