fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 07:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlighTaskManual lock condition: rename boolean

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 5d06464340
commit 153b012df6
2 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+3 -3
View File
@@ -74,13 +74,13 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
*/ */
/* handle position and altitude hold */ /* handle position and altitude hold */
const bool stick_z_zero = fabsf(_sticks_expo(2)) <= FLT_EPSILON; const bool apply_brake_z = fabsf(_vel_sp_z) <= FLT_EPSILON;
const bool stopped_z = (_vel_z_dz.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_z_dz.get()); const bool stopped_z = (_vel_z_dz.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_z_dz.get());
if (stick_z_zero && stopped_z && !PX4_ISFINITE(_pos_sp_z)) { if (apply_brake_z && stopped_z && !PX4_ISFINITE(_pos_sp_z)) {
_pos_sp_z = _position(2); _pos_sp_z = _position(2);
} else if (!stick_z_zero) { } else if (!apply_brake_z) {
_pos_sp_z = NAN; _pos_sp_z = NAN;
} }
} }
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+3 -3
View File
@@ -84,13 +84,13 @@ void FlightTaskManualPosition::_updateXYlock()
{ {
/* If position lock is not active, position setpoint is set to NAN.*/ /* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(&_velocity(0)).length(); const float vel_xy_norm = Vector2f(&_velocity(0)).length();
const bool stick_zero = matrix::Vector2f(_sticks_expo(0), _sticks_expo(1)).length() < FLT_EPSILON; const bool apply_brake = _vel_sp_xy.length() < FLT_EPSILON;
const bool stopped = (_vel_xy_dz.get() < FLT_EPSILON || vel_xy_norm < _vel_xy_dz.get()); const bool stopped = (_vel_xy_dz.get() < FLT_EPSILON || vel_xy_norm < _vel_xy_dz.get());
if (stick_zero && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) { if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) {
_pos_sp_xy = matrix::Vector2f(&_position(0)); _pos_sp_xy = matrix::Vector2f(&_position(0));
} else if (!stick_zero) { } else if (!apply_brake) {
_pos_sp_xy = _pos_sp_xy * NAN; _pos_sp_xy = _pos_sp_xy * NAN;
} }
} }