def get_pqr_angvel_df(tmpdir, ulgfile):
csvname = 'vehicle_angular_velocity_0'
angvel_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
angvel_df['roll rate'] = angvel_df['xyz[0]'].values * 180 / np.pi
angvel_df['pitch rate'] = angvel_df['xyz[1]'].values * 180 / np.pi
angvel_df['yaw rate'] = angvel_df['xyz[2]'].values * 180 / np.pi
angvel_df['pqr norm'] = np.linalg.norm([
angvel_df['roll rate'].values, angvel_df['pitch rate'].values,
angvel_df['yaw rate'].values
],
axis=0)
angvel_df = PandasTools.convert_index_from_us_to_s(angvel_df)
csvname = 'vehicle_rates_setpoint_0'
angvelsp_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
angvelsp_df['roll rate sp'] = angvelsp_df['roll'].values * 180 / np.pi
angvelsp_df[
'pitch rate sp'] = angvelsp_df['pitch'].values * 180 / np.pi
angvelsp_df['yaw rate sp'] = angvelsp_df['yaw'].values * 180 / np.pi
angvelsp_df['pqr norm sp'] = np.linalg.norm([
angvelsp_df['roll rate sp'].values,
angvelsp_df['pitch rate sp'].values,
angvelsp_df['yaw rate sp'].values
],
axis=0)
angvelsp_df = PandasTools.convert_index_from_us_to_s(angvelsp_df)
return [angvel_df, angvelsp_df]
def get_man_ctrl_df(tmpdir, ulgfile):
csvname = 'manual_control_setpoint_0'
sticks_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
sticks_df.rename(columns={
"x": "roll stick",
"y": "pitch stick",
"z": "throttle stick",
'r': "yaw stick"
},
inplace=True)
sticks_df = PandasTools.convert_index_from_us_to_s(sticks_df)
csvname = 'manual_control_switches_0'
switches_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
switches_df = PandasTools.convert_index_from_us_to_s(switches_df)
return [sticks_df, switches_df]
def get_firefly_delta_df(tmpdir, ulgfile):
csvname = 'firefly_delta'
in_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
in_df.rename(columns={
"control[0]": "p rate cmd",
"control[1]": "q rate cmd",
"control[2]": "r rate cmd",
'control[3]': "az cmd"
},
inplace=True)
in_df = PandasTools.convert_index_from_us_to_s(in_df)
# csvname = 'actuator_outputs_0'
csvname = 'actuator_outputs_1'
out_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
out_df = PandasTools.convert_index_from_us_to_s(out_df)
return [in_df, out_df]
def get_xyz_posvel_df(tmpdir, ulgfile):
csvname = 'vehicle_local_position_0'
pv_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
pv_df['vnorm'] = np.linalg.norm(
[pv_df['vx'].values, pv_df['vy'].values, pv_df['vz'].values],
axis=0)
pv_df['pnorm'] = np.linalg.norm(
[pv_df['x'].values, pv_df['y'].values, pv_df['z'].values], axis=0)
pv_df = PandasTools.convert_index_from_us_to_s(pv_df)
return pv_df
def get_rpy_angles_df(tmpdir, ulgfile):
csvname = 'vehicle_attitude_0_deg'
att_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
att_df['angle norm'] = np.linalg.norm([
att_df['roll'].values, att_df['pitch'].values, att_df['yaw'].values
],
axis=0)
att_df = PandasTools.convert_index_from_us_to_s(att_df)
csvname = 'vehicle_attitude_setpoint_0'
att_dfsp = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
att_dfsp['roll sp'] = att_dfsp['roll_body'].values * 180 / np.pi
att_dfsp['pitch sp'] = att_dfsp['pitch_body'].values * 180 / np.pi
att_dfsp['yaw sp'] = att_dfsp['yaw_body'].values * 180 / np.pi
att_dfsp['angle norm sp'] = np.linalg.norm([
att_dfsp['roll sp'].values, att_dfsp['pitch sp'].values,
att_dfsp['yaw sp'].values
],
axis=0)
att_dfsp = PandasTools.convert_index_from_us_to_s(att_dfsp)
return [att_df, att_dfsp]
def get_xyz_accel_df(tmpdir, ulgfile):
csvname = 'vehicle_acceleration_0'
accel_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
accel_df['ax'] = accel_df['xyz[0]'].values
accel_df['ay'] = accel_df['xyz[1]'].values
accel_df['az'] = accel_df['xyz[2]'].values
accel_df['acc norm'] = np.linalg.norm([
accel_df['ax'].values, accel_df['ay'].values, accel_df['az'].values
],
axis=0)
accel_df = PandasTools.convert_index_from_us_to_s(accel_df)
return accel_df
def get_pqr_angacc_df(tmpdir, ulgfile):
csvname = 'vehicle_angular_acceleration_0'
angacc_df = UlgParser.get_pandas_dataframe(tmpdir, ulgfile, csvname)
angacc_df['p rate'] = angacc_df['xyz[0]'].values * 180 / np.pi
angacc_df['q rate'] = angacc_df['xyz[1]'].values * 180 / np.pi
angacc_df['r rate'] = angacc_df['xyz[2]'].values * 180 / np.pi
angacc_df['angacc norm'] = np.linalg.norm([
angacc_df['p rate'].values, angacc_df['q rate'].values,
angacc_df['r rate'].values
],
axis=0)
angacc_df = PandasTools.convert_index_from_us_to_s(angacc_df)
return angacc_df
def ctrl_alloc_model(self, ulgfile, time_win):
[df_in, df_out] = UlgParser.get_ctrl_alloc_df(self.tmpdir, ulgfile, time_win)
# PandasTools.resample(df_in, 'roll rate cmd', df_out, 'output[0]')
df_in = PandasTools.interpolate_df1_according_to_df2_index(df_in, df_out)
xvect = df_in[['roll rate cmd', 'pitch rate cmd', 'yaw rate cmd', 'az cmd']].values
yvect = df_out[['output[0]', 'output[1]', 'output[2]', 'output[3]',
'output[4]', 'output[5]', 'output[6]', 'output[7]']].values
xvect = np.array(xvect).transpose()
yvect = np.array(yvect).transpose()
print(xvect.shape)
print(yvect.shape)
# Convert from [1000 2000] to [-1 +1]
# [1000 2000] - 1500 = [-500 +500]
# [-500 +500] / 500 = [-1 +1]
yvect = yvect - 1500
yvect = yvect / 500
[lsq_matrix, lsq_bias, lsq_error] = UlgPlotMixer.least_square_fit(xvect, yvect)
_ = lsq_error
self.ctrl_alloc_model_plot(ulgfile, df_out, xvect, yvect, lsq_matrix, lsq_bias)