From 4f17fb1303823495ddfe6e504e70e827c43c31d1 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Wed, 10 Jan 2018 08:39:05 +0100
Subject: [PATCH] ManualSmoothingXY: detect alignement based on body frame; do
direction change only if not yawspeed is demanded
---
.../tasks/Utility/ManualSmoothingXY.cpp | 78 ++++++++++++-------
.../tasks/Utility/ManualSmoothingXY.hpp | 17 ++--
2 files changed, 64 insertions(+), 31 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
index efcc4233bf..00417e42a9 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
@@ -41,7 +41,7 @@
#include <float.h>
ManualSmoothingXY::ManualSmoothingXY(const matrix::Vector2f &vel) :
- _vel(vel), _vel_sp_prev(vel)
+ _vel_sp_prev(vel)
{
_acc_hover_h = param_find("MPC_ACC_HOR_MAX");
_acc_xy_max_h = param_find("MPC_ACC_HOR");
@@ -55,11 +55,12 @@ ManualSmoothingXY::ManualSmoothingXY(const matrix::Vector2f &vel) :
}
void
-ManualSmoothingXY::smoothVelFromSticks(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt)
+ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp, const float dt)
{
_updateParams();
- _updateAcceleration(vel_sp, vel, dt);
+ _updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
_velocitySlewRate(vel_sp, dt);
}
@@ -89,7 +90,8 @@ ManualSmoothingXY::_setParams()
}
void
-ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt)
+ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp, const float dt)
{
/*
* In manual mode we consider four states with different acceleration handling:
@@ -98,7 +100,7 @@ ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::V
* 3. user wants to accelerate
* 4. user wants to decelerate
*/
- Intention intention = _getIntention(vel_sp);
+ Intention intention = _getIntention(vel_sp, vel, yaw, yawrate_sp);
/* Adapt acceleration and jerk based on current state and
* intention. Jerk is only used for braking.
@@ -107,44 +109,51 @@ ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::V
/* Smooth velocity setpoint based on acceleration */
_velocitySlewRate(vel_sp, dt);
+
+ _vel_sp_prev = vel_sp;
}
ManualSmoothingXY::Intention
-ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp)
+ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp)
{
+
if (vel_sp.length() > FLT_EPSILON) {
/* Distinguish between acceleration, deceleration and direction change */
- /* Check if stick direction and current velocity are within 120.
- * If current setpoint and velocity are more than 120 apart, we assume
+ /* Check if stick direction and current velocity are within 135.
+ * If current setpoint and velocity are more than 135 apart, we assume
* that the user demanded a direction change.
- * The detecton has to happen in body frame.*/
- matrix::Vector2f vel_sp_unit = vel_sp;;
- matrix::Vector2f vel_sp_prev_unit = _vel_sp_prev;
+ * The detection is done in body frame. */
+ /* Rotate velocity setpoint into body frame */
+ matrix::Vector2f vel_sp_heading = _getInHeadingFrame(vel_sp, yaw);
+ matrix::Vector2f vel_heading = _getInHeadingFrame(vel, yaw);
- if (vel_sp.length() > FLT_EPSILON) {
- vel_sp_unit.normalize();
+ if (vel_sp_heading.length() > FLT_EPSILON) {
+ vel_sp_heading.normalize();
}
- if (_vel_sp_prev.length() > FLT_EPSILON) {
- vel_sp_prev_unit.normalize();
+ if (vel_heading.length() > FLT_EPSILON) {
+ vel_heading.normalize();
}
- const bool is_aligned = (vel_sp_unit * vel_sp_prev_unit) > -0.5f;
+ const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f;
- /* Check if user wants to accelerate */
- bool do_acceleration = _vel_sp_prev.length() < FLT_EPSILON; // Because current is not zero but previous sp was zero
- do_acceleration = do_acceleration || (is_aligned
- && (vel_sp.length() >= _vel_sp_prev.length() - 0.01f)); //User demands larger or same speed
-
- if (do_acceleration) {
- return Intention::acceleration;
+ /* In almost all cases we want to use acceleration.
+ * Only use direction change if not aligned, no yawspeed demand and demand larger than 0.7 of max speed.
+ * Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand. */
+ bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
+ bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand;
+ bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
- } else if (!is_aligned) {
+ if (direction_change) {
return Intention::direction_change;
- } else {
+ } else if (deceleration) {
return Intention::deceleration;
+
+ } else {
+ return Intention::acceleration;
}
}
@@ -237,8 +246,25 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
matrix::Vector2f acc = (vel_sp - _vel_sp_prev) / dt;
if (acc.length() > _acc_state_dependent) {
+
vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev;
}
+}
- _vel_sp_prev = vel_sp;
+matrix::Vector2f
+ManualSmoothingXY::_getInHeadingFrame(const matrix::Vector2f &vec, const float &yaw)
+{
+
+ matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw);
+ matrix::Vector3f vec_heading = q_yaw.conjugate_inversed(matrix::Vector3f(vec(0), vec(1), 0.0f));
+ return matrix::Vector2f(&vec_heading(0));
+}
+
+matrix::Vector2f
+ManualSmoothingXY::_getInWorldFrame(const matrix::Vector2f &vec, const float &yaw)
+{
+
+ matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw);
+ matrix::Vector3f vec_heading = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f));
+ return matrix::Vector2f(&vec_heading(0));
}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
index ec98496688..59a07b2f19 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
@@ -53,7 +53,8 @@ public:
* @param vel_sp: velocity setpoint in xy
* @param dt: time delta in seconds
*/
- void smoothVelFromSticks(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt);
+ void smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp, const float dt);
/* User intention: brake or acceleration */
enum class Intention {
@@ -87,8 +88,10 @@ private:
float _acc_state_dependent{0.0f};
float _jerk_state_dependent{0.0f};
- matrix::Vector2f _vel; // current velocity xy
- matrix::Vector2f _vel_sp_prev; // previous velocity setpoint
+ /* Previous setpoints */
+ float _yaw_prev{};
+ matrix::Vector2f _vel_sp_prev{}; // previous velocity setpoint
+
/* Params */
param_t _acc_hover_h{PARAM_INVALID};
@@ -108,10 +111,14 @@ private:
/* Helper methods */
void _setParams();
void _updateParams();
- void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt);
- Intention _getIntention(const matrix::Vector2f &vel_sp);
+ void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp, const float dt);
+ Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp);
void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
const float dt);
void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
+ matrix::Vector2f _getInHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ;
+ matrix::Vector2f _getInWorldFrame(const matrix::Vector2f &vec, const float &yaw);
};
--
GitLab