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