fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ManualSmoothingXY: ensure that maximum speed is correctly propagated to the smoothing class

Browse Source
This commit is contained in:
Dennis Mannhart 2018-04-26 14:44:37 +02:00 committed by Lorenz Meier
parent 9df337e243
commit 5ef2a61be5
3 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
View File

@ -52,6 +52,7 @@ void FlightTaskManualPositionSmooth::_updateSetpoints()
/* Smooth velocity setpoint in xy.*/
matrix::Vector2f vel(&_velocity(0));
Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
_smoothingXY.updateMaxVelocity(_constraints.speed_xy);
_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
_velocity_setpoint(0) = vel_sp_xy(0);
_velocity_setpoint(1) = vel_sp_xy(1);

6
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
View File

@ -106,7 +106,7 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
* Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
* 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.get()) && !yawspeed_demand
bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_max) && !yawspeed_demand
&& (vel.length() > 2.0f);
bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
@ -153,7 +153,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
if (_jerk_max.get() > _jerk_min.get()) {
_jerk_state_dependent = math::min((_jerk_max.get() - _jerk_min.get())
/ _vel_manual.get() * vel.length() + _jerk_min.get(), _jerk_max.get());
/ _vel_max * vel.length() + _jerk_min.get(), _jerk_max.get());
}
/* Since user wants to brake smoothly but NOT continuing to fly
@ -195,7 +195,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
case Intention::acceleration: {
/* Limit acceleration linearly based on velocity setpoint.*/
_acc_state_dependent = (_acc_xy_max.get() - _dec_xy_min.get())
/ _vel_manual.get() * vel_sp.length() + _dec_xy_min.get();
/ _vel_max * vel_sp.length() + _dec_xy_min.get();
break;
}

16
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
View File

@ -48,6 +48,15 @@ public:
ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel);
~ManualSmoothingXY() = default;
/**
* Maximum velocity is required to detect user intention.
* Maximum velocity changes depending on task. Consequently,
* in order to deduce user intention from velocity, the maximum
* allowed velocity has to updated.
* @param vel_max corresponds to vehicle constraint
*/
void updateMaxVelocity(const float &vel_max) {_vel_max = vel_max;};
/**
* Smoothing of velocity setpoint horizontally based
* on flight direction.
@ -97,6 +106,12 @@ private:
float _acc_state_dependent{0.0f};
float _jerk_state_dependent{0.0f};
/**
* Maximum velocity.
* It is used to deduce user intention.
*/
float _vel_max{0.0f};
/* Previous setpoints */
matrix::Vector2f _vel_sp_prev{}; // previous velocity setpoint
@ -108,5 +123,4 @@ private:
(ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max, ///< jerk max during brake
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_manual ///< maximum velocity in manual controlled mode
)
};