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

FlightTaskManualPosition: fix position lock

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 46c7b1c26c
commit e6f04ad34a
2 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+8 -5
View File
@@ -53,7 +53,7 @@ FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent,
bool FlightTaskManualPosition::activate() bool FlightTaskManualPosition::activate()
{ {
_pos_sp_xy = matrix::Vector2f(_position(0), _position(1)); _pos_sp_xy = _pos_sp_xy_lock = matrix::Vector2f(_position(0), _position(1));
_vel_sp_xy = matrix::Vector2f(NAN, NAN); _vel_sp_xy = matrix::Vector2f(NAN, NAN);
_lock_time = 0.0f; _lock_time = 0.0f;
_lock_time_max = 1.0f; // 1s time to brake as default _lock_time_max = 1.0f; // 1s time to brake as default
@@ -88,17 +88,17 @@ void FlightTaskManualPosition::updateXYsetpoints()
if (stick_xy.length() < _stick_deadzone.get()) { if (stick_xy.length() < _stick_deadzone.get()) {
/* Want to hold position */ /* Want to hold position */
if (_lock_time <= _lock_time_max) { if (_lock_time <= _lock_time_max) {
/* Don't lock: time has not been reached */ /* Don't lock: time has not been reached */
_vel_sp_xy.zero(); _vel_sp_xy.zero();
_pos_sp_xy = matrix::Vector2f(NAN, NAN); _pos_sp_xy = matrix::Vector2f(NAN, NAN);
_pos_sp_xy_lock = matrix::Vector2f(&(_position(0)));
_lock_time += _deltatime; _lock_time += _deltatime;
} else { } else {
/* want to hold position */ /* want to hold position */
_vel_sp_xy = matrix::Vector2f(NAN, NAN); _vel_sp_xy = matrix::Vector2f(NAN, NAN);
_pos_sp_xy = matrix::Vector2f(&(_position(0))); _pos_sp_xy = matrix::Vector2f(&(_pos_sp_xy_lock(0)));
} }
} else { } else {
@@ -109,10 +109,13 @@ void FlightTaskManualPosition::updateXYsetpoints()
* maximum acceleration. * maximum acceleration.
*/ */
if (PX4_ISFINITE(_acc_xy_max.get())) { if (PX4_ISFINITE(_acc_xy_max.get())) {
_lock_time_max = matrix::Vector2f(&(_velocity(0))).length() / _acc_xy_max.get(); /* We take half max acceleration because it is better to lock
* position setpoint late than early to prevent backward and forward movement.
*/
_lock_time_max = matrix::Vector2f(&(_velocity(0))).length() / (0.5f * _acc_xy_max.get());
} else { } else {
_lock_time_max = 1.0f; // 1 second time to brake if no acceleration is set _lock_time_max = 2.0f; // 2 second time to brake if no acceleration is set
} }
_lock_time = 0.0f; _lock_time = 0.0f;
src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
+1
View File
@@ -56,6 +56,7 @@ public:
protected: protected:
matrix::Vector2f _vel_sp_xy{}; /* Scaled velocity setpoint from stick. NAN during position lock. */ matrix::Vector2f _vel_sp_xy{}; /* Scaled velocity setpoint from stick. NAN during position lock. */
matrix::Vector2f _pos_sp_xy{}; /* Position setpoint during lock. Otherwise NAN.*/ matrix::Vector2f _pos_sp_xy{}; /* Position setpoint during lock. Otherwise NAN.*/
matrix::Vector2f _pos_sp_xy_lock{}; /* Position setpoint during lock */
control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */ control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
control::BlockParamFloat _acc_xy_max; control::BlockParamFloat _acc_xy_max;