diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index b7f4d15f4a4f0dc34d99980b5861911618c9e9c1..1d792046acd005e3bdc063874dd1fa20251a33f9 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -21,7 +21,7 @@ bool FlightTasks::update()
if (isAnyTaskActive()) {
_subscription_array.update();
- return _current_task.task->updateInitialize() && _current_task.task->update();
+ return _current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize();
}
return false;
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index ef58a7b995720a7a796d055a992eab40f3946a5c..9b7e50fc2e6f9c8e6f17bc573fdb0d9a8b248f81 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -78,7 +78,7 @@ bool FlightTaskAuto::activate()
bool ret = FlightTask::activate();
_position_setpoint = _position;
_velocity_setpoint = _velocity;
- _yaw_setpoint = _yaw;
+ _yaw_setpoint = _yaw_sp_prev = _yaw;
_yawspeed_setpoint = 0.0f;
_setDefaultConstraints();
return ret;
@@ -100,6 +100,44 @@ bool FlightTaskAuto::updateInitialize()
return ret;
}
+bool FlightTaskAuto::updateFinalize()
+{
+ // All the auto FlightTasks have to comply with defined maximum yaw rate
+ // If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value
+ // if will see its setpoint constrained here
+ _limitYawRate();
+ return true;
+}
+
+void FlightTaskAuto::_limitYawRate()
+{
+ if (PX4_ISFINITE(_yaw_setpoint) || PX4_ISFINITE(_yaw_sp_prev)) {
+ // Limit the rate of change of the yaw setpoint
+ float dy_max = math::radians(_param_mc_yawrauto_max.get()) * _deltatime;
+
+ if (fabsf(_yaw_setpoint - _yaw_sp_prev) < M_PI_F) {
+ // Wrap around 0
+ _yaw_setpoint = math::constrain(_yaw_setpoint,
+ _yaw_sp_prev - dy_max,
+ _yaw_sp_prev + dy_max);
+
+ } else {
+ // Wrap around PI/-PI
+ _yaw_setpoint = matrix::wrap_pi(
+ math::constrain(matrix::wrap_2pi(_yaw_setpoint),
+ matrix::wrap_2pi(_yaw_sp_prev) - dy_max,
+ matrix::wrap_2pi(_yaw_sp_prev) + dy_max)
+ );
+ }
+
+ _yaw_sp_prev = _yaw_setpoint;
+ }
+
+ if (PX4_ISFINITE(_yawspeed_setpoint)) {
+ _yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -_param_mc_yawrauto_max.get(), _param_mc_yawrauto_max.get());
+ }
+}
+
bool FlightTaskAuto::_evaluateTriplets()
{
// TODO: fix the issues mentioned below
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index 104cdb2a55ee599aa79b1ce09a8bbf5692d606b5..85a7654144ee0039ddde1afa96355c94a1432f35 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -79,6 +79,7 @@ public:
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
bool activate() override;
bool updateInitialize() override;
+ bool updateFinalize() override;
/**
* Sets an external yaw handler which can be used to implement a different yaw control strategy.
@@ -111,12 +112,14 @@ protected:
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
- (ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid // obstacle avoidance active
+ (ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
+ (ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _param_mc_yawrauto_max
);
private:
matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
+ float _yaw_sp_prev = NAN;
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr};
@@ -136,6 +139,7 @@ private:
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+ void _limitYawRate(); /**< Limits the rate of change of 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/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index 7e15cd9d32750cded83b9bd9f9c500b956853cf9..38fe00c7015b25a75b74fc118cc0253b28005a39 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -105,6 +105,14 @@ public:
*/
virtual bool update() = 0;
+ /**
+ * Call after update()
+ * to constrain the generated setpoints in order to comply
+ * with the constraints of the current mode
+ * @return true on success, false on error
+ */
+ virtual bool updateFinalize() { return true; };
+
/**
* Get the output data
* @return task output setpoints that get executed by the positon controller
diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index 72ce93ccf87cbc2c78cf920fde3f8636eb957155..c6fd04743e0d4cfdb5edc6f2eff5600eae6928c7 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -147,9 +147,6 @@ private:
*/
matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate);
- /** lower yawspeed limit in auto modes because we expect yaw steps */
- void adapt_auto_yaw_rate_limit();
-
AttitudeControl _attitude_control; /**< class for attitude control calculations */
int _v_att_sub{-1}; /**< vehicle attitude subscription */
@@ -249,7 +246,6 @@ private:
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
- (ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _param_mc_yawrauto_max,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 901a7c9181ff79815fc10e05f8a948ae9da8d8d9..d55b468affcf2097cb33929415fe0593c9e5869d 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -149,7 +149,6 @@ MulticopterAttitudeControl::parameters_updated()
// angular rate limits
using math::radians;
_attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()), radians(_param_mc_yawrate_max.get())));
- adapt_auto_yaw_rate_limit();
// manual rate control acro mode rate limits
_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get()));
@@ -195,16 +194,6 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
- adapt_auto_yaw_rate_limit();
- }
-}
-
-void MulticopterAttitudeControl::adapt_auto_yaw_rate_limit() {
- if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
- !_v_control_mode.flag_control_manual_enabled) {
- _attitude_control.setRateLimitYaw(math::radians(_param_mc_yawrauto_max.get()));
- } else {
- _attitude_control.setRateLimitYaw(math::radians(_param_mc_yawrate_max.get()));
}
}