From 9c5a42375330217cfbacedadc28416ecb05bf640 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 28 Sep 2022 16:04:24 +0200 Subject: [PATCH] VTOL: in update_transition_state(): check that needed virtual attitude setpoints are recent This is to prevent that that data from non-recent publications of the virtual attitude setpoints are used in the transition code. It removes the previous implementation where the update_transition_state() method was only run when both mc and fw virtual att sp where recent, independetly of whether both are used or not. Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/standard.cpp | 15 +++++++++++++-- src/modules/vtol_att_control/tailsitter.cpp | 8 +++++++- src/modules/vtol_att_control/tiltrotor.cpp | 14 +++++++++++++- .../vtol_att_control/vtol_att_control_main.cpp | 9 ++------- 4 files changed, 35 insertions(+), 11 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c8536fd964..37dd6b319a 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -203,18 +203,29 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { + const hrt_abstime now = hrt_absolute_time(); float mc_weight = 1.0f; - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f; VtolType::update_transition_state(); - // we get attitude setpoint from a multirotor flighttask if altitude is controlled. + // we get attitude setpoint from a multirotor flighttask if climbrate is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { + // we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active) + if (_mc_virtual_att_sp->timestamp < (now - 1_s) && _fw_virtual_att_sp->timestamp < (now - 1_s)) { + return; + } + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; } else { + // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) + if (_fw_virtual_att_sp->timestamp < (now - 1_s)) { + return; + } + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 46640a4d89..9b9d436e3e 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -188,7 +188,13 @@ void Tailsitter::update_vtol_state() void Tailsitter::update_transition_state() { - const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + const hrt_abstime now = hrt_absolute_time(); + const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f; + + // we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active) + if (_mc_virtual_att_sp->timestamp < (now - 1_s) && _fw_virtual_att_sp->timestamp < (now - 1_s)) { + return; + } if (!_flag_was_in_trans_mode) { _flag_was_in_trans_mode = true; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 1cd7b13d22..3ce0ecbe91 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -280,19 +280,31 @@ void Tiltrotor::update_transition_state() { VtolType::update_transition_state(); + const hrt_abstime now = hrt_absolute_time(); + // we get attitude setpoint from a multirotor flighttask if altitude is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { + // we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active) + if (_mc_virtual_att_sp->timestamp < (now - 1_s) && _fw_virtual_att_sp->timestamp < (now - 1_s)) { + return; + } + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { + // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) + if (_fw_virtual_att_sp->timestamp < (now - 1_s)) { + return; + } + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); _thrust_transition = _fw_virtual_att_sp->thrust_body[0]; } - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f; if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { // for the first part of the transition all rotors are enabled diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index be0db852c4..fbd0703972 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -322,11 +322,6 @@ VtolAttitudeControl::Run() const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); - // for transition code to publish an attitude setpoint require both mc and fw virtual attitude setpoint to not contain data older than one second. - // this prevents either topic from being used with old data at the time when we switch into transition mode - const bool mc_and_fw_att_sp_are_recent = _mc_virtual_att_sp.timestamp > (now - 1_s) - && _fw_virtual_att_sp.timestamp > (now - 1_s); - // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); @@ -336,7 +331,7 @@ VtolAttitudeControl::Run() // vehicle is doing a transition to FW _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW; - if (mc_and_fw_att_sp_are_recent && (mc_att_sp_updated || fw_att_sp_updated)) { + if (mc_att_sp_updated || fw_att_sp_updated) { _vtol_type->update_transition_state(); _vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp); } @@ -347,7 +342,7 @@ VtolAttitudeControl::Run() // vehicle is doing a transition to MC _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC; - if (mc_and_fw_att_sp_are_recent && (mc_att_sp_updated || fw_att_sp_updated)) { + if (mc_att_sp_updated || fw_att_sp_updated) { _vtol_type->update_transition_state(); _vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp); }