fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ManualSmoothingXY: detect alignement based on body frame; do direction change only if not yawspeed is demanded

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 40d77d2234
commit 4f17fb1303
2 changed files with 65 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
+53 -27
View File
@@ -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
/* 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;
if (do_acceleration) {
return Intention::acceleration;
} 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));
}
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
+12 -5
View File
@@ -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);
};