From 079b756f1b79866720c76417b8edd24547894703 Mon Sep 17 00:00:00 2001 From: Alvaro Fernandez Date: Mon, 11 Nov 2024 02:53:28 -0800 Subject: [PATCH] VTOL: Added position feedback to backward transition braking controller(#23731) Instead of tracking a fixed deceleration setpoint during the backtransition we added here position feedback, such that the vehicle comes to a stop latest at the current position setpoint. This reduces the risk of overshooting the landing point. If no position feedback/position setpoint is present the old logic still applies. It further moves the braking controller to the FlightTaskTransition instad of doing it in the VTOL attitude mode. * vtol_type: added position feedback to backward transition * FlgithTaskTransition: refactored backtransition deceleration/pitch setpoint computation * FlightTaskTransition: minor improvements * FlightTaskTransition: use .dot() consistently and remove unnecessary comments Signed-off-by: Silvan Fuhrer --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Silvan Fuhrer --- .../tasks/Transition/FlightTaskTransition.cpp | 151 ++++++++++++++---- .../tasks/Transition/FlightTaskTransition.hpp | 18 +++ src/modules/vtol_att_control/standard.cpp | 3 +- src/modules/vtol_att_control/tiltrotor.cpp | 2 +- src/modules/vtol_att_control/vtol_type.cpp | 31 ---- src/modules/vtol_att_control/vtol_type.h | 1 - 6 files changed, 140 insertions(+), 66 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index bf5d2799d3..badf32fe7f 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -42,32 +42,10 @@ using namespace matrix; FlightTaskTransition::FlightTaskTransition() { _param_handle_pitch_cruise_degrees = param_find("FW_PSP_OFF"); + _param_handle_vt_b_dec_i = param_find("VT_B_DEC_I"); + _param_handle_vt_b_dec_mss = param_find("VT_B_DEC_MSS"); - if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { - param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); - } - -} - -bool FlightTaskTransition::updateInitialize() -{ - updateParameters(); - return FlightTask::updateInitialize(); -} - -void FlightTaskTransition::updateParameters() -{ - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { - param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); - } - } + updateParametersFromStorage(); } bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) @@ -76,6 +54,8 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); + _decel_error_bt_int = 0.f; + if (PX4_ISFINITE(last_setpoint.velocity[2])) { _vel_z_filter.reset(last_setpoint.velocity[2]); @@ -85,12 +65,7 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) _velocity_setpoint(2) = _vel_z_filter.getState(); - _sub_vehicle_status.update(); - - const bool is_vtol_front_transition = _sub_vehicle_status.get().in_transition_mode - && _sub_vehicle_status.get().in_transition_to_fw; - - if (is_vtol_front_transition) { + if (isVtolFrontTransition()) { _gear.landing_gear = landing_gear_s::GEAR_UP; } else { @@ -100,6 +75,13 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) return ret; } +bool FlightTaskTransition::updateInitialize() +{ + updateParameters(); + updateSubscribers(); + return FlightTask::updateInitialize(); +} + bool FlightTaskTransition::update() { // tailsitters will override attitude and thrust setpoint @@ -111,7 +93,13 @@ bool FlightTaskTransition::update() // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees // and zero roll angle const Vector2f tmp = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f); - _acceleration_setpoint.xy() = tmp * tanf(math::radians(_param_pitch_cruise_degrees)) * CONSTANTS_ONE_G; + float pitch_setpoint = math::radians(_param_pitch_cruise_degrees); + + if (isVtolBackTransition()) { + pitch_setpoint = computeBackTranstionPitchSetpoint(); + } + + _acceleration_setpoint.xy() = tmp * tanf(pitch_setpoint) * CONSTANTS_ONE_G; // slowly move vertical velocity setpoint to zero _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); @@ -120,3 +108,102 @@ bool FlightTaskTransition::update() _yaw_setpoint = NAN; return ret; } + +void FlightTaskTransition::updateParameters() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + updateParametersFromStorage(); + } +} + +void FlightTaskTransition::updateParametersFromStorage() +{ + if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { + param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); + } + + if (_param_handle_vt_b_dec_i != PARAM_INVALID) { + param_get(_param_handle_vt_b_dec_i, &_param_vt_b_dec_i); + } + + if (_param_handle_vt_b_dec_mss != PARAM_INVALID) { + param_get(_param_handle_vt_b_dec_mss, &_param_vt_b_dec_mss); + } +} + +void FlightTaskTransition::updateSubscribers() +{ + _sub_vehicle_status.update(); + _sub_position_sp_triplet.update(); + _sub_vehicle_local_position.update(); +} + +bool FlightTaskTransition::isVtolFrontTransition() const +{ + return _sub_vehicle_status.get().in_transition_mode + && _sub_vehicle_status.get().in_transition_to_fw; + +} + +bool FlightTaskTransition::isVtolBackTransition() const +{ + return _sub_vehicle_status.get().in_transition_mode + && !_sub_vehicle_status.get().in_transition_to_fw; +} + +float FlightTaskTransition::computeBackTranstionPitchSetpoint() +{ + const vehicle_local_position_s &local_pos = _sub_vehicle_local_position.get(); + const position_setpoint_s ¤t_pos_sp = _sub_position_sp_triplet.get().current; + + // Retrieve default decelaration setpoint from param + const float default_deceleration_sp = _param_vt_b_dec_mss; + + float deceleration_sp = default_deceleration_sp; + + const Vector2f position_local{local_pos.x, local_pos.y}; + const Vector2f velocity_local{local_pos.vx, local_pos.vy}; + const Vector2f acceleration_local{local_pos.ax, local_pos.ay}; + + const float accel_in_flight_direction = acceleration_local.dot(velocity_local.unit_or_zero()); + + if (current_pos_sp.valid && local_pos.xy_valid && local_pos.xy_global) { + + Vector2f position_setpoint_local {0.f, 0.f}; + _geo_projection.project(current_pos_sp.lat, current_pos_sp.lon, position_setpoint_local(0), + position_setpoint_local(1)); + + const Vector2f pos_sp_dist = position_setpoint_local - position_local; // backtransition end-point w.r.t. vehicle + + const float dist_body_forward = pos_sp_dist.dot(velocity_local.unit_or_zero()); + + // Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle + if (dist_body_forward > FLT_EPSILON && PX4_ISFINITE(velocity_local.norm())) { + + // Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped + deceleration_sp = velocity_local.norm_squared() / (2.f * dist_body_forward); + // Limit the deceleration setpoint to maximal twice the default deceleration setpoint (param) + deceleration_sp = math::constrain(deceleration_sp, 0.f, 2.f * default_deceleration_sp); + } + } + + // positive means decelerating too slow, need to pitch up (must reverse accel_body_forward, as it is a positive number) + const float deceleration_error = deceleration_sp - (-accel_in_flight_direction); + + updateBackTransitioDecelerationErrorIntegrator(deceleration_error); + + return _decel_error_bt_int; +} + +void FlightTaskTransition::updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error) +{ + const float integrator_input = _param_vt_b_dec_i * deceleration_error; + + _decel_error_bt_int += integrator_input * math::constrain(_deltatime, 0.001f, 0.1f); + _decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, _pitch_limit_bt); +} diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp index 6e255dbd1c..901da114cc 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include using namespace time_literals; @@ -66,12 +67,29 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _sub_position_sp_triplet{ORB_ID(position_setpoint_triplet)}; param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID}; float _param_pitch_cruise_degrees{0.f}; + param_t _param_handle_vt_b_dec_i{PARAM_INVALID}; + float _param_vt_b_dec_i{0.f}; + param_t _param_handle_vt_b_dec_mss{PARAM_INVALID}; + float _param_vt_b_dec_mss{0.f}; AlphaFilter _vel_z_filter; + float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value + + static constexpr float _pitch_limit_bt = .3f; ///< Bactransition pitch limit void updateParameters(); + void updateParametersFromStorage(); + + void updateSubscribers(); + + bool isVtolFrontTransition() const; + bool isVtolBackTransition() const; + + float computeBackTranstionPitchSetpoint(); + void updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error); }; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d8d640790e..5422a01898 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -241,10 +241,11 @@ void Standard::update_transition_state() if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. - pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta(); } const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3a19042579..9a05cf1034 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -295,7 +295,7 @@ void Tiltrotor::update_transition_state() // control backtransition deceleration using pitch. if (_v_control_mode->flag_control_climb_rate_enabled) { - pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta(); } if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 458b3927fb..e7ec2d3e01 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -168,37 +168,6 @@ void VtolType::update_transition_state() _last_thr_in_mc = _vehicle_thrust_setpoint_virtual_mc->xyz[2]; } -float VtolType::update_and_get_backtransition_pitch_sp() -{ - // maximum up or down pitch the controller is allowed to demand - const float pitch_lim = 0.3f; - const Eulerf euler(Quatf(_v_att->q)); - - const float track = atan2f(_local_pos->vy, _local_pos->vx); - const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay; - - // increase the target deceleration setpoint provided to the controller by 20% - // to make overshooting the transition waypoint less likely in the presence of tracking errors - const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f; - - // get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number) - const float accel_error_forward = dec_sp + accel_body_forward; - - const float pitch_sp_new = _accel_to_pitch_integ; - - float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward; - - if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) || - (pitch_sp_new <= 0.f && accel_error_forward < 0.0f)) { - integrator_input = 0.0f; - } - - _accel_to_pitch_integ += integrator_input * _transition_dt; - - // only allow positive (pitch up) pitch setpoint - return math::constrain(pitch_sp_new, 0.f, pitch_lim); -} - bool VtolType::isFrontTransitionCompleted() { bool ret = isFrontTransitionCompletedBase(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 8f9ae9ef38..737746a78b 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -317,7 +317,6 @@ protected: bool _quadchute_command_treated{false}; - float update_and_get_backtransition_pitch_sp(); bool isFrontTransitionCompleted(); virtual bool isFrontTransitionCompletedBase();