def get_ulog(filepath, topics=None):
"""Read a ulg file from the given filepath and return it as a ulog structure.
It can be that sometimes, topics are missing.
Thus, check if the required topic are available in the ulog file.
Arguments:
filepath -- absoulte path to the .ulg file
topics -- list of required topics
"""
if topics:
ulog = pyulog.ULog(filepath, topics)
tmp = topics.copy()
for topic in ulog.data_list:
if topic.name in tmp:
idx = tmp.index(topic.name)
tmp.pop(idx)
if len(tmp) > 0:
warnings.warn(
"The following topics do not exist: \n {0}".format(tmp))
else:
ulog = pyulog.ULog(filepath)
if not ulog.data_list:
warnings.warn("No topics present.")
return ulog
def load_log(self):
print("\nLoad logs")
self.log = pyulog.ULog(self.log_file)
self.log_start = self.log.start_timestamp
self.log_keys = [x.name for x in self.log.data_list]
print("start timestam: ", self.log_start)
def loadDataset(filename):
"""Load datasets into a nested dictionary format"""
# use these keys to differentiate duplicate datasets
dup_dataset_key = {'actuator_outputs' : 'output[1]',
'battery_status' : 'current_a',
'input_rc' : 'values[0]'
}
ulog = pyulog.ULog(filename)
# combine datasets into dictionary
dataset_dict = {}
for d in ulog.data_list:
if d.name in dataset_dict.keys():
if d.name in dup_dataset_key:
key = dup_dataset_key[d.name]
dset_old = dataset_dict[d.name]
std_old = np.std(dset_old[key])
std_new = np.std(d.data[key])
if std_new > std_old: # compare variation of data
dataset_dict[d.name] = d.data # replace old dataset
else:
print('WARNING: ignoring duplicate dataset: {:}'.format(d.name))
continue
dataset_dict[d.name] = d.data
return dataset_dict
def read_ulog(ulog_filename, messages=None, verbose=False):
"""
Convert ulog to pandas dataframe.
"""
log = pyulog.ULog(ulog_filename, messages)
# column naming
d_col_rename = {
'[': '_',
']': '_',
}
col_rename_pattern = re.compile(
r'(' + '|'.join([re.escape(key)
for key in d_col_rename.keys()]) + r')')
data = {}
for msg in log.data_list:
msg_data = pd.DataFrame.from_dict(msg.data)
msg_data.columns = [
col_rename_pattern.sub(lambda x: d_col_rename[x.group()], col)
for col in msg_data.columns
]
msg_data.index = pd.TimedeltaIndex(msg_data['timestamp'] * 1e3,
unit='ns')
data['t_{:s}_{:d}'.format(msg.name, msg.multi_id)] = msg_data
return PX4MessageDict(data)
def get_attitude_state_setpoint_from_file(f):
"""
return attitude-euler, setpoint and error
"""
topics = ["vehicle_attitude", "vehicle_attitude_setpoint"]
ulog = pyulog.ULog(f, topics)
pandadict = conv.createPandaDict(ulog)
return conv.merge(pandadict)
def setup_class(self):
filepath = getfilepath()
topics = [
"vehicle_attitude",
"vehicle_attitude_setpoint",
"vehicle_status",
]
self.ulog = pyulog.ULog(filepath, topics)
self.df = ulogconv.merge(ulogconv.createPandaDict(self.ulog))
def load_log_file(filename):
useful_messages = [
'estimator_status', 'vehicle_attitude', 'vehicle_local_position',
'vehicle_status'
]
try:
ulog = pyulog.ULog(filename, message_name_filter_list=useful_messages)
except FileNotFoundError:
print(f"File {filename} does not exist.")
except:
print("Error while reading or parsing the ulog file.")
else:
return ulog
def test_replace_nan_with_inf():
"""test replace nan with inf."""
file = "testlogs/position.ulg"
topics = ["vehicle_local_position", "vehicle_attitude"]
nan_msg = [np.nan, 2, 4, np.nan, np.nan, 5]
inf_msg = [np.inf, 2, 4, np.inf, np.inf, 5]
ulog = pyulog.ULog(file, topics)
ulog.data_list[0].data["fake_msg_0"] = np.array(nan_msg)
ulog.data_list[1].data["fake_msg_0"] = np.array(nan_msg)
topic_msgs_list = [
TopicMsgs("vehicle_local_position", ["fake_msg_0"]),
TopicMsgs("vehicle_attitude", ["fake_msg_0"]),
]
ulogconv.replace_nan_with_inf(ulog, topic_msgs_list)
assert_almost_equal(ulog.data_list[0].data["fake_msg_0"], inf_msg)
assert_almost_equal(ulog.data_list[1].data["fake_msg_0"], inf_msg)
def prepare_data(filename: str):
"""Combine gyro and actuator data, and resample"""
log = ulog_to_dict(pyulog.ULog(filename))
combined = pd.merge_asof(log['t_actuator_controls_0_0'],
log['t_sensor_combined_0'],
on='f_timestamp')
combined.index = pd.TimedeltaIndex(data=combined['f_timestamp'] -
combined['f_timestamp'][0],
unit='us')
sample_period_micros = int(
np.ceil(1e6 / np.floor(ut.ulog.sample_frequency(combined))))
combined = combined.resample(
'{:d} us'.format(sample_period_micros)).ffill().bfill()
# pylint: disable=redefined-variable-type
combined.index = pd.Float64Index(
data=np.array(combined.index.values, dtype=np.float) / 1.0e9,
name='time, s')
return combined
def create_plots(log_file, message_name, parameters, file_name=None):
"""
Args:
log_file:
message_name:
parameters:
file_name:
Returns:
"""
if isinstance(log_file, pyulog.ULog):
ulog = log_file
elif isinstance(log_file, str):
if os.path.isfile(log_file):
ulog = pyulog.ULog(log_file)
else:
raise FileNotFoundError(log_file)
msg = ulog.get_dataset(message_name)
time_data = msg.data['timestamp']
fig, axs = plt.subplots(len(parameters), 1, sharex='all')
for i in range(len(parameters)):
axs[i].plot(time_data, msg.data[parameters[i]], drawstyle='steps-post')
axs[i].set_ylabel(parameters[i])
axs[i].grid(True)
fig.tight_layout()
if file_name is None:
file_name = message_name
fig.savefig(f"{file_name}.pdf",
dpi=None,
facecolor='w',
edgecolor='w',
orientation='portrait',
papertype='a4',
format='pdf',
transparent=False,
bbox_inches=None,
pad_inches=0.1,
frameon=None,
metadata=None)
def test_createPandaDict():
"""test create dictionary of panda-topics."""
file = "testlogs/position.ulg"
topics = ["vehicle_local_position", "vehicle_attitude"]
ulog = pyulog.ULog(file, topics)
dp = ulogconv.create_pandadict(ulog)
expected_names = {
"T_vehicle_local_position_0": "T_vehicle_local_position_0",
"T_vehicle_attitude_0": "T_vehicle_attitude_0",
}
for key in dp:
assert key == expected_names[key]
for name in dp[key].columns:
if name != "timestamp":
assert name[:2] == "F_"
def print_changed_parameters(log_file):
"""
Args:
log_file:
Returns:
"""
if isinstance(log_file, pyulog.ULog):
ulog = log_file
elif isinstance(log_file, str):
if os.path.isfile(log_file):
ulog = pyulog.ULog(log_file)
else:
raise FileNotFoundError(log_file)
for cp in ulog.changed_parameters:
print(cp)
def print_dropouts(log_file):
"""
Args:
log_file:
Returns:
"""
if isinstance(log_file, pyulog.ULog):
ulog = log_file
elif isinstance(log_file, str):
if os.path.isfile(log_file):
ulog = pyulog.ULog(log_file)
else:
raise FileNotFoundError(log_file)
for d in ulog.dropouts:
print(d)
def print_initial_parameters(log_file):
"""
Args:
log_file:
Returns:
"""
if isinstance(log_file, pyulog.ULog):
ulog = log_file
elif isinstance(log_file, str):
if os.path.isfile(log_file):
ulog = pyulog.ULog(log_file)
else:
raise FileNotFoundError(log_file)
for k, v in ulog.initial_parameters.items():
print(k, v)
def print_logged_messages(log_file):
"""
Args:
log_file:
Returns:
"""
if isinstance(log_file, pyulog.ULog):
ulog = log_file
elif isinstance(log_file, str):
if os.path.isfile(log_file):
ulog = pyulog.ULog(log_file)
else:
raise FileNotFoundError(log_file)
for lm in ulog.logged_messages:
print(lm)
def print_available_parameters(log_file):
"""
Args:
log_file:
Returns:
"""
if isinstance(log_file, pyulog.ULog):
ulog = log_file
elif isinstance(log_file, str):
if os.path.isfile(log_file):
ulog = pyulog.ULog(log_file)
else:
raise FileNotFoundError(log_file)
for msg in ulog.data_list:
for k, v in msg.data.items():
print('{}.{}'.format(msg.name, k))
def print_message_formats(log_file):
"""
Args:
log_file:
Returns:
"""
if isinstance(log_file, pyulog.ULog):
ulog = log_file
elif isinstance(log_file, str):
if os.path.isfile(log_file):
ulog = pyulog.ULog(log_file)
else:
raise FileNotFoundError(log_file)
for k, v in ulog.message_formats.items():
print("{} ({}):".format(v.name, dir(v)))
for f in v.fields:
print("\t\t{}".format(f))
def get_ulog(filepath, topics):
ulog = pyulog.ULog(filepath, topics)
if not ulog.data_list:
print("\033[93m" + "Not a single desired topic present" + "\033[0m")
return None
tmp = topics.copy()
for topic in ulog.data_list:
if topic.name in tmp:
idx = tmp.index(topic.name)
tmp.pop(idx)
if len(tmp) > 0:
print("\033[93m" +
"The following topics do not exist in the provided ulog file: " +
"\033[0m")
print(tmp)
return None
return ulog
def setup_dataframe(self, filepath, topics):
# Check if any of the topics exist in the topics exists in the log file
try:
self.ulog = pyulog.ULog(filepath, topics)
except:
print(
"Not a single topic that is needed for this test exists in the provided ulog file. Abort test"
)
assert False
# Check for every topic separately if it exists in the log file
for i in range(len(self.ulog.data_list)):
if self.ulog.data_list[i].name in topics:
idx = topics.index(self.ulog.data_list[i].name)
topics.pop(idx)
if len(topics) > 0:
print("\033[93m" +
"The following topics do not exist in the provided ulog file: " +
"\033[0m")
print(topics)
pytest.skip("Skip this test because topics are missing")
else:
self.df = ulogconv.merge(ulogconv.createPandaDict(self.ulog))
def logextract(path,topic_list=None):
""" Returns an info dictionnary containing the relevant information in the log "path" according to "topic_list" """
if topic_list == None :
topic_list = []
topic_list.append('sensor_combined')
topic_list.append('actuator_outputs')
topic_list.append('vehicle_local_position_setpoint')
topic_list.append('vehicle_status')
topic_list.append('vehicle_land_detected')
topic_list.append('manual_control_setpoint')
ulog = pyulog.ULog(path,topic_list)
datalist = ulog.data_list # is a list of Data objects, which contain the final topic data for a single topic and instance
info = {}
for topic in datalist:
if topic.name == 'sensor_combined':
data_sc = topic.data
time_sc = data_sc['timestamp']/1e6 # convert it from us to s
acc_x=data_sc['accelerometer_m_s2[0]']
acc_y=data_sc['accelerometer_m_s2[1]']
acc_z=data_sc['accelerometer_m_s2[2]']
roll_rate = data_sc['gyro_rad[0]'] # angular rates in rad/s
pitch_rate = data_sc['gyro_rad[1]']
yaw_rate = data_sc['gyro_rad[2]']
info.update({'time_sc':time_sc, 'acc_x':acc_x, 'acc_y':acc_y,'acc_z':acc_z, 'roll_rate': roll_rate, 'pitch_rate': pitch_rate, 'yaw_rate': yaw_rate})
info.update (sixaxes_spectrum(info))
elif topic.name == 'actuator_outputs':
if (np.all(topic.data['noutputs'] <= 1)):
continue
else:
data_ao = topic.data
time_ao = data_ao['timestamp']/1e6
columns = ['motor 1','motor 2','motor 3','motor 4','motor 5','motor 6']
rpm=[]
for k in range(np.unique(data_ao['noutputs']).item()):
rpm.insert(k,data_ao[f'output[{k}]'])
rpm_df = pd.DataFrame(np.array([rpm[0],rpm[1],rpm[2],rpm[3],rpm[4],rpm[5]]).transpose(),index=range(len(time_ao)),columns=columns)
info.update({'time_ao':time_ao,'rpm': rpm,'rpm_df': rpm_df})
elif topic.name == 'vehicle_local_position':
data_vlp = topic.data
time_vlp = data_vlp['timestamp']/1e6
vx = data_vlp['vx']
vy = data_vlp['vy']
vz = data_vlp['vz']
x = data_vlp['x']
y = data_vlp['y']
z = data_vlp['z']
info.update({'time_vlp':time_vlp,'x':x,'y':y,'z':z,'vx':vx,'vy':vy,'vz':vz})
elif topic.name == 'vehicle_gps_position':
data_vgp = topic.data
time_vgp = data_vgp['timetsamp']/1e6
heading = data_vgp['heading']
info.update({'heading':heading})
elif topic.name == 'vehicle_local_position_setpoint':
data_vlps = topic.data
time_vlps = data_vlps['timestamp']/1e6
x_thrust = data_vlps['thrust[0]']
vert_thrust = data_vlps['thrust[2]']
info.update({'time_vlps': time_vlps,'x_thrust': x_thrust, 'vert_thrust': vert_thrust})
elif topic.name == 'vehicle_attitude_setpoint':
data_vas = topic.data
time_vas = data_vas['timestamp']/1e6
pitch_sp = data_vas['pitch_body']
info.update({'time_vas': time_vas,'pitch_sp': pitch_sp})
elif topic.name == 'vehicle_status':
data_vs = topic.data
time_vs = data_vs['timestamp']/1e6
navstate = data_vs['nav_state']
info.update({'time_vs': time_vs,'navstate': navstate})
elif topic.name == 'vehicle_land_detected':
data_vld = topic.data
elif topic.name == 'distance_sensor':
data_ds = topic.data
time_ds = data_ds['timestamp']/1e6
dist = data_ds['current_distance'] # current distance reading (m)
info.update({'time_ds':time_ds,'dist':dist})
elif topic.name == 'manual_control_setpoint':
data_mcs = topic.data
time_mcs = data_mcs['timestamp']/1e6
stick_in_x = data_mcs['x']
stick_in_y = data_mcs['y']
stick_in_z = data_mcs['z']
info.update({'time_mcs':time_mcs,'stick_in_x': stick_in_x, 'stick_in_y': stick_in_y, 'stick_in_z': stick_in_z})
elif topic.name == 'battery_status':
data_bs = topic.data
time_bs = data_bs['timestamp']/1e6
battery_current = data_bs['current_a']
battery_voltage = data_bs['voltage_v']
battery_filtered_current = data_bs['current_filtered_a']
battery_filtered_voltage = data_bs['voltage_filtered_v']
discharged_mah = data_bs['discharged_mah']
remaining = data_bs['remaining']
n_cells = data_bs['cell_count']
info.update({'time_bs':time_bs,'n_cells':n_cells,'battery_current':battery_current,'battery_filtered_current':battery_filtered_current,'battery_voltage':battery_voltage,'battery_filtered_voltage':battery_filtered_voltage,'discharged_mah':discharged_mah,'remaining':remaining})
elif topic.name == 'battery_status_ekf':
data_bkf = topic.data
time_bkf = data_bkf['timestamp']/1e6
remaining_ekf = data_bkf['remaining']
iR1 = data_bkf['resistor_current']
covx = np.array([[data_bkf['covx[0]'],data_bkf['covx[1]']],[data_bkf['covx[2]'],data_bkf['covx[3]']]])
kalman_gain = np.array([[data_bkf['kalman_gain[0]']],[data_bkf['kalman_gain[1]']]])
innovation = data_bkf['innovation']
info.update({'remaining_ekf':remaining_ekf,'covx':covx,'kalman_gain':kalman_gain,'innovation':innovation,'iR1':iR1})
return info
def open_file(arq):
class dados:
class param:
pass
# Carrega Mensagens
msg = [
'vehicle_attitude', 'vehicle_attitude_setpoint', 'actuator_controls_0',
'vehicle_rates_setpoint'
]
ulog = pyulog.ULog(arq, msg)
dados.param.r_r_p = ulog.initial_parameters['MC_ROLLRATE_P']
dados.param.r_r_i = ulog.initial_parameters['MC_ROLLRATE_I']
dados.param.r_r_d = ulog.initial_parameters['MC_ROLLRATE_D']
dados.param.r_p_p = ulog.initial_parameters['MC_PITCHRATE_P']
dados.param.r_p_i = ulog.initial_parameters['MC_PITCHRATE_I']
dados.param.r_p_d = ulog.initial_parameters['MC_PITCHRATE_D']
dados.param.r_y_p = ulog.initial_parameters['MC_YAWRATE_P']
dados.param.r_y_i = ulog.initial_parameters['MC_YAWRATE_I']
dados.param.r_y_d = ulog.initial_parameters['MC_YAWRATE_D']
dados.param.a_r_p = ulog.initial_parameters['MC_ROLL_P']
dados.param.a_p_p = ulog.initial_parameters['MC_PITCH_P']
dados.param.a_y_p = ulog.initial_parameters['MC_YAW_P']
for d in ulog.data_list:
if d.name == 'vehicle_rates_setpoint':
t0 = d.data['timestamp']
r_r_r = d.data['roll']
r_r_p = d.data['pitch']
r_r_y = d.data['yaw']
if d.name == 'vehicle_attitude':
t1 = d.data['timestamp']
y_r_r = d.data['rollspeed']
y_r_p = d.data['pitchspeed']
y_r_y = d.data['yawspeed']
q0 = d.data['q[0]']
q1 = d.data['q[1]']
q2 = d.data['q[2]']
q3 = d.data['q[3]']
if d.name == 'actuator_controls_0':
t2 = d.data['timestamp']
u_r = d.data['control[0]']
u_p = d.data['control[1]']
u_y = d.data['control[2]']
u_z = d.data['control[3]']
if d.name == 'vehicle_attitude_setpoint':
t3 = d.data['timestamp']
q0d = d.data['q_d[0]']
q1d = d.data['q_d[1]']
q2d = d.data['q_d[2]']
q3d = d.data['q_d[3]']
N = np.size(q0)
y_a_p = np.zeros(N)
y_a_r = np.zeros(N)
y_a_y = np.zeros(N)
for i in range(0, N):
y_a_p[i] = -math.asin(2 * q1[i] * q3[i] - 2 * q2[i] * q0[i])
y_a_r[i] = math.asin(2 * q0[i] * q1[i] + 2 * q2[i] * q3[i])
a0 = q0[i]**2 + q1[i]**2 - q2[i]**2 - q3[i]**2
a1 = 2 * q0[i] * q3[i] + 2 * q1[i] * q2[i]
y_a_y[i] = math.atan2(a1, a0)
while (y_a_y[i] > (y_a_y[i - 1] + math.pi)):
y_a_y[i] = y_a_y[i] - 2 * math.pi
while (y_a_y[i] < (y_a_y[i - 1] - math.pi)):
y_a_y[i] = y_a_y[i] + 2 * math.pi
#else:
#print("igual")
N = np.size(q0d)
r_a_p = np.zeros(N)
r_a_r = np.zeros(N)
r_a_y = np.zeros(N)
for i in range(0, N):
r_a_p[i] = -math.asin(2 * q1d[i] * q3d[i] - 2 * q2d[i] * q0d[i])
r_a_r[i] = math.asin(2 * q0d[i] * q1d[i] + 2 * q2d[i] * q3d[i])
a0 = q0d[i]**2 + q1d[i]**2 - q2d[i]**2 - q3d[i]**2
a1 = 2 * q0d[i] * q3d[i] + 2 * q1d[i] * q2d[i]
r_a_y[i] = math.atan2(a1, a0)
while (r_a_y[i] > (r_a_y[i - 1] + math.pi)):
r_a_y[i] = r_a_y[i] - 2 * math.pi
while (r_a_y[i] < (r_a_y[i - 1] - math.pi)):
r_a_y[i] = r_a_y[i] + 2 * math.pi
umax = np.max(u_z)
utemp = 0
ini = 0
while utemp < umax / 2:
utemp = u_z[ini]
ini = ini + 1
tini = t2[ini]
utemp = 0
ini = -1
while utemp < umax / 2:
utemp = u_z[ini]
ini = ini - 1
tfim = t2[ini]
#t=np.arange(t0[0],t0[-1],4000)
t = np.arange(tini, tfim, 4000)
# Interpola os dados para todos terem a mesma base de tempo
dados.r_r_r = np.interp(t, t0, r_r_r)
dados.r_r_p = np.interp(t, t0, r_r_p)
dados.r_r_y = np.interp(t, t0, r_r_y)
dados.y_r_r = np.interp(t, t1, y_r_r)
dados.y_r_p = np.interp(t, t1, y_r_p)
dados.y_r_y = np.interp(t, t1, y_r_y)
dados.u_r = np.interp(t, t2, u_r)
dados.u_p = np.interp(t, t2, u_p)
dados.u_y = np.interp(t, t2, u_y)
dados.u_z = np.interp(t, t2, u_z)
dados.y_a_r = np.interp(t, t1, y_a_r)
dados.y_a_p = np.interp(t, t1, y_a_p)
dados.y_a_y = np.interp(t, t1, y_a_y)
dados.r_a_r = np.interp(t, t3, r_a_r)
dados.r_a_p = np.interp(t, t3, r_a_p)
dados.r_a_y = np.interp(t, t3, r_a_y)
dados.t0 = t
dados.t1 = t1
N = np.size(t0)
P = (N - 1) / (t0[-1] - t0[0]) * 1000000
print("\nSampling rate:", P, "Hz")
"""
if P<200:
print("Low sampling rate:")
print("The following topics should be sampled at least at 200Hz")
print("'vehicle_attitude','vehicle_attitude_setpoint','actuator_controls_0','vehicle_rates_setpoint'")
return -1
"""
return dados
def __init__(self, core: uros.Core, ulog_file: str):
with open(ulog_file, 'rb') as f:
ulog = pyulog.ULog(f)
# annotate types for python editors
topic_list = ulog.data_list # type: List[pyulog.ULog.Data]
event_list = []
# sort every publication up front by timestamp, might be a faster way to do this, but
# very quick during actual sim
for topic in topic_list:
for i, timestamp in enumerate(topic.data['timestamp']):
event_list.append(
LogEvent(timestamp=timestamp, index=i, topic=topic))
event_list.sort(key=lambda x: x.timestamp)
self.ignored_events = [
'vehicle_air_data', 'vehicle_rates_setpoint',
'vehicle_attitude_setpoint', 'rate_ctrl_status',
'rate_ctrl_status', 'actuator_controls_0',
'vehicle_local_position', 'vehicle_local_position_groundtruth',
'vehicle_global_position', 'vehicle_global_position_groundtruth',
'vehicle_actuator_outputs', 'vehicle_gps_position',
'vehicle_local_position_setpoint', 'actuator_outputs',
'battery_status', 'manual_control_setpoint',
'vehicle_land_detected', 'telemetry_status',
'vehicle_status_flags', 'vehicle_status', 'sensor_preflight',
'vehicle_command', 'commander_state', 'actuator_armed',
'sensor_selection', 'input_rc', 'ekf2_innovations', 'system_power',
'radio_status', 'cpuload', 'ekf_gps_drift', 'home_position',
'mission_result', 'position_setpoint_triplet', 'ekf2_timestamps'
]
self.pubs = {}
for topic in topic_list:
if topic.name in self.ignored_events:
pass
elif topic.name == "sensor_combined":
self.pubs[topic.name] = Publisher(core, 'imu', msgs.Imu)
elif topic.name == "vehicle_magnetometer":
self.pubs[topic.name] = Publisher(core, 'mag', msgs.Mag)
elif topic.name == "estimator_status":
self.pubs[topic.name] = Publisher(core, 'log_status',
msgs.EstimatorStatus)
elif topic.name == "vehicle_attitude_groundtruth":
self.pubs[topic.name] = Publisher(core,
'ground_truth_attitude',
msgs.Attitude)
print('ground truth attitude is published')
elif topic.name == "vehicle_attitude":
self.pubs[topic.name] = Publisher(core, 'log_attitude',
msgs.Attitude)
else:
print('unhandled init for event', topic.name)
# class members
self.core = core # type: simpy.Environment
self.event_list = event_list # type: List[LogEvent]
# start process
self.core.process(self.run())
def process_file(log_path, out_path, template_path):
"""
Command line interface to temperature calibration.
"""
log = pyulog.ULog(log_path, 'sensor_gyro, sensor_accel, sensor_baro')
data = {}
for d in log.data_list:
data['{:s}_{:d}'.format(d.name, d.multi_id)] = d.data
params = {}
# open file to save plots to PDF
# from matplotlib.backends.backend_pdf import PdfPages
# output_plot_filename = ulog_file_name + ".pdf"
# pp = PdfPages(output_plot_filename)
configs = [
{
'msg': 'sensor_gyro',
'fields': ['x', 'y', 'z'],
'units': 'rad/s',
'label': 'TC_G'
},
{
'msg': 'sensor_accel',
'fields': ['x', 'y', 'z'],
'units': 'm/s^2',
'label': 'TC_A'
},
{
'msg': 'sensor_baro',
'fields': ['pressure'],
'units': 'm',
'label': 'TC_B'
},
]
for config in configs:
for d in log.data_list:
if d.name == config['msg']:
plt.figure(figsize=(20, 13))
topic = '{:s}_{:d}'.format(d.name, d.multi_id)
print('found {:s} data'.format(topic))
label = '{:s}{:d}'.format(config['label'], d.multi_id)
params[topic] = {
'params':
temp_calibration(data=d.data,
topic=topic,
fields=config['fields'],
units=config['units'],
label=label),
'label':
label
}
plt.savefig('{:s}_cal.pdf'.format(topic))
# JSON file generation
# import json
# print(json.dumps(params, indent=2))
body = ''
for sensor in sorted(params.keys()):
for param in sorted(params[sensor]['params'].keys()):
label = params[sensor]['label']
pdict = params[sensor]['params']
if pdict[param]['type'] == 'INT':
type_id = 6
elif pdict[param]['type'] == 'FLOAT':
type_id = 9
val = pdict[param]['val']
name = '{:s}_{:s}'.format(label, param)
body += "1\t1\t{name:20s}\t{val:15g}\t{type_id:5d}\n".format(
**locals())
# simple template file output
text = """# Sensor thermal compensation parameters
#
# Vehicle-Id Component-Id Name Value Type
{body:s}
""".format(body=body)
with open(out_path, 'w') as f:
f.write(text)
def main():
args = parser.parse_args()
check_directory(args.filename)
with PdfPages("px4_attitude.pdf") as pdf:
topics = ["vehicle_attitude", "vehicle_attitude_setpoint"]
ulog = pyulog.ULog(args.filename, topics)
pandadict = conv.createPandaDict(ulog)
df = conv.merge(pandadict)
df.timestamp = (df.timestamp -
df.timestamp[0]) * 1e-6 # change to seconds
# roll pitch and yaw error
add_roll_pitch_yaw(df)
add_euler_error(df)
plt.figure(0, figsize=(20, 13))
df_tmp = df[[
"timestamp",
"T_vehicle_attitude_setpoint_0__NF_e_roll",
"T_vehicle_attitude_setpoint_0__NF_e_pitch",
"T_vehicle_attitude_setpoint_0__NF_e_yaw",
]].copy()
df_tmp.plot(x="timestamp", linewidth=0.8)
pltw.plot_time_series(df_tmp, plt)
plt.title("Roll-Pitch-Yaw-Error")
plt.ylabel("rad")
pdf.savefig()
plt.close(0)
# inverted
add_vehicle_z_axis(df)
add_vehicle_inverted(df)
plt.figure(1, figsize=(20, 13))
df_tmp = df[["timestamp",
"T_vehicle_attitude_0__NF_tilt_more_90"]].copy()
df_tmp.plot(x="timestamp", linewidth=0.8)
pltw.plot_time_series(df_tmp, plt)
plt.title("Inverted")
plt.ylabel("boolean")
pdf.savefig()
plt.close(1)
# tilt and desired tilt
add_desired_z_axis(df)
add_desired_tilt(df)
add_tilt(df)
pos_tilt = loginfo.get_param(ulog, "MPC_TILTMAX_AIR", 0)
man_tilt = loginfo.get_param(ulog, "MPC_MAN_TILT_MAX", 0)
plt.figure(2, figsize=(20, 13))
df_tmp = df[[
"timestamp",
"T_vehicle_attitude_0__NF_tilt",
"T_vehicle_attitude_setpoint_0__NF_tilt_desired",
]].copy()
df_tmp["MPC_TILTMAX_AIR"] = pos_tilt * np.pi / 180
df_tmp["MPC_MAN_TILT_MAX"] = man_tilt * np.pi / 180
df_tmp.plot(x="timestamp", linewidth=0.8, style=["-", "-", "--", "--"])
pltw.plot_time_series(df_tmp, plt)
plt.title("Tilt / Desired Tilt")
plt.ylabel("rad")
pdf.savefig()
plt.close(2)
def main(ulog_file):
log_file = pyulog.ULog(ulog_file)
create_plots(log_file, 'vehicle_global_position',
['alt', 'pressure_alt', 'terrain_alt'])
create_plots(
log_file, 'vehicle_command',
['param1', 'param2', 'param3', 'param4', 'param5', 'param6', 'param7'])
create_plots(log_file,
'vehicle_attitude', ['rollspeed', 'pitchspeed', 'yawspeed'],
file_name='vehicle_attitude_rates')
create_plots(log_file, 'actuator_outputs', [
'output[0]', 'output[1]', 'output[2]', 'output[3]', 'output[4]',
'output[5]'
])
# list of parameters recorded in the log
print_available_parameters(log_file)
# vehicle_attitude
vehicle_attitude = log_file.get_dataset('vehicle_attitude')
time_data = vehicle_attitude.data['timestamp']
q0 = vehicle_attitude.data['q[0]']
q1 = vehicle_attitude.data['q[1]']
q2 = vehicle_attitude.data['q[2]']
q3 = vehicle_attitude.data['q[3]']
quaternion2euler_array = np.frompyfunc(quaternion2euler, 4, 3)
roll, pitch, yaw = quaternion2euler_array(q0, q1, q2, q3)
fig, axs = plt.subplots(3, 1, sharex='all')
axs[0].set_title('vehicle_attitude')
axs[0].plot(time_data, roll * __rad2deg__, drawstyle='steps-post')
axs[0].set_ylabel('Roll')
axs[0].grid(True)
axs[1].plot(time_data, pitch * __rad2deg__, drawstyle='steps-post')
axs[1].set_ylabel('Pitch')
axs[1].grid(True)
axs[2].plot(time_data, yaw * __rad2deg__, drawstyle='steps-post')
axs[2].set_ylabel('Yaw')
axs[2].grid(True)
fig.tight_layout()
fig.savefig(f"vehicle_attitude.pdf",
dpi=None,
facecolor='w',
edgecolor='w',
orientation='portrait',
papertype='a4',
format='pdf',
transparent=False,
bbox_inches=None,
pad_inches=0.1,
frameon=None,
metadata=None)
# VERTICAL AXIS
actuator_outputs = log_file.get_dataset('actuator_outputs')
time_data_outputs = actuator_outputs.data['timestamp']
# motors 2 & 3
left_motors = (actuator_outputs.data['output[1]'] +
actuator_outputs.data['output[2]']) / 2
# motors 1 & 4
right_motors = (actuator_outputs.data['output[0]'] +
actuator_outputs.data['output[3]']) / 2
thrust = (actuator_outputs.data['output[1]'] +
actuator_outputs.data['output[2]'] +
actuator_outputs.data['output[0]'] +
actuator_outputs.data['output[3]']) / 4
fig, axs = plt.subplots(2, 1, sharex='all')
axs[0].set_title('Vertical axis')
axs[0].plot(time_data_outputs, thrust, drawstyle='steps-post')
axs[0].set_ylabel('Thrust')
axs[0].grid(True)
vehicle_global_position = log_file.get_dataset('vehicle_global_position')
time_global_position = vehicle_global_position.data['timestamp']
alt = vehicle_global_position.data['alt']
axs[1].plot(time_global_position, alt, drawstyle='steps-post')
axs[1].set_ylabel('Altitude')
axs[1].grid(True)
fig.tight_layout()
fig.savefig(f"vertical_axis.pdf",
dpi=None,
facecolor='w',
edgecolor='w',
orientation='portrait',
papertype='a4',
format='pdf',
transparent=False,
bbox_inches=None,
pad_inches=0.1,
frameon=None,
metadata=None)
# LATERAL AXIS
fig, axs = plt.subplots(4, 1, sharex='all')
axs[0].set_title('Lateral axis')
axs[0].plot(time_data, roll * __rad2deg__, drawstyle='steps-post')
axs[0].set_ylabel('Roll')
axs[0].grid(True)
axs[1].plot(time_data_outputs, left_motors, drawstyle='steps-post')
axs[1].set_ylabel('Left motors')
axs[1].grid(True)
axs[2].plot(time_data_outputs, right_motors, drawstyle='steps-post')
axs[2].set_ylabel('Right motors')
axs[2].grid(True)
axs[3].plot(time_data_outputs,
left_motors - right_motors,
drawstyle='steps-post')
axs[3].grid(True)
fig.tight_layout()
fig.savefig(f"lateral_axis.pdf",
dpi=None,
facecolor='w',
edgecolor='w',
orientation='portrait',
papertype='a4',
format='pdf',
transparent=False,
bbox_inches=None,
pad_inches=0.1,
frameon=None,
metadata=None)
def setUp(self):
test_log_path = ut.ulog.download_log(TEST_LOG_URL, TEST_TMP_DIR)
self.log = pyulog.ULog(test_log_path)
import config
import matplotlib.pyplot as plt
import pandas as pd
import sys
import pyulog
if __name__ == '__main__':
# Usage: ./verify_motor_outputs.py <log_file_path> <this_drone_docking_slot> [other_drone_docking_slot]
log_file_path = sys.argv[1]
this_drone_docking_slot = int(
sys.argv[2]) # drone that the log file is from
other_drone_docking_slot = this_drone_docking_slot # drone to show expected actuator outputs for. If it is a different drone, they won't be very accurate.
if len(sys.argv) >= 4:
other_drone_docking_slot = int(sys.argv[3])
ulog = pyulog.ULog(log_file_path)
params = ulog.initial_parameters
PWM_MIN = params['PWM_AUX_MIN']
PWM_MAX = params['PWM_AUX_MAX']
THR_MDL_FAC = params['THR_MDL_FAC']
actuator_controls_data = ulog.get_dataset('actuator_controls_0').data
rotor_index_start = other_drone_docking_slot * config.constants.num_rotors
rotor_index_end = rotor_index_start + config.constants.num_rotors
missing_drones = [] # 0 through 7
geometry = config.generate_matrices.generate_aviata_matrices(
missing_drones,
optimize=False) # 10/30/21 tests did not use optimized mixer
mixer = geometry['mix']['B_px_4dof']
