def conv_udb_vpython(vector):
"""Convert a vector from UDB coordinate reference to vpython reference"""
# conversion from UDB coordinates to V python coordinates.
# X axis is out of left wing in both coordinate systems.
# Y is straight up in vpython, and is the negative of the UDB's Z axis,
# which points down.
# Z in vpython coordinates is the Y axis in UDB coordinates.
# So it turns out that one can covert from UDB to Vpython by means
# of a 90 degree rotation matrix.
vpython_rotate = (1, 0, 0, 0, 0, -1, 0, 1, 0)
return matrix_multiply_3x3_3x1(vpython_rotate, vector)
def conv_udb_vpython(vector):
"""Convert a vector from UDB coordinate reference to vpython reference"""
# conversion from UDB coordinates to V python coordinates.
# X axis is out of left wing in both coordinate systems.
# Y is straight up in vpython, and is the negative of the UDB's Z axis,
# which points down.
# Z in vpython coordinates is the Y axis in UDB coordinates.
# So it turns out that one can covert from UDB to Vpython by means
# of a 90 degree rotation matrix.
vpython_rotate = (1, 0, 0, 0, 0, -1, 0, 1, 0)
return (matrix_multiply_3x3_3x1(vpython_rotate, vector))
line = ser.readline()
result = telemetry_line.parse(line, line_no, max_tm_actual)
print line
if result == "F2": # If received SERIAL_UDB or SERIAL_UDB_EXTRA format telemetry
rmat[0] = telemetry_line.rmat0 / 16384.0
rmat[1] = telemetry_line.rmat1 / 16384.0
rmat[2] = telemetry_line.rmat2 / 16384.0
rmat[3] = telemetry_line.rmat3 / 16384.0
rmat[4] = telemetry_line.rmat4 / 16384.0
rmat[5] = telemetry_line.rmat5 / 16384.0
rmat[6] = telemetry_line.rmat6 / 16384.0
rmat[7] = telemetry_line.rmat7 / 16384.0
rmat[8] = telemetry_line.rmat8 / 16384.0
fuselage_vector_rotated = matrix_multiply_3x3_3x1(rmat, fuselage_vector_udb)
fuselage_plane.axis = conv_udb_vpython(fuselage_vector_rotated)
fuselage_plane.length = f_length
fuselage_plane.pos = (0, 0, 0)
left_wing_vector_rotated = matrix_multiply_3x3_3x1(rmat, left_wing_vector_udb)
left_wing.axis = conv_udb_vpython(left_wing_vector_rotated)
left_wing.length = w_length
left_wing.pos = (0, 0, 0)
right_wing_vector_rotated = matrix_multiply_3x3_3x1(rmat, right_wing_vector_udb)
right_wing.axis = conv_udb_vpython(right_wing_vector_rotated)
right_wing.length = w_length
right_wing.pos = (0, 0, 0)
magnetic_scale_factor = (
10
) # Change this to scale the line appropriately. The vector should be about 1 unit in length.
mag_udb_axis = (
line = ser.readline()
result = telemetry_line.parse(line, line_no, max_tm_actual)
print line
if result == "F2": # If received SERIAL_UDB or SERIAL_UDB_EXTRA format telemetry
rmat[0] = telemetry_line.rmat0 / 16384.0
rmat[1] = telemetry_line.rmat1 / 16384.0
rmat[2] = telemetry_line.rmat2 / 16384.0
rmat[3] = telemetry_line.rmat3 / 16384.0
rmat[4] = telemetry_line.rmat4 / 16384.0
rmat[5] = telemetry_line.rmat5 / 16384.0
rmat[6] = telemetry_line.rmat6 / 16384.0
rmat[7] = telemetry_line.rmat7 / 16384.0
rmat[8] = telemetry_line.rmat8 / 16384.0
fuselage_vector_rotated = matrix_multiply_3x3_3x1(
rmat, fuselage_vector_udb)
fuselage_plane.axis = conv_udb_vpython(fuselage_vector_rotated)
fuselage_plane.length = f_length
fuselage_plane.pos = (0, 0, 0)
left_wing_vector_rotated = matrix_multiply_3x3_3x1(
rmat, left_wing_vector_udb)
left_wing.axis = conv_udb_vpython(left_wing_vector_rotated)
left_wing.length = w_length
left_wing.pos = (0, 0, 0)
right_wing_vector_rotated = matrix_multiply_3x3_3x1(
rmat, right_wing_vector_udb)
right_wing.axis = conv_udb_vpython(right_wing_vector_rotated)
right_wing.length = w_length
right_wing.pos = (0, 0, 0)
magnetic_scale_factor = 10 # Change this to scale the line appropriately. The vector should be about 1 unit in length.
mag_udb_axis = (telemetry_line.earth_mag_vec_E / magnetic_scale_factor, \
