diff --git a/sw/in_progress/python/attitude_viz.py b/sw/in_progress/python/attitude_viz.py index 695f3ef661..ebc5074440 100755 --- a/sw/in_progress/python/attitude_viz.py +++ b/sw/in_progress/python/attitude_viz.py @@ -52,6 +52,8 @@ class Visualization: self.airspeed = 0.0 self.display_list = None self.display_dirty = True + self.rotate_theta = parent.rotate_theta + for message_name, index, name, bfp in VEHICLE_QUATS: self.quats.append(TelemetryQuat(message_name, index, name, bfp)) for message_name, index, name, offset, scale, max in BAR_VALUES: @@ -227,7 +229,8 @@ class Visualization: try: scaled_quat = [telemetry_quat.qi * telemetry_quat.scale, telemetry_quat.qx * telemetry_quat.scale, telemetry_quat.qy * telemetry_quat.scale, telemetry_quat.qz * telemetry_quat.scale] glRotate(360 * math.acos(scaled_quat[0] ) / math.pi, scaled_quat[2], -scaled_quat[3], -scaled_quat[1]) - glRotate(-90, 1, 0, 0) + glRotate(self.rotate_theta, 1, 0, 0) + self.DrawVehicle(telemetry_quat.name) except Exception: raise Exception @@ -237,7 +240,8 @@ class Visualization: glPopMatrix() class Visualizer: - def __init__(self): + def __init__(self, rotate_theta): + self.rotate_theta = rotate_theta self.visualization = Visualization(self) # listen to Ivy @@ -326,21 +330,25 @@ def run(): VEHICLE_QUATS = [ ["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] BAR_VALUES = [ ["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100] ] window_title = "Attitude_Viz" + rotate_theta = -90 try: - opts, args = getopt.getopt(sys.argv[1:], "t:", ["title"]) + opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"]) for o, a in opts: if o in ("-t", "--title"): window_title = a + if o in ("-r", "--rotate_theta"): + rotate_theta = int(a) except getopt.error as msg: print(msg) print("""usage: -t, --title set window title +-r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: -90) """) pygame.init() screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL|pygame.DOUBLEBUF) #resize(*SCREEN_SIZE) init() - visualizer = Visualizer() + visualizer = Visualizer(rotate_theta) try: while True: