diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index f23ec7dc02eb..1ae5d733f099 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -70,7 +70,10 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint) _position_smoothing.reset(accel_prev, vel_prev, pos_prev); - _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; + _yaw_setpoint_previous = last_setpoint.yaw; + _heading_smoothing.reset(PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw, + PX4_ISFINITE(last_setpoint.yawspeed) ? last_setpoint.yawspeed : 0.f); + _updateTrajConstraints(); _is_emergency_braking_active = false; _time_last_cruise_speed_override = 0; @@ -190,16 +193,14 @@ bool FlightTaskAuto::update() // no valid heading -> generate heading in this flight task // Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint if (!_generateHeadingAlongTraj()) { - _yaw_setpoint = PX4_ISFINITE(_yaw_sp_prev) ? _yaw_sp_prev : _yaw; + _yaw_setpoint = _yaw_setpoint_previous; } } // update previous type _type_previous = _type; - // If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value - // it will see its setpoint constrained here - _limitYawRate(); + _smoothYaw(); _constraints.want_takeoff = _checkTakeoff(); @@ -257,7 +258,7 @@ void FlightTaskAuto::_prepareLandSetpoints() if (sticks_xy.longerThan(FLT_EPSILON)) { // Ensure no unintended yawing when nudging horizontally during initial heading alignment - _land_heading = _yaw_sp_prev; + _land_heading = _yaw_setpoint_previous; } rcHelpModifyYaw(_land_heading); @@ -300,39 +301,32 @@ void FlightTaskAuto::_prepareLandSetpoints() _gear.landing_gear = landing_gear_s::GEAR_DOWN; } -void FlightTaskAuto::_limitYawRate() +void FlightTaskAuto::_smoothYaw() { const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get()); + _heading_smoothing.setMaxHeadingRate(yawrate_max); + _heading_smoothing.setMaxHeadingAccel(math::radians(_param_mpc_yawrauto_acc.get())); _yaw_sp_aligned = true; - if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) { - // Limit the rate of change of the yaw setpoint - const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev); - const float dyaw_max = yawrate_max * _deltatime; - const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max); - const float yaw_setpoint_sat = matrix::wrap_pi(_yaw_sp_prev + dyaw); + if (PX4_ISFINITE(_yaw_setpoint)) { + const float yaw_sp_unsmoothed = _yaw_setpoint; + _heading_smoothing.update(_yaw_setpoint, _deltatime); + _yaw_setpoint = _heading_smoothing.getSmoothedHeading(); + _yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate(); // The yaw setpoint is aligned when it is within tolerance - _yaw_sp_aligned = fabsf(matrix::wrap_pi(_yaw_setpoint - yaw_setpoint_sat)) < math::radians(_param_mis_yaw_err.get()); - - _yaw_setpoint = yaw_setpoint_sat; + _yaw_sp_aligned = fabsf(matrix::wrap_pi(yaw_sp_unsmoothed - _yaw_setpoint)) < math::radians(_param_mis_yaw_err.get()); - if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) { - // Create a feedforward using the filtered derivative - _yawspeed_filter.setParameters(_deltatime, .2f); - _yawspeed_filter.update(dyaw / _deltatime); - _yawspeed_setpoint = _yawspeed_filter.getState(); - } + } else { + _heading_smoothing.reset(_yaw, 0.f); } - _yaw_sp_prev = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; + _yaw_setpoint_previous = _yaw_setpoint; if (PX4_ISFINITE(_yawspeed_setpoint)) { // The yaw setpoint is aligned when its rate is not saturated _yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max); - - _yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max); } } @@ -725,7 +719,7 @@ void FlightTaskAuto::_ekfResetHandlerVelocityZ(float delta_vz) void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi) { - _yaw_sp_prev += delta_psi; + _yaw_setpoint_previous += delta_psi; } void FlightTaskAuto::_checkEmergencyBraking() diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index c177a74d5f96..00610d910cc2 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -136,8 +137,8 @@ class FlightTaskAuto : public FlightTask State _current_state{State::none}; float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */ - float _yaw_sp_prev{NAN}; - AlphaFilter _yawspeed_filter; + float _yaw_setpoint_previous{NAN}; /**< Used because _yaw_setpoint is overwritten in multiple places */ + HeadingSmoothing _heading_smoothing; bool _yaw_sp_aligned{false}; PositionSmoothing _position_smoothing; @@ -156,6 +157,7 @@ class FlightTaskAuto : public FlightTask (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, + (ParamFloat) _param_mpc_yawrauto_acc, (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight @@ -201,7 +203,7 @@ class FlightTaskAuto : public FlightTask matrix::Vector3f _initial_land_position; - void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */ + void _smoothYaw(); /**< Smoothen the yaw setpoint. */ bool _evaluateTriplets(); /**< Checks and sets triplets. */ bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */ bool _evaluateGlobalReference(); /**< Check is global reference is available. */ diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 3136c9805176..70b5835a6c77 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -179,8 +179,8 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) _orbit_velocity = 1.f; _center = _position; _initial_heading = _yaw; - _slew_rate_yaw.setForcedValue(_yaw); - _slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get())); + _heading_smoothing.reset(PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw, + PX4_ISFINITE(last_setpoint.yawspeed) ? last_setpoint.yawspeed : 0.f); _slew_rate_velocity.setSlewRate(_param_mpc_acc_hor.get()); // need a valid position and velocity @@ -238,7 +238,14 @@ bool FlightTaskOrbit::update() } // Apply yaw smoothing - _yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime); + _heading_smoothing.setMaxHeadingRate(math::radians(_param_mpc_yawrauto_max.get())); + _heading_smoothing.setMaxHeadingAccel(math::radians(_param_mpc_yawrauto_acc.get())); + _heading_smoothing.update(_yaw_setpoint, _deltatime); + _yaw_setpoint = _heading_smoothing.getSmoothedHeading(); + + if (_in_circle_approach) { // don't override feed-forward which is already calculated for circling + _yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate(); + } // publish information to UI sendTelemetry(); @@ -362,7 +369,7 @@ void FlightTaskOrbit::_generate_circle_yaw_setpoints() case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED: // no yaw setpoint _yaw_setpoint = NAN; - _yawspeed_setpoint = NAN; + _yawspeed_setpoint = 0.f; // No yaw setpoint is invalid -> just brake to 0°/s break; case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE: diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 13e53bf83fe9..af4ddc67be07 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -127,7 +128,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel bool _started_clockwise{true}; bool _currently_orbiting{false}; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ - SlewRateYaw _slew_rate_yaw; + HeadingSmoothing _heading_smoothing; SlewRate _slew_rate_velocity; orb_advert_t _mavlink_log_pub{nullptr}; @@ -138,6 +139,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel (ParamInt) _param_mc_orbit_yaw_mod, (ParamFloat) _param_mpc_xy_cruise, /**< cruise speed for circle approach */ (ParamFloat) _param_mpc_yawrauto_max, + (ParamFloat) _param_mpc_yawrauto_acc, (ParamFloat) _param_mpc_xy_traj_p, (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c index e2e453d975ed..4237c02a7778 100644 --- a/src/modules/mc_pos_control/multicopter_autonomous_params.c +++ b/src/modules/mc_pos_control/multicopter_autonomous_params.c @@ -145,7 +145,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f); * @increment 5 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f); +PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 60.f); /** * Maximum yaw acceleration in autonomous modes @@ -160,7 +160,7 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f); * @increment 5 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 60.f); +PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 20.f); /** * Heading behavior in autonomous modes