def read_control(conn): global PACKET, PACKETS, LAST_CONTROL data = conn.read() got_message = False while data: for ch in data: if ch == "\x00" or PACKET: if ch == "\x00" and PACKET: if PACKET[-1] == "\x00": PACKET = PACKET[:-1] else: got_message = True PACKET = PACKET + ch if len(PACKET) > 1024: PACKET = "" if got_message: PACKETS.append(PACKET) PACKET = "" got_message = False data = conn.read() last_packet = PACKETS[-1] if PACKETS else None PACKETS = [] try: control_log = plog.ParameterLog.deserialize(last_packet) # print control_log control_param = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT) path, err_type = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID).values refp = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_KEY_VALUE) cycles, obj_val, errors, resets = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS).values LAST_CONTROL = ( map(lambda x: float(x) / float(2**16), control_param.values), plog.extract_waypoint(refp.value), obj_val, cycles, errors, resets, control_log.tick, path, err_type ) return LAST_CONTROL except Exception: return LAST_CONTROL or ([0.0, 0.5, 0.5], {}, 0, 0, 0, 0, 0, 0xFFFF, 0x0)
def tick(t, data): global LAST_CONTROL_DATA state_pos = None state_velocity = (-1, -1, -1) state_wind = (0, 0, 0) state_attitude = (0, 0, 0) state_angular_velocity = (0, 0, 0) control_refp = None control_path = -1 control_cycles = -1 control_obj_val = -1 control_errors = -1 control_resets = -1 control_values = (-1, -1, -1) control_mode = [-1, -1] control_nav_version = -1 control_pos = (-1, -1, -1) control_error_type = 0 last_gps = 0 last_data = 0 payload_release = False for param in data: pt = param.parameter_type if isinstance(param, plog.DataParameter): pv = param.values else: pv = param.value if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA: state_pos = ( pv[0] * math.pi / 2**31, pv[1] * math.pi / 2**31, pv[2] * 1e-2 ) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED: state_velocity = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q: att_q = map(lambda x: float(x) / 32767.0, pv) state_attitude = plog.q_to_euler(att_q) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ: state_angular_velocity = map( lambda x: math.degrees(float(x) / (32767.0 / math.pi * 0.25)), pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED: state_wind = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE: mode = chr(pv[0]) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE: control_mode[param.device_id] = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT: control_values = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS and param.device_id == 1: control_pos = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID: control_path = pv[0] control_error_type = pv[1] if len(pv) > 1 else 0 elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE: control_refp = plog.extract_waypoint(pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS: control_cycles = pv[0] control_obj_val = pv[1] control_errors = pv[2] control_resets = pv[3] elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS: last_gps = pv[0] last_data = pv[1] elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION: control_nav_version = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_GP_OUT: payload_release = True if pv[0] else False control_data = (control_cycles, control_obj_val, control_errors, control_resets) if not state_pos or not control_refp or LAST_CONTROL_DATA == control_data: return "" #if control_mode[1] != 1: # return "" LAST_CONTROL_DATA = control_data state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 + (state_velocity[1] - state_wind[1])**2 + (state_velocity[2] - state_wind[2])**2) state_groundspeed = math.sqrt(state_velocity[0] ** 2 + state_velocity[1] ** 2) ned_off = plog.lla_to_ned( state_pos, (control_refp["lat"], control_refp["lon"], control_refp["alt"])) ref_v = ( control_refp["airspeed"] * math.cos(control_refp["yaw"]) + state_wind[0], control_refp["airspeed"] * math.sin(control_refp["yaw"]) + state_wind[1] ) ate = abs((ned_off[0] * ref_v[0] + ned_off[1] * ref_v[1]) / \ math.sqrt(ref_v[0] ** 2 + ref_v[1] ** 2)) xte = math.sqrt((ned_off[0] ** 2 + ned_off[1] ** 2) - ate ** 2) data = ( (t, ) + (state_pos[2], control_refp["alt"]) + ned_off + (xte, ate) + (state_airspeed, control_refp["airspeed"], state_groundspeed) + (math.degrees(math.atan2(state_velocity[1], state_velocity[0])), math.degrees(math.atan2(ref_v[1], ref_v[0]))) + (state_attitude[0], math.degrees(control_refp["yaw"])) + (state_attitude[1], math.degrees(control_refp["pitch"])) + (state_attitude[2], math.degrees(control_refp["roll"])) + (state_angular_velocity[2], state_angular_velocity[1], state_angular_velocity[0]) + (math.sqrt(state_wind[0] ** 2 + state_wind[1] ** 2), state_wind[0], state_wind[1]) + (control_values[0], control_values[1], control_values[2]) + (control_pos[0], control_pos[1], control_pos[2]) + (control_obj_val, control_cycles, control_errors, control_resets) + (control_path, last_gps, control_mode[1], control_refp["flags"], control_error_type, 1 if payload_release else 0) ) format = ( "t=%8d, " + "alt=%3.3f, alt_ref=%3.3f, " + "n=%6.2f, e=%6.2f, d=%6.2f, xte=%4.2f, ate=%4.2f, " + "tas=%5.2f, tas_ref=%5.2f, gs=%5.2f, " + "heading=%3.0f, heading_ref=%3.0f, " + "yaw=%3.0f, yaw_ref=%3.0f, " + "pitch=%4.0f, pitch_ref=%4.0f, " + "roll=%4.0f, roll_ref=%4.0f, " + "vyaw=%4.0f, vpitch=%4.0f, vroll=%4.0f, " + "wind=%5.2f, wind_n=%5.2f, wind_e=%5.2f, " + "t=%.3f, l=%.3f, r=%.3f, " + "tp=%.3f, lp=%.3f, rp=%.3f, " + "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " + "path=%4d, last_gps=%4d, mode1=%d, flags=%08x, err=%04x, release=%d\n" ) dump = ( "%d," + "%.3f,%.3f," + "%.2f,%.2f,%.2f,%.2f,%.2f," + "%.2f,%.2f,%.2f," + "%.1f,%.1f," + "%.1f,%.1f," + "%.1f,%.1f," + "%.1f,%.1f," + "%.1f,%.1f,%.1f," + "%.2f,%.2f,%.2f," + "%.3f,%.3f,%.3f," + "%.3f,%.3f,%.3f," + "%.1f,%d,%d,%d," + "%d,%d,%d,%d,%d,%d\n" ) #return format % data return dump % data
def tick(t, data): global LAST_CONTROL_DATA state_pos = None state_velocity = (-1, -1, -1) state_wind = (0, 0, 0) state_attitude = (0, 0, 0) state_angular_velocity = (0, 0, 0) control_refp = None control_path = -1 control_cycles = -1 control_obj_val = -1 control_errors = -1 control_resets = -1 control_values = (-1, -1, -1) control_mode = [-1, -1] control_nav_version = -1 control_pos = (-1, -1, -1) control_error_type = 0 last_gps = 0 last_data = 0 payload_release = False for param in data: pt = param.parameter_type if isinstance(param, plog.DataParameter): pv = param.values else: pv = param.value if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA: state_pos = (pv[0] * math.pi / 2**31, pv[1] * math.pi / 2**31, pv[2] * 1e-2) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED: state_velocity = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q: att_q = map(lambda x: float(x) / 32767.0, pv) state_attitude = plog.q_to_euler(att_q) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ: state_angular_velocity = map( lambda x: math.degrees(float(x) / (32767.0 / math.pi * 0.25)), pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED: state_wind = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE: mode = chr(pv[0]) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE: control_mode[param.device_id] = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT: control_values = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS and param.device_id == 1: control_pos = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID: control_path = pv[0] control_error_type = pv[1] if len(pv) > 1 else 0 elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE: control_refp = plog.extract_waypoint(pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS: control_cycles = pv[0] control_obj_val = pv[1] control_errors = pv[2] control_resets = pv[3] elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS: last_gps = pv[0] last_data = pv[1] elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION: control_nav_version = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_GP_OUT: payload_release = True if pv[0] else False control_data = (control_cycles, control_obj_val, control_errors, control_resets) if not state_pos or not control_refp or LAST_CONTROL_DATA == control_data: return "" #if control_mode[1] != 1: # return "" LAST_CONTROL_DATA = control_data state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 + (state_velocity[1] - state_wind[1])**2 + (state_velocity[2] - state_wind[2])**2) state_groundspeed = math.sqrt(state_velocity[0]**2 + state_velocity[1]**2) ned_off = plog.lla_to_ned( state_pos, (control_refp["lat"], control_refp["lon"], control_refp["alt"])) ref_v = (control_refp["airspeed"] * math.cos(control_refp["yaw"]) + state_wind[0], control_refp["airspeed"] * math.sin(control_refp["yaw"]) + state_wind[1]) ate = abs((ned_off[0] * ref_v[0] + ned_off[1] * ref_v[1]) / \ math.sqrt(ref_v[0] ** 2 + ref_v[1] ** 2)) xte = math.sqrt((ned_off[0]**2 + ned_off[1]**2) - ate**2) data = ((t, ) + (state_pos[2], control_refp["alt"]) + ned_off + (xte, ate) + (state_airspeed, control_refp["airspeed"], state_groundspeed) + (math.degrees(math.atan2(state_velocity[1], state_velocity[0])), math.degrees(math.atan2(ref_v[1], ref_v[0]))) + (state_attitude[0], math.degrees(control_refp["yaw"])) + (state_attitude[1], math.degrees(control_refp["pitch"])) + (state_attitude[2], math.degrees(control_refp["roll"])) + (state_angular_velocity[2], state_angular_velocity[1], state_angular_velocity[0]) + (math.sqrt(state_wind[0]**2 + state_wind[1]**2), state_wind[0], state_wind[1]) + (control_values[0], control_values[1], control_values[2]) + (control_pos[0], control_pos[1], control_pos[2]) + (control_obj_val, control_cycles, control_errors, control_resets) + (control_path, last_gps, control_mode[1], control_refp["flags"], control_error_type, 1 if payload_release else 0)) format = ( "t=%8d, " + "alt=%3.3f, alt_ref=%3.3f, " + "n=%6.2f, e=%6.2f, d=%6.2f, xte=%4.2f, ate=%4.2f, " + "tas=%5.2f, tas_ref=%5.2f, gs=%5.2f, " + "heading=%3.0f, heading_ref=%3.0f, " + "yaw=%3.0f, yaw_ref=%3.0f, " + "pitch=%4.0f, pitch_ref=%4.0f, " + "roll=%4.0f, roll_ref=%4.0f, " + "vyaw=%4.0f, vpitch=%4.0f, vroll=%4.0f, " + "wind=%5.2f, wind_n=%5.2f, wind_e=%5.2f, " + "t=%.3f, l=%.3f, r=%.3f, " + "tp=%.3f, lp=%.3f, rp=%.3f, " + "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " + "path=%4d, last_gps=%4d, mode1=%d, flags=%08x, err=%04x, release=%d\n") dump = ("%d," + "%.3f,%.3f," + "%.2f,%.2f,%.2f,%.2f,%.2f," + "%.2f,%.2f,%.2f," + "%.1f,%.1f," + "%.1f,%.1f," + "%.1f,%.1f," + "%.1f,%.1f," + "%.1f,%.1f,%.1f," + "%.2f,%.2f,%.2f," + "%.3f,%.3f,%.3f," + "%.3f,%.3f,%.3f," + "%.1f,%d,%d,%d," + "%d,%d,%d,%d,%d,%d\n") #return format % data return dump % data
def tick(lat=None, lon=None, alt=None, velocity=None, attitude=None, angular_velocity=None, wind_velocity=None, measurement_input=None): """ Runs the FCS control and comms tasks with the state data provided as though it came from the AHRS, and returns the control output. """ if not fcs._fcs: raise RuntimeError("Please call fcs.init()") estimate_log = plog.ParameterLog( log_type=plog.LogType.FCS_LOG_TYPE_ESTIMATE) estimate_log.append( plog.DataParameter( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=32, values=[ int(lat * (2**31 - 1) / math.pi), int(lon * (2**31 - 1) / math.pi), int(alt * 1e2) ] ) ) estimate_log.append( plog.DataParameter( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map(lambda x: int(x * 1e2), velocity) ) ) estimate_log.append( plog.DataParameter( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map(lambda x: int(x * (2**15 - 1)), attitude) ) ) estimate_log.append( plog.DataParameter( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map(lambda x: int(x * (2**15 - 1) / math.pi * 0.25), angular_velocity) ) ) estimate_log.append( plog.DataParameter( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map(lambda x: int(x * 1e2), wind_velocity) ) ) estimate_log.append( plog.DataParameter( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_AHRS_STATUS, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=[0, 0] ) ) fcs.write(3, estimate_log.serialize()) fcs.write(1, measurement_input) #print binascii.b2a_hex(estimate_log.serialize()) fcs._fcs.fcs_board_tick() fcs._fcs.fcs_ahrs_tick() fcs._fcs.fcs_control_tick() # Read out ignored streams fcs.read(0, 1023) fcs.read(1, 1023) fcs.read(2, 1023) sys.stderr.write(fcs.read(4, 1023)) try: control_log = plog.ParameterLog.deserialize(fcs.read(3, 1023)) # print control_log control_param = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT) path = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID).values[0] refp = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_KEY_VALUE) cycles, obj_val, errors, resets = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS).values last_control = ( map(lambda x: float(x) / float(2**16), control_param.values), plog.extract_waypoint(refp.value), obj_val, cycles, errors, resets, control_log.tick, path ) return last_control except Exception: return ([0.0, 0.5, 0.5], {}, 0, 0, 0, 0, 0, 0xFFFF)
def tick(lat=None, lon=None, alt=None, velocity=None, attitude=None, angular_velocity=None, wind_velocity=None, measurement_input=None): """ Runs the FCS control and comms tasks with the state data provided as though it came from the AHRS, and returns the control output. """ if not fcs._fcs: raise RuntimeError("Please call fcs.init()") estimate_log = plog.ParameterLog( log_type=plog.LogType.FCS_LOG_TYPE_ESTIMATE) estimate_log.append( plog.DataParameter(device_id=0, parameter_type=plog.ParameterType. FCS_PARAMETER_ESTIMATED_POSITION_LLA, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=32, values=[ int(lat * (2**31 - 1) / math.pi), int(lon * (2**31 - 1) / math.pi), int(alt * 1e2) ])) estimate_log.append( plog.DataParameter(device_id=0, parameter_type=plog.ParameterType. FCS_PARAMETER_ESTIMATED_VELOCITY_NED, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map(lambda x: int(x * 1e2), velocity))) estimate_log.append( plog.DataParameter(device_id=0, parameter_type=plog.ParameterType. FCS_PARAMETER_ESTIMATED_ATTITUDE_Q, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map(lambda x: int(x * (2**15 - 1)), attitude))) estimate_log.append( plog.DataParameter(device_id=0, parameter_type=plog.ParameterType. FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map( lambda x: int(x * (2**15 - 1) / math.pi * 0.25), angular_velocity))) estimate_log.append( plog.DataParameter(device_id=0, parameter_type=plog.ParameterType. FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=map(lambda x: int(x * 1e2), wind_velocity))) estimate_log.append( plog.DataParameter( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_AHRS_STATUS, value_type=plog.ValueType.FCS_VALUE_SIGNED, value_precision=16, values=[0, 0])) fcs.write(3, estimate_log.serialize()) fcs.write(1, measurement_input) #print binascii.b2a_hex(estimate_log.serialize()) fcs._fcs.fcs_board_tick() fcs._fcs.fcs_ahrs_tick() fcs._fcs.fcs_control_tick() # Read out ignored streams fcs.read(0, 1023) fcs.read(1, 1023) fcs.read(2, 1023) sys.stderr.write(fcs.read(4, 1023)) try: control_log = plog.ParameterLog.deserialize(fcs.read(3, 1023)) # print control_log control_param = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT) path = control_log.find_by(device_id=0, parameter_type=plog.ParameterType. FCS_PARAMETER_NAV_PATH_ID).values[0] refp = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_KEY_VALUE) cycles, obj_val, errors, resets = control_log.find_by( device_id=0, parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS ).values last_control = (map(lambda x: float(x) / float(2**16), control_param.values), plog.extract_waypoint(refp.value), obj_val, cycles, errors, resets, control_log.tick, path) return last_control except Exception: return ([0.0, 0.5, 0.5], {}, 0, 0, 0, 0, 0, 0xFFFF)
# KML is lon, lat math.degrees(pv[1] * math.pi / 2**31), math.degrees(pv[0] * math.pi / 2**31), pv[2] * 1e-2 ) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED: velocity = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q: att_q = map(lambda x: float(x) / 32767.0, pv) attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format(*plog.q_to_euler(att_q)) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED: wind = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS: control_pos = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE: ref_waypoint = plog.extract_waypoint(pv) ref_point = '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format( math.degrees(ref_waypoint['lon']), math.degrees(ref_waypoint['lat']), ref_waypoint['alt'] ) ref_attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format( math.degrees(ref_waypoint['yaw']), math.degrees(ref_waypoint['pitch']), math.degrees(ref_waypoint['roll']) ) ref_airspeed = '<gx:value>{0:.2f}</gx:value>'.format(ref_waypoint['airspeed']) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE: if param.device_id == 1: auto = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID:
# KML is lon, lat math.degrees(pv[1] * math.pi / 2**31), math.degrees(pv[0] * math.pi / 2**31), pv[2] * 1e-2) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED: velocity = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q: att_q = map(lambda x: float(x) / 32767.0, pv) attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format( *plog.q_to_euler(att_q)) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED: wind = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS: control_pos = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE: ref_waypoint = plog.extract_waypoint(pv) ref_point = '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format( math.degrees(ref_waypoint['lon']), math.degrees(ref_waypoint['lat']), ref_waypoint['alt']) ref_attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format( math.degrees(ref_waypoint['yaw']), math.degrees(ref_waypoint['pitch']), math.degrees(ref_waypoint['roll'])) ref_airspeed = '<gx:value>{0:.2f}</gx:value>'.format( ref_waypoint['airspeed']) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE: if param.device_id == 1: auto = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID: path = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS:
def tick(t, data): global LAST_CONTROL_DATA state_pos = None state_velocity = (-1, -1, -1) state_wind = (0, 0, 0) state_attitude = (0, 0, 0) state_angular_velocity = (0, 0, 0) control_refp = None control_path = -1 control_cycles = -1 control_obj_val = -1 control_errors = -1 control_resets = -1 control_values = (-1, -1, -1) control_mode = [-1, -1] control_nav_version = -1 control_pos = (-1, -1, -1) control_error_type = 0 last_gps = 0 last_data = 0 for param in data: pt = param.parameter_type if isinstance(param, plog.DataParameter): pv = param.values else: pv = param.value if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA: state_pos = (pv[0] * math.pi / 2**31, pv[1] * math.pi / 2**31, pv[2] * 1e-2) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED: state_velocity = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q: att_q = map(lambda x: float(x) / 32767.0, pv) state_attitude = plog.q_to_euler(att_q) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED: state_wind = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE: mode = chr(pv[0]) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE: control_mode[param.device_id] = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT: control_values = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID: control_path = pv[0] control_error_type = pv[1] elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE: control_refp = plog.extract_waypoint(pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS: control_cycles = pv[0] control_obj_val = pv[1] control_errors = pv[2] control_resets = pv[3] elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS: last_gps = pv[0] last_data = pv[1] elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION: control_nav_version = pv[0] control_data = (control_cycles, control_obj_val, control_errors, control_resets) if not state_pos or not control_refp: return "" LAST_CONTROL_DATA = control_data state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 + (state_velocity[1] - state_wind[1])**2 + (state_velocity[2] - state_wind[2])**2) return ( "t=%8d, " + "alt=%3.3f, alt_ref=%3.3f, " + "n=%6.2f, e=%6.2f, d=%6.2f, " + "tas=%5.2f, tas_ref=%5.2f, " + "yaw=%3.0f, yaw_ref=%3.0f, " + "pitch=%4.0f, pitch_ref=%4.0f, " + "roll=%4.0f, roll_ref=%4.0f, " + "t=%.3f, l=%.3f, r=%.3f, " + "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " + "path=%4d, last_gps=%4d, mode1=%d, nav_state=%d") % ( (t, ) + (state_pos[2], control_refp.get("alt", 0)) + plog.lla_to_ned( (control_refp.get("lat", 0), control_refp.get( "lon", 0), control_refp.get("alt", 0)), state_pos) + (state_airspeed, control_refp.get("airspeed", 0)) + (state_attitude[0], math.degrees(control_refp.get("yaw", 0))) + (state_attitude[1], math.degrees(control_refp.get("pitch", 0))) + (state_attitude[2], math.degrees(control_refp.get("roll", 0))) + (control_values[0], control_values[1], control_values[2]) + (control_obj_val, control_cycles, control_errors, control_resets) + (control_path, last_gps, control_mode[1], control_nav_version))
def tick(t, data): global LAST_CONTROL_DATA state_pos = None state_velocity = (-1, -1, -1) state_wind = (0, 0, 0) state_attitude = (0, 0, 0) state_angular_velocity = (0, 0, 0) control_refp = None control_path = -1 control_cycles = -1 control_obj_val = -1 control_errors = -1 control_resets = -1 control_values = (-1, -1, -1) control_mode = [-1, -1] control_nav_version = -1 control_pos = (-1, -1, -1) control_error_type = 0 last_gps = 0 last_data = 0 for param in data: pt = param.parameter_type if isinstance(param, plog.DataParameter): pv = param.values else: pv = param.value if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA: state_pos = ( pv[0] * math.pi / 2**31, pv[1] * math.pi / 2**31, pv[2] * 1e-2 ) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED: state_velocity = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q: att_q = map(lambda x: float(x) / 32767.0, pv) state_attitude = plog.q_to_euler(att_q) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED: state_wind = map(lambda x: float(x) * 1e-2, pv) elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE: mode = chr(pv[0]) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE: control_mode[param.device_id] = pv[0] elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT: control_values = map(lambda x: float(x) / 65535.0, pv) elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID: control_path = pv[0] control_error_type = pv[1] elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE: control_refp = plog.extract_waypoint(pv) elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS: control_cycles = pv[0] control_obj_val = pv[1] control_errors = pv[2] control_resets = pv[3] elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS: last_gps = pv[0] last_data = pv[1] elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION: control_nav_version = pv[0] control_data = (control_cycles, control_obj_val, control_errors, control_resets) if not state_pos or not control_refp: return "" LAST_CONTROL_DATA = control_data state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 + (state_velocity[1] - state_wind[1])**2 + (state_velocity[2] - state_wind[2])**2) return ( "t=%8d, " + "alt=%3.3f, alt_ref=%3.3f, " + "n=%6.2f, e=%6.2f, d=%6.2f, " + "tas=%5.2f, tas_ref=%5.2f, " + "yaw=%3.0f, yaw_ref=%3.0f, " + "pitch=%4.0f, pitch_ref=%4.0f, " + "roll=%4.0f, roll_ref=%4.0f, " + "t=%.3f, l=%.3f, r=%.3f, " + "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " + "path=%4d, last_gps=%4d, mode1=%d, nav_state=%d" ) % ( (t, ) + (state_pos[2], control_refp.get("alt", 0)) + plog.lla_to_ned( (control_refp.get("lat", 0), control_refp.get("lon", 0), control_refp.get("alt", 0)), state_pos) + (state_airspeed, control_refp.get("airspeed", 0)) + (state_attitude[0], math.degrees(control_refp.get("yaw", 0))) + (state_attitude[1], math.degrees(control_refp.get("pitch", 0))) + (state_attitude[2], math.degrees(control_refp.get("roll", 0))) + (control_values[0], control_values[1], control_values[2]) + (control_obj_val, control_cycles, control_errors, control_resets) + (control_path, last_gps, control_mode[1], control_nav_version) )