From 2dad6c1cbd9282a5268b55fec3a2f45bd92ad854 Mon Sep 17 00:00:00 2001 From: Simone Ciresa <149970685+ciresimo@users.noreply.github.com> Date: Wed, 29 May 2024 13:38:56 +0200 Subject: [PATCH] tailsitter : correct rate setpoint to match rate for tailsitter (#275) --------- Co-authored-by: simone.ciresa@rigi.tech --- app/plot_app/configured_plots.py | 20 ++++++++----- app/plot_app/vtol_tailsitter.py | 50 ++++++++++++++++++++++++++++++-- 2 files changed, 61 insertions(+), 9 deletions(-) diff --git a/app/plot_app/configured_plots.py b/app/plot_app/configured_plots.py index a16b0c603..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_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']): @@ -250,16 +251,21 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page, 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) + 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..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 (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,51 @@ 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]