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
pv = param.values else: pv = param.value if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA: point = '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format( # 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'])
pv = param.values else: pv = param.value if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA: point = '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format( # 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'])
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))
estimate_t += 0.001 for param in data: pt = param.parameter_type pv = param.values if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA: pos = [ math.degrees(pv[0] * math.pi / 2**31), math.degrees(pv[1] * math.pi / 2**31), pv[2] * 1e-2 ] elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED: v = 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) att_ypr = list(plog.q_to_euler(att_q)) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ: angular_v = map( lambda x: math.degrees(float(x) / (32767.0 / math.pi * 0.25)), pv) elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED: wind_v = 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_ESTIMATED_GYRO_BIAS_XYZ: gyro_bias = map( lambda x: math.degrees(float(x) / (32767.0 / math.pi * 2.0)), pv) vv = math.sqrt(v[0] ** 2 + v[1] ** 2 + v[2] ** 2)
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) )