From 84fe077dd9cae77d41f75261b20669713a21a69a Mon Sep 17 00:00:00 2001 From: "simone.ciresa@rigi.tech" Date: Thu, 23 May 2024 16:25:32 +0200 Subject: [PATCH 1/3] Correct rate setpoint to match rate for tailsitter --- app/plot_app/configured_plots.py | 28 ++++++++++++------ app/plot_app/vtol_tailsitter.py | 51 ++++++++++++++++++++++++++++++-- 2 files changed, 68 insertions(+), 11 deletions(-) diff --git a/app/plot_app/configured_plots.py b/app/plot_app/configured_plots.py index a16b0c603..cad89af60 100644 --- a/app/plot_app/configured_plots.py +++ b/app/plot_app/configured_plots.py @@ -207,7 +207,7 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page, # VTOL tailistter orientation conversion, if relevant if is_vtol_tailsitter: - [tailsitter_attitude, tailsitter_rates] = tailsitter_orientation(ulog, vtol_states) + [tailsitter_attitude, tailsitter_rates, tailsitter_rates_setpoint] = tailsitter_orientation(ulog, vtol_states) # Roll/Pitch/Yaw angle & angular rate for index, axis in enumerate(['roll', 'pitch', 'yaw']): @@ -248,18 +248,28 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page, plot_height='small', changed_params=changed_params, x_range=x_range) if is_vtol_tailsitter: - if tailsitter_rates[axis] is not None: - data_plot.add_graph([lambda data: (axis+'_q', - np.rad2deg(tailsitter_rates[axis]))], - colors3[0:1], [axis_name+' Rate Estimated'], mark_nan=True) + # if tailsitter_rates[axis] is not None: + # data_plot.add_graph([lambda data: (axis+'_q', + # np.rad2deg(tailsitter_rates[axis]))], + # colors3[0:1], [axis_name+' Rate Estimated'], mark_nan=True) + # data_plot.add_graph([lambda data: (axis, np.rad2deg(tailsitter_rates_setpoint[axis]))], + # colors3[1:2], [axis_name+' Rate Setpoint'], + # mark_nan=True, use_step_lines=True) + data_plot.add_graph([lambda data: (axis+'_q', + np.rad2deg(tailsitter_rates[axis]))], + colors3[0:1], [axis_name+' Rate Estimated'], mark_nan=True) + data_plot.change_dataset('vehicle_rates_setpoint') + data_plot.add_graph([lambda data: (axis, np.rad2deg(tailsitter_rates_setpoint[axis]))], + colors3[1:2], [axis_name+' Rate Setpoint'], + mark_nan=True, use_step_lines=True) else: data_plot.add_graph([lambda data: (axis+'speed', np.rad2deg(data[rate_field_names[index]]))], colors3[0:1], [axis_name+' Rate Estimated'], mark_nan=True) - data_plot.change_dataset('vehicle_rates_setpoint') - data_plot.add_graph([lambda data: (axis, np.rad2deg(data[axis]))], - colors3[1:2], [axis_name+' Rate Setpoint'], - mark_nan=True, use_step_lines=True) + data_plot.change_dataset('vehicle_rates_setpoint') + data_plot.add_graph([lambda data: (axis, np.rad2deg(data[axis]))], + colors3[1:2], [axis_name+' Rate Setpoint'], + mark_nan=True, use_step_lines=True) axis_letter = axis[0].upper() rate_int_limit = '(*100)' # this param is MC/VTOL only (it will not exist on FW) diff --git a/app/plot_app/vtol_tailsitter.py b/app/plot_app/vtol_tailsitter.py index e2116e3e6..aa14a0779 100644 --- a/app/plot_app/vtol_tailsitter.py +++ b/app/plot_app/vtol_tailsitter.py @@ -81,7 +81,7 @@ def tailsitter_orientation(ulog, vtol_states): w_y = cur_dataset.data['xyz[2]'] w_t = cur_dataset.data['timestamp'] - # fw rates (roll and yaw swap, roll is negative axis) + #fw rates and setpoints(roll and yaw swap, roll is negative axis) w_r_fw = w_y*-1 w_y_fw = w_r*1 # *1 to get python to copy not reference # temporary variables for storing VTOL states @@ -112,5 +112,52 @@ def tailsitter_orientation(ulog, vtol_states): except (KeyError, IndexError) as error: vtol_rates = {'roll': None, 'pitch': None, 'yaw': None} + print("ERROR: Tailsitter rate ") + - return [vtol_attitude, vtol_rates] + # correct rates setpoint for VTOL tailsitter in FW mode + try: + + setp_dataset = ulog.get_dataset('vehicle_rates_setpoint') + setp_r = setp_dataset.data['roll'] + setp_p = setp_dataset.data['pitch'] + setp_y = setp_dataset.data['yaw'] + w_t = setp_dataset.data['timestamp'] + + # fw setpoints(roll and yaw swap, roll is negative axis) + + setp_r_fw = setp_y*-1 + setp_y_fw = setp_r*1 + # temporary variables for storing VTOL states + + is_vtol_fw = False + fw_start = np.nan + fw_end = np.nan + + for i in vtol_states: + # states: 1=transition, 2=FW, 3=MC + # if in FW mode then used FW conversions + if is_vtol_fw: + fw_end = i[0] + setp_r[np.logical_and(w_t > fw_start, w_t < fw_end)] = \ + setp_r_fw[np.logical_and(w_t > fw_start, w_t < fw_end)] + setp_y[np.logical_and(w_t > fw_start, w_t < fw_end)] = \ + setp_y_fw[np.logical_and(w_t > fw_start, w_t < fw_end)] + is_vtol_fw = False + if i[1] == 2: + fw_start = i[0] + is_vtol_fw = True + + # if flight ended as FW, convert the final data segment to FW + if is_vtol_fw: + setp_r[quat_t > fw_start] = setp_r_fw[quat_t > fw_start] + setp_y[quat_t > fw_start] = setp_y_fw[quat_t > fw_start] + + vtol_rates_setpoint = {'roll': setp_r, 'pitch': setp_p, 'yaw': setp_y} + + except (KeyError, IndexError) as error: + vtol_rates_setpoint = {'roll': None, 'pitch': None, 'yaw': None} + print("ERROR: Tailsitter rate setpoint") + + + return [vtol_attitude, vtol_rates, vtol_rates_setpoint] From 4dc05ff2ce19cdac3d7e4a9ba7fe8d7255780e0d Mon Sep 17 00:00:00 2001 From: "simone.ciresa@rigi.tech" Date: Thu, 23 May 2024 16:25:32 +0200 Subject: [PATCH 2/3] old comments removal --- app/plot_app/configured_plots.py | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/app/plot_app/configured_plots.py b/app/plot_app/configured_plots.py index cad89af60..2d05013e3 100644 --- a/app/plot_app/configured_plots.py +++ b/app/plot_app/configured_plots.py @@ -248,18 +248,12 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page, plot_height='small', changed_params=changed_params, x_range=x_range) if is_vtol_tailsitter: - # if tailsitter_rates[axis] is not None: - # data_plot.add_graph([lambda data: (axis+'_q', - # np.rad2deg(tailsitter_rates[axis]))], - # colors3[0:1], [axis_name+' Rate Estimated'], mark_nan=True) - # data_plot.add_graph([lambda data: (axis, np.rad2deg(tailsitter_rates_setpoint[axis]))], - # colors3[1:2], [axis_name+' Rate Setpoint'], - # mark_nan=True, use_step_lines=True) - data_plot.add_graph([lambda data: (axis+'_q', + if tailsitter_rates[axis] is not None: + data_plot.add_graph([lambda data: (axis+'_q', np.rad2deg(tailsitter_rates[axis]))], colors3[0:1], [axis_name+' Rate Estimated'], mark_nan=True) - data_plot.change_dataset('vehicle_rates_setpoint') - data_plot.add_graph([lambda data: (axis, np.rad2deg(tailsitter_rates_setpoint[axis]))], + data_plot.change_dataset('vehicle_rates_setpoint') + data_plot.add_graph([lambda data: (axis, np.rad2deg(tailsitter_rates_setpoint[axis]))], colors3[1:2], [axis_name+' Rate Setpoint'], mark_nan=True, use_step_lines=True) else: From 36422488cfaecabcfd83c52a99d72f5887c106e0 Mon Sep 17 00:00:00 2001 From: "simone.ciresa@rigi.tech" Date: Thu, 23 May 2024 17:20:23 +0200 Subject: [PATCH 3/3] Trailing whitespece --- app/plot_app/configured_plots.py | 6 ++++-- app/plot_app/vtol_tailsitter.py | 11 +++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/app/plot_app/configured_plots.py b/app/plot_app/configured_plots.py index 2d05013e3..d85733590 100644 --- a/app/plot_app/configured_plots.py +++ b/app/plot_app/configured_plots.py @@ -207,7 +207,8 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page, # VTOL tailistter orientation conversion, if relevant if is_vtol_tailsitter: - [tailsitter_attitude, tailsitter_rates, tailsitter_rates_setpoint] = tailsitter_orientation(ulog, vtol_states) + [tailsitter_attitude, tailsitter_rates, tailsitter_rates_setpoint] = tailsitter_orientation( + ulog, vtol_states) # Roll/Pitch/Yaw angle & angular rate for index, axis in enumerate(['roll', 'pitch', 'yaw']): @@ -253,7 +254,8 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page, np.rad2deg(tailsitter_rates[axis]))], colors3[0:1], [axis_name+' Rate Estimated'], mark_nan=True) data_plot.change_dataset('vehicle_rates_setpoint') - data_plot.add_graph([lambda data: (axis, np.rad2deg(tailsitter_rates_setpoint[axis]))], + data_plot.add_graph([lambda data: (axis, np.rad2deg( + tailsitter_rates_setpoint[axis]))], colors3[1:2], [axis_name+' Rate Setpoint'], mark_nan=True, use_step_lines=True) else: diff --git a/app/plot_app/vtol_tailsitter.py b/app/plot_app/vtol_tailsitter.py index aa14a0779..2a59f6182 100644 --- a/app/plot_app/vtol_tailsitter.py +++ b/app/plot_app/vtol_tailsitter.py @@ -81,7 +81,7 @@ def tailsitter_orientation(ulog, vtol_states): w_y = cur_dataset.data['xyz[2]'] w_t = cur_dataset.data['timestamp'] - #fw rates and setpoints(roll and yaw swap, roll is negative axis) + # fw rates and setpoints(roll and yaw swap, roll is negative axis) w_r_fw = w_y*-1 w_y_fw = w_r*1 # *1 to get python to copy not reference # temporary variables for storing VTOL states @@ -113,19 +113,18 @@ def tailsitter_orientation(ulog, vtol_states): except (KeyError, IndexError) as error: vtol_rates = {'roll': None, 'pitch': None, 'yaw': None} print("ERROR: Tailsitter rate ") - # correct rates setpoint for VTOL tailsitter in FW mode try: - + setp_dataset = ulog.get_dataset('vehicle_rates_setpoint') setp_r = setp_dataset.data['roll'] setp_p = setp_dataset.data['pitch'] setp_y = setp_dataset.data['yaw'] w_t = setp_dataset.data['timestamp'] - + # fw setpoints(roll and yaw swap, roll is negative axis) - + setp_r_fw = setp_y*-1 setp_y_fw = setp_r*1 # temporary variables for storing VTOL states @@ -138,7 +137,7 @@ def tailsitter_orientation(ulog, vtol_states): # states: 1=transition, 2=FW, 3=MC # if in FW mode then used FW conversions if is_vtol_fw: - fw_end = i[0] + fw_end = i[0] setp_r[np.logical_and(w_t > fw_start, w_t < fw_end)] = \ setp_r_fw[np.logical_and(w_t > fw_start, w_t < fw_end)] setp_y[np.logical_and(w_t > fw_start, w_t < fw_end)] = \