From 581d25f77f005f7250e4c4819550b2fb82f5c70e Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 24 Apr 2019 14:34:00 +0200
Subject: [PATCH] Auto mode traj - limit yaw setpoint rate of change when
generated in the flight task instead of clamping the yaw rate in the
controller
Move yaw setpoint slew rate from AutoLineSmoothVel to Auto. The slew rate is now applied consistently to all the auto FlightTasks
---
src/lib/FlightTasks/FlightTasks.cpp | 2 +-
.../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 40 ++++++++++++++++++-
.../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 6 ++-
.../tasks/FlightTask/FlightTask.hpp | 8 ++++
src/modules/mc_att_control/mc_att_control.hpp | 4 --
.../mc_att_control/mc_att_control_main.cpp | 11 -----
6 files changed, 53 insertions(+), 18 deletions(-)
diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index b7f4d15f4a..1d792046ac 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 ef58a7b995..9b7e50fc2e 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 104cdb2a55..85a7654144 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 7e15cd9d32..38fe00c701 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 72ce93ccf8..c6fd04743e 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 901a7c9181..d55b468aff 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()));
}
}
--
GitLab