diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index bf5d2799d3fc..671c176c66cf 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -36,38 +36,17 @@ */ #include "FlightTaskTransition.hpp" +#include 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 +55,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 +66,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 +76,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 +94,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(math::radians(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 +109,126 @@ 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() +{ + return _sub_vehicle_status.get().in_transition_mode + && _sub_vehicle_status.get().in_transition_to_fw; + +} + +bool FlightTaskTransition::isVtolBackTransition() +{ + 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 current_pos_sp = _sub_position_sp_triplet.get().current; + + // Retrieve default decelaration setpoint + const float default_deceleration_sp = _param_vt_b_dec_mss; + // Maximum allowed deceleration setpoint as a function of the nominal deceleration setpoint + const float max_deceleration_sp = 2.5f * default_deceleration_sp; + + float deceleration_sp = default_deceleration_sp; + + 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; + const float vel_body_forward = cosf(track) * local_pos.vx + sinf(track) * local_pos.vy; + + + if (current_pos_sp.valid && local_pos.xy_valid) { + + // Compute backtransition end-point in local reference frame body x-direction -> dist_body_forward + MapProjection map_proj{local_pos.ref_lat, local_pos.ref_lon}; + + float pos_sp_x, pos_sp_y = 0.f; + map_proj.project(current_pos_sp.lat, current_pos_sp.lon, pos_sp_x, + pos_sp_y); + + // Compute backtransition end-point w.r.t. vehicle + const float pos_sp_dx = pos_sp_x - local_pos.x; + const float pos_sp_dy = pos_sp_y - local_pos.y; + + // Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle + const float vel_proj = local_pos.vx * pos_sp_dx + local_pos.vy * pos_sp_dy; + + if (vel_proj > 0.0f) { + const float dist_body_forward = cosf(track) * pos_sp_dx + sinf(track) * pos_sp_dy; + + if (fabsf(dist_body_forward) > FLT_EPSILON) { + // Compute deceleration setpoint + // Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped + deceleration_sp = vel_body_forward * vel_body_forward / (2.f * dist_body_forward); + + // Check if the deceleration setpoint is finite and within limits + if (!PX4_ISFINITE(deceleration_sp)) { + deceleration_sp = default_deceleration_sp; + + } else { + // Limit the deceleration setpoint + deceleration_sp = math::constrain(deceleration_sp, 0.0f, max_deceleration_sp); + } + } + } + } + + // get accel error, positive means decelerating too slow, need to pitch up (must reverse accel_body_forward, as it is a positive number) + float deceleration_error = deceleration_sp - (-accel_body_forward); + + float pitch_sp_new = _decel_error_bt_int; + + updateBackTransitioDecelerationErrorIntegrator(deceleration_error); + + return math::constrain(pitch_sp_new, .0f, _pitch_limit_bt);; +} + +void FlightTaskTransition::updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error) +{ + float integrator_input = _param_vt_b_dec_i * deceleration_error; + _decel_error_bt_int += integrator_input * math::constrain(_deltatime, 0.01f, 0.1f); + // Saturate the integrator value + _decel_error_bt_int = math::constrain(_decel_error_bt_int, .0f, _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 6e255dbd1c00..384589630638 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 @@ class FlightTaskTransition : public FlightTask 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.}; ///< Backtransition deceleration error integrator value + + static constexpr float _pitch_limit_bt = .3f; ///< Bactransition pitch limit void updateParameters(); + void updateParametersFromStorage(); + + void updateSubscribers(); + + bool isVtolFrontTransition(); + bool isVtolBackTransition(); + + 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 d8d640790e75..0fb6ed9c9e21 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -239,11 +239,6 @@ void Standard::update_transition_state() } else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) { - if (_v_control_mode->flag_control_climb_rate_enabled) { - // control backtransition deceleration using pitch. - pitch_body = update_and_get_backtransition_pitch_sp(); - } - const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3a1904257993..5f844bb9fbd7 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -293,10 +293,6 @@ void Tiltrotor::update_transition_state() _mc_yaw_weight = 1.0f; - // control backtransition deceleration using pitch. - if (_v_control_mode->flag_control_climb_rate_enabled) { - pitch_body = update_and_get_backtransition_pitch_sp(); - } if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { // blend throttle from FW value to 0 diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 5dd9bddc194d..e7ec2d3e01d3 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -46,7 +46,6 @@ #include #include #include -#include using namespace matrix; @@ -169,77 +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; - const float vel_body_forward = cosf(track) * _local_pos->vx + sinf(track) * _local_pos->vy; - - const float default_dec_sp = _param_vt_b_dec_mss.get(); - // Maximum allowed deceleration setpoint as a function of the nominal deceleration setpoint - const float max_dec_sp = 2.5f * default_dec_sp; - - float dec_sp = default_dec_sp; - - if (_attc->get_pos_sp_triplet()->current.valid && _attc->get_local_pos()->xy_valid) { - // Add position feedback to the deceleration setpoint calculation - - // Compute backtransition end-point in local reference frame body x-direction -> dist_body_forward - MapProjection map_proj{_attc->get_local_pos()->ref_lat, _attc->get_local_pos()->ref_lon}; - - float pos_sp_x, pos_sp_y = 0.f; - map_proj.project(_attc->get_pos_sp_triplet()->current.lat, _attc->get_pos_sp_triplet()->current.lon, pos_sp_x, - pos_sp_y); - - // Compute backtransition end-point w.r.t. vehicle - const float pos_sp_dx = pos_sp_x - _local_pos->x; - const float pos_sp_dy = pos_sp_y - _local_pos->y; - - // Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle - const float vel_proj = _local_pos->vx * pos_sp_dx + _local_pos->vy * pos_sp_dy; - - if (vel_proj > 0.0f) { - const float dist_body_forward = cosf(track) * pos_sp_dx + sinf(track) * pos_sp_dy; - - if (fabsf(dist_body_forward) > FLT_EPSILON) { - // Compute deceleration setpoint - // Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped - dec_sp = vel_body_forward * vel_body_forward / (2.f * dist_body_forward); - - // Check if the deceleration setpoint is finite and within limits - if (!PX4_ISFINITE(dec_sp)) { - dec_sp = default_dec_sp; - - } else { - // Limit the deceleration setpoint - dec_sp = math::constrain(dec_sp, 0.0f, max_dec_sp); - } - } - } - } - - // 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 8f9ae9ef3861..737746a78bcf 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -317,7 +317,6 @@ class VtolType : public ModuleParams bool _quadchute_command_treated{false}; - float update_and_get_backtransition_pitch_sp(); bool isFrontTransitionCompleted(); virtual bool isFrontTransitionCompletedBase();