fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:58:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

StickAccelerationXY: fix losing the internal velocity setpoint state

Browse Source 
because of a failing position unlock with NAN velocity setpoint feedback.
This commit is contained in:
Matthias Grob
2021-03-11 00:40:27 +01:00
committed by Lorenz Meier
parent 54fe0ae98c
commit 63a35ccabd
1 changed files with 13 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+13 -8
View File
@@ -149,15 +149,20 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt)
{
if (_velocity_setpoint.norm_squared() < FLT_EPSILON) {
if (!PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint = pos.xy();
}
const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON;
const bool position_locked = PX4_ISFINITE(_position_setpoint(0)) || PX4_ISFINITE(_position_setpoint(1));
} else {
if (PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint.setNaN();
// avoid velocity control jump because of remaining position error when unlocking
// lock position
if (!moving && !position_locked) {
_position_setpoint = pos.xy();
}
// open position loop
if (moving && position_locked) {
_position_setpoint.setNaN();
// avoid velocity setpoint jump caused by ignoring remaining position error
if (PX4_ISFINITE(vel_sp_feedback(0)) && PX4_ISFINITE(vel_sp_feedback(1))) {
_velocity_setpoint = vel_sp_feedback;
}
}