diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index badf32fe7f..3dd2c6b6a2 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -41,21 +41,15 @@ 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"); - - updateParametersFromStorage(); + param_get(param_find("FW_PSP_OFF"), &_param_fw_psp_off); + param_get(param_find("VT_B_DEC_I"), &_param_vt_b_dec_i); + param_get(param_find("VT_B_DEC_MSS"), &_param_vt_b_dec_mss); } bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(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]); @@ -63,9 +57,7 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) _vel_z_filter.reset(_velocity(2)); } - _velocity_setpoint(2) = _vel_z_filter.getState(); - - if (isVtolFrontTransition()) { + if (_sub_vehicle_status.get().in_transition_to_fw) { _gear.landing_gear = landing_gear_s::GEAR_UP; } else { @@ -77,9 +69,10 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) bool FlightTaskTransition::updateInitialize() { - updateParameters(); - updateSubscribers(); - return FlightTask::updateInitialize(); + bool ret = FlightTask::updateInitialize(); + _sub_vehicle_status.update(); + _sub_position_sp_triplet.update(); + return ret; } bool FlightTaskTransition::update() @@ -88,122 +81,56 @@ bool FlightTaskTransition::update() // tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint bool ret = FlightTask::update(); - _position_setpoint.setAll(NAN); + // slowly move vertical velocity setpoint to zero + _velocity_setpoint(2) = _vel_z_filter.update(0.0f, _deltatime); - // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees + // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_fw_psp_off // and zero roll angle - const Vector2f tmp = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f); - float pitch_setpoint = math::radians(_param_pitch_cruise_degrees); + float pitch_setpoint = math::radians(_param_fw_psp_off); - if (isVtolBackTransition()) { + if (!_sub_vehicle_status.get().in_transition_to_fw) { 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); - _velocity_setpoint(2) = _vel_z_filter.update(0.0f); + // Calculate horizontal acceleration components to follow a pitch setpoint with the current vehicle heading + const Vector2f horizontal_acceleration_direction = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f); + _acceleration_setpoint.xy() = tanf(pitch_setpoint) * CONSTANTS_ONE_G * horizontal_acceleration_direction; _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; + const Vector2f position_xy{_position}; + const Vector2f velocity_xy{_velocity}; + const Vector2f velocity_xy_direction = velocity_xy.unit_or_zero(); - // Retrieve default decelaration setpoint from param - const float default_deceleration_sp = _param_vt_b_dec_mss; + float deceleration_setpoint = _param_vt_b_dec_mss; - float deceleration_sp = default_deceleration_sp; + if (_sub_position_sp_triplet.get().current.valid && _sub_vehicle_local_position.get().xy_global + && position_xy.isAllFinite() && velocity_xy.isAllFinite()) { + Vector2f position_setpoint_local; + _geo_projection.project(_sub_position_sp_triplet.get().current.lat, _sub_position_sp_triplet.get().current.lon, + position_setpoint_local(0), position_setpoint_local(1)); - 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 Vector2f pos_to_target = position_setpoint_local - position_xy; // backtransition end-point w.r.t. vehicle + const float dist_to_target_in_moving_direction = pos_to_target.dot(velocity_xy_direction); - 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); + if (dist_to_target_in_moving_direction > FLT_EPSILON) { + // Backtransition target point is ahead of the vehicle, compute the desired deceleration + deceleration_setpoint = velocity_xy.norm_squared() / (2.f * dist_to_target_in_moving_direction); + deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss); } } - // 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); + // Pitch up to reach a negative accel_in_flight_direction otherwise we decelerate too slow + const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay}; + const float deceleration = -acceleration_xy.dot(velocity_xy_direction); + const float deceleration_error = deceleration_setpoint - deceleration; + // Update back-transition deceleration error integrator + _decel_error_bt_int += (_param_vt_b_dec_i * deceleration_error) * _deltatime; + _decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, DECELERATION_INTEGRATOR_LIMIT); 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 901da114cc..12687223b5 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -61,35 +61,18 @@ public: bool update() override; private: - - static constexpr float _vel_z_filter_time_const = 2.0f; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + static constexpr float VERTICAL_VELOCITY_TIME_CONSTANT = 2.0f; + static constexpr float DECELERATION_INTEGRATOR_LIMIT = .3f; 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_fw_psp_off{0.f}; 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; + AlphaFilter _vel_z_filter{VERTICAL_VELOCITY_TIME_CONSTANT}; 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); - };