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

minimum follow target alt of 8 m added, protecting against nan values in pos controller, fixing ci build error

Browse Source
This commit is contained in:
Jimmy Johnson
2016-05-16 15:11:57 -07:00
parent 42e04d4c11
commit 38b4278998
4 changed files with 12 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+8 -2
View File
@@ -1417,10 +1417,16 @@ MulticopterPositionControl::task_main()
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
// guard against any bad velocity values
bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) &&
PX4_ISFINITE(_pos_sp_triplet.current.vy) &&
_pos_sp_triplet.current.velocity_valid;
// do not go slower than the follow target velocity when position tracking is active (set to valid)
if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid &&
velocity_valid &&
_pos_sp_triplet.current.position_valid) {
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
@@ -1444,7 +1450,7 @@ MulticopterPositionControl::task_main()
// track target using velocity only
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid) {
velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;