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, \