def draw_heading_bug(self):
color = medium_orchid
size = 2
a = math.atan2(self.ve, self.vn)
q0 = transformations.quaternion_about_axis(self.ap_hdg * d2r,
[0.0, 0.0, -1.0])
center = self.ladder_helper(q0, 0, 0)
pts = []
pts.append(self.ladder_helper(q0, 0, 2.0))
pts.append(self.ladder_helper(q0, 0.0, -2.0))
pts.append(self.ladder_helper(q0, 1.5, -2.0))
pts.append(self.ladder_helper(q0, 1.5, -1.0))
pts.append(center)
pts.append(self.ladder_helper(q0, 1.5, 1.0))
pts.append(self.ladder_helper(q0, 1.5, 2.0))
for i, p in enumerate(pts):
if p == None or center == None:
return
cv2.line(self.frame, pts[0], pts[1], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[1], pts[2], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[2], pts[3], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[3], pts[4], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[4], pts[5], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[5], pts[6], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[6], pts[0], color, self.line_width,
cv2.LINE_AA)
def draw_vbars(self):
color = medium_orchid
size = self.line_width
a1 = 10.0
a2 = 1.5
a3 = 3.0
q0 = transformations.quaternion_about_axis(self.psi_rad,
[0.0, 0.0, -1.0])
a0 = self.ap_pitch
# rotation point (about nose)
rot = self.ladder_helper(q0, self.the_rad * r2d, 0.0)
if rot == None:
return
# center point
tmp1 = self.ladder_helper(q0, a0, 0.0)
if tmp1 == None:
return
center = self.rotate_pt(tmp1, rot, self.ap_roll * d2r)
# right vbar
tmp1 = self.ladder_helper(q0, a0 - a3, a1)
tmp2 = self.ladder_helper(q0, a0 - a3, a1 + a3)
tmp3 = self.ladder_helper(q0, a0 - a2, a1 + a3)
if tmp1 != None and tmp2 != None and tmp3 != None:
uv1 = self.rotate_pt(tmp1, rot, self.ap_roll * d2r)
uv2 = self.rotate_pt(tmp2, rot, self.ap_roll * d2r)
uv3 = self.rotate_pt(tmp3, rot, self.ap_roll * d2r)
if uv1 != None and uv2 != None and uv3 != None:
cv2.line(self.frame, center, uv1, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, center, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv2, uv3, color, self.line_width,
cv2.LINE_AA)
# left vbar
tmp1 = self.ladder_helper(q0, a0 - a3, -a1)
tmp2 = self.ladder_helper(q0, a0 - a3, -a1 - a3)
tmp3 = self.ladder_helper(q0, a0 - a2, -a1 - a3)
if tmp1 != None and tmp2 != None and tmp3 != None:
uv1 = self.rotate_pt(tmp1, rot, self.ap_roll * d2r)
uv2 = self.rotate_pt(tmp2, rot, self.ap_roll * d2r)
uv3 = self.rotate_pt(tmp3, rot, self.ap_roll * d2r)
if uv1 != None and uv2 != None and uv3 != None:
cv2.line(self.frame, center, uv1, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, center, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv2, uv3, color, self.line_width,
cv2.LINE_AA)
def draw_alpha_beta_marker(self):
if self.alpha_rad == None or self.beta_rad == None:
return
q0 = transformations.quaternion_about_axis(self.psi_rad,
[0.0, 0.0, -1.0])
a0 = self.the_rad * r2d
center = self.ladder_helper(q0, a0, 0.0)
alpha = self.alpha_rad * r2d
beta = self.beta_rad * r2d
tmp = self.ladder_helper(q0, a0 - alpha, beta)
if tmp != None:
uv = self.rotate_pt(tmp, center, self.phi_rad)
if uv != None:
r1 = int(round(self.render_h / 60))
r2 = int(round(self.render_h / 30))
uv1 = (uv[0] + r1, uv[1])
uv2 = (uv[0] + r2, uv[1])
uv3 = (uv[0] - r1, uv[1])
uv4 = (uv[0] - r2, uv[1])
uv5 = (uv[0], uv[1] - r1)
uv6 = (uv[0], uv[1] - r2)
cv2.circle(self.frame, uv, r1, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv3, uv4, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv5, uv6, self.color, self.line_width,
cv2.LINE_AA)
def draw_course(self):
color = yellow
size = 2
self.filter_vn = (1.0 -
self.tf_vel) * self.filter_vn + self.tf_vel * self.vn
self.filter_ve = (1.0 -
self.tf_vel) * self.filter_ve + self.tf_vel * self.ve
a = math.atan2(self.filter_ve, self.filter_vn)
q0 = transformations.quaternion_about_axis(a, [0.0, 0.0, -1.0])
uv1 = self.ladder_helper(q0, 0, 0)
uv2 = self.ladder_helper(q0, 1.5, 1.0)
uv3 = self.ladder_helper(q0, 1.5, -1.0)
if uv1 != None and uv2 != None and uv3 != None:
#uv2 = self.rotate_pt(tmp2, tmp1, -self.cam_roll*d2r)
#uv3 = self.rotate_pt(tmp3, tmp1, -self.cam_roll*d2r)
cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA)
cv2.line(self.frame, uv1, uv3, color, self.line_width, cv2.LINE_AA)
def draw_bird(self):
color = yellow
size = 2
a1 = 10.0
a2 = 3.0
q0 = transformations.quaternion_about_axis(self.psi_rad,
[0.0, 0.0, -1.0])
a0 = self.the_rad * r2d
# print 'pitch:', a0, 'ap:', self.ap_pitch
# center point
center = self.ladder_helper(q0, a0, 0.0)
if center == None:
return
# right vbar
tmp1 = self.ladder_helper(q0, a0 - a2, a1)
tmp2 = self.ladder_helper(q0, a0 - a2, a1 - a2)
if tmp1 != None and tmp2 != None:
uv1 = self.rotate_pt(tmp1, center, self.phi_rad)
uv2 = self.rotate_pt(tmp2, center, self.phi_rad)
if uv1 != None and uv2 != None:
cv2.line(self.frame, center, uv1, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, center, uv2, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width,
cv2.LINE_AA)
# left vbar
tmp1 = self.ladder_helper(q0, a0 - a2, -a1)
tmp2 = self.ladder_helper(q0, a0 - a2, -a1 + a2)
if tmp1 != None and tmp2 != None:
uv1 = self.rotate_pt(tmp1, center, self.phi_rad)
uv2 = self.rotate_pt(tmp2, center, self.phi_rad)
if uv1 != None and uv2 != None:
cv2.line(self.frame, center, uv1, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, center, uv2, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width,
cv2.LINE_AA)
def draw_pitch_ladder(self, beta_rad=0.0):
a1 = 2.0
a2 = 8.0
#slide_rad = self.psi_rad - beta_rad
slide_rad = self.psi_rad
q0 = transformations.quaternion_about_axis(slide_rad, [0.0, 0.0, -1.0])
for a0 in range(5, 35, 5):
# above horizon
# right horizontal
uv1 = self.ladder_helper(q0, a0, a1)
uv2 = self.ladder_helper(q0, a0, a2)
if uv1 != None and uv2 != None:
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
du = uv2[0] - uv1[0]
dv = uv2[1] - uv1[1]
uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv))
self.draw_label("%d" % a0, uv, self.font_size, self.line_width)
# right tick
uv1 = self.ladder_helper(q0, a0 - 0.5, a1)
uv2 = self.ladder_helper(q0, a0, a1)
if uv1 != None and uv2 != None:
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
# left horizontal
uv1 = self.ladder_helper(q0, a0, -a1)
uv2 = self.ladder_helper(q0, a0, -a2)
if uv1 != None and uv2 != None:
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
du = uv2[0] - uv1[0]
dv = uv2[1] - uv1[1]
uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv))
self.draw_label("%d" % a0, uv, self.font_size, self.line_width)
# left tick
uv1 = self.ladder_helper(q0, a0 - 0.5, -a1)
uv2 = self.ladder_helper(q0, a0, -a1)
if uv1 != None and uv2 != None:
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
# below horizon
# right horizontal
uv1 = self.ladder_helper(q0, -a0, a1)
uv2 = self.ladder_helper(q0, -a0 - 0.5, a2)
if uv1 != None and uv2 != None:
du = uv2[0] - uv1[0]
dv = uv2[1] - uv1[1]
for i in range(0, 3):
tmp1 = (uv1[0] + int(0.375 * i * du),
uv1[1] + int(0.375 * i * dv))
tmp2 = (tmp1[0] + int(0.25 * du), tmp1[1] + int(0.25 * dv))
cv2.line(self.frame, tmp1, tmp2, self.color,
self.line_width, cv2.LINE_AA)
uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv))
self.draw_label("%d" % a0, uv, self.font_size, self.line_width)
# right tick
uv1 = self.ladder_helper(q0, -a0 + 0.5, a1)
uv2 = self.ladder_helper(q0, -a0, a1)
if uv1 != None and uv2 != None:
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
# left horizontal
uv1 = self.ladder_helper(q0, -a0, -a1)
uv2 = self.ladder_helper(q0, -a0 - 0.5, -a2)
if uv1 != None and uv2 != None:
du = uv2[0] - uv1[0]
dv = uv2[1] - uv1[1]
for i in range(0, 3):
tmp1 = (uv1[0] + int(0.375 * i * du),
uv1[1] + int(0.375 * i * dv))
tmp2 = (tmp1[0] + int(0.25 * du), tmp1[1] + int(0.25 * dv))
cv2.line(self.frame, tmp1, tmp2, self.color,
self.line_width, cv2.LINE_AA)
uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv))
self.draw_label("%d" % a0, uv, self.font_size, self.line_width)
# left tick
uv1 = self.ladder_helper(q0, -a0 + 0.5, -a1)
uv2 = self.ladder_helper(q0, -a0, -a1)
if uv1 != None and uv2 != None:
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)