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)
)
