From 982c998ab9f4a6355e810c425d5a24ef631fed47 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 22 Jan 2024 16:51:12 +0100 Subject: [PATCH] mc_attitude_control: move attitude setpoint pulling to right before usage Signed-off-by: Silvan Fuhrer --- .../mc_att_control/mc_att_control_main.cpp | 59 ++++++++++--------- 1 file changed, 30 insertions(+), 29 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 756311b4aec2..39cb033e74dc 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -226,34 +226,6 @@ MulticopterAttitudeControl::Run() const Quatf q{v_att.q}; - // Check for new attitude setpoint - if (_vehicle_attitude_setpoint_sub.updated()) { - vehicle_attitude_setpoint_s vehicle_attitude_setpoint; - - if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) - && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { - - _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); - _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); - _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; - } - } - - // Check for a heading reset - if (_quat_reset_counter != v_att.quat_reset_counter) { - const Quatf delta_q_reset(v_att.delta_q_reset); - - // for stabilized attitude generation only extract the heading change from the delta quaternion - _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); - - if (v_att.timestamp > _last_attitude_setpoint) { - // adapt existing attitude setpoint unless it was generated after the current attitude estimate - _attitude_control.adaptAttitudeSetpoint(delta_q_reset); - } - - _quat_reset_counter = v_att.quat_reset_counter; - } - /* check for updates in other topics */ _manual_control_setpoint_sub.update(&_manual_control_setpoint); _vehicle_control_mode_sub.update(&_vehicle_control_mode); @@ -295,7 +267,8 @@ MulticopterAttitudeControl::Run() // vehicle is a tailsitter in transition mode const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode); - bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); + const bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering + || is_tailsitter_transition); if (run_att_ctrl) { @@ -313,6 +286,34 @@ MulticopterAttitudeControl::Run() _man_pitch_input_filter.reset(0.f); } + // Check for new attitude setpoint + if (_vehicle_attitude_setpoint_sub.updated()) { + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + + if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) + && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { + + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); + _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); + _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; + } + } + + // Check for a heading reset + if (_quat_reset_counter != v_att.quat_reset_counter) { + const Quatf delta_q_reset(v_att.delta_q_reset); + + // for stabilized attitude generation only extract the heading change from the delta quaternion + _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); + + if (v_att.timestamp > _last_attitude_setpoint) { + // adapt existing attitude setpoint unless it was generated after the current attitude estimate + _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + } + + _quat_reset_counter = v_att.quat_reset_counter; + } + Vector3f rates_sp = _attitude_control.update(q); const hrt_abstime now = hrt_absolute_time();