diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index 727d7cd529..8192bfdd3d 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -57,6 +57,9 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) _vel_z_filter.reset(_velocity(2)); } + const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay}; + _accel_filter.reset(acceleration_xy); + if (_sub_vehicle_status.get().in_transition_to_fw) { _gear.landing_gear = landing_gear_s::GEAR_UP; @@ -86,21 +89,27 @@ bool FlightTaskTransition::update() // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_fw_psp_off // and zero roll angle - float pitch_setpoint = math::radians(_param_fw_psp_off); + float tilt_setpoint = math::radians(_param_fw_psp_off); + Vector2f horizontal_acceleration_direction; if (!_sub_vehicle_status.get().in_transition_to_fw) { - pitch_setpoint = computeBackTranstionPitchSetpoint(); + tilt_setpoint = computeBackTransitionTiltSetpoint(); + const Vector2f velocity_xy{_velocity}; + horizontal_acceleration_direction = -velocity_xy.unit_or_zero(); + + } else { + // Forward transition: use heading direction + horizontal_acceleration_direction = Dcm2f(_yaw) * Vector2f(-1.0f, 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; + _acceleration_setpoint.xy() = tanf(tilt_setpoint) * CONSTANTS_ONE_G * horizontal_acceleration_direction; _yaw_setpoint = NAN; + _yawspeed_setpoint = 0.f; return ret; } -float FlightTaskTransition::computeBackTranstionPitchSetpoint() +float FlightTaskTransition::computeBackTransitionTiltSetpoint() { const Vector2f position_xy{_position}; const Vector2f velocity_xy{_velocity}; @@ -109,8 +118,11 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint() float deceleration_setpoint = _param_vt_b_dec_mss; - if (_sub_position_sp_triplet.get().current.valid && _sub_vehicle_local_position.get().xy_global - && position_xy.isAllFinite() && velocity_xy.isAllFinite()) { + const float max_hor_pos_uncertainty_limit = 10.f; + + if (_sub_vehicle_local_position.get().eph < max_hor_pos_uncertainty_limit + && _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)); @@ -129,13 +141,13 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint() deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss); } - // 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 Vector2f acceleration_xy_raw{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay}; + const Vector2f acceleration_xy = _accel_filter.update(acceleration_xy_raw, _deltatime); const float deceleration = -acceleration_xy.dot(velocity_xy_direction); // Zero when velocity invalid 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); + _decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, kDecelerationIntegratorLimit); return _decel_error_bt_int; } diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp index 12687223b5..a226ee9f6b 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -61,8 +61,9 @@ public: bool update() override; private: - static constexpr float VERTICAL_VELOCITY_TIME_CONSTANT = 2.0f; - static constexpr float DECELERATION_INTEGRATOR_LIMIT = .3f; + static constexpr float kVerticalVelocityTimeConstant = 2.0f; + static constexpr float kDecelerationIntegratorLimit = 0.3f; + static constexpr float kAccelerationFilterTimeConstant = 0.05f; uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; uORB::SubscriptionData _sub_position_sp_triplet{ORB_ID(position_setpoint_triplet)}; @@ -71,8 +72,9 @@ private: float _param_vt_b_dec_i{0.f}; float _param_vt_b_dec_mss{0.f}; - AlphaFilter _vel_z_filter{VERTICAL_VELOCITY_TIME_CONSTANT}; + AlphaFilter _vel_z_filter{kVerticalVelocityTimeConstant}; + AlphaFilter _accel_filter{kAccelerationFilterTimeConstant}; float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value - float computeBackTranstionPitchSetpoint(); + float computeBackTransitionTiltSetpoint(); }; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5ab114ba16..6c5bba004e 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -180,7 +180,7 @@ void Standard::update_transition_state() return; } - memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + *_v_att_sp = *_mc_virtual_att_sp; } else { // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) @@ -188,7 +188,7 @@ void Standard::update_transition_state() return; } - memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + *_v_att_sp = *_fw_virtual_att_sp; _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; } @@ -198,11 +198,12 @@ void Standard::update_transition_state() float pitch_body = attitude_setpoint_euler.theta(); float yaw_body = attitude_setpoint_euler.psi(); - if (_v_control_mode->flag_control_climb_rate_enabled) { - roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); - } - if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) { + + if (_v_control_mode->flag_control_climb_rate_enabled) { + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + } + if (_param_vt_psher_slew.get() <= FLT_EPSILON) { // just set the final target throttle value _pusher_throttle = _param_vt_f_trans_thr.get(); @@ -239,12 +240,24 @@ void Standard::update_transition_state() _v_att_sp->thrust_body[0] = _pusher_throttle; const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); + mc_weight = math::constrain(mc_weight, 0.0f, 1.0f); } else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) { + // continually increase mc attitude control as we transition back to mc mode + if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) { + mc_weight = _time_since_trans_start / _param_vt_b_trans_ramp.get(); + } + + mc_weight = math::constrain(mc_weight, 0.0f, 1.0f); + if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta(); + + // blend roll setpoint between FW and MC + const float roll_body_fw = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + roll_body = mc_weight * roll_body + (1.0f - mc_weight) * roll_body_fw; } const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); @@ -252,15 +265,8 @@ void Standard::update_transition_state() q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f; - - // continually increase mc attitude control as we transition back to mc mode - if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) { - mc_weight = _time_since_trans_start / _param_vt_b_trans_ramp.get(); - } } - mc_weight = math::constrain(mc_weight, 0.0f, 1.0f); - _mc_roll_weight = mc_weight; _mc_pitch_weight = mc_weight; _mc_yaw_weight = mc_weight; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index af664bd632..96727f0c53 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -341,7 +341,7 @@ PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f); PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f); /** - * Backtransition deceleration setpoint to pitch I gain. + * Backtransition deceleration setpoint to tilt I gain. * * @unit rad s/m * @min 0