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

mc_pos_control: check if triplets are valid, otherwise ignore

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent 5b03c5f68e
commit 44e4beeeec
1 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+11 -7
View File
@@ -754,17 +754,21 @@ MulticopterPositionControl::poll_subscriptions()
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
/* we need either a valid position setpoint or a valid velocity setpoint */
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.previous.valid = false;
if (PX4_ISFINITE(_pos_sp_triplet.current.lat) && PX4_ISFINITE(_pos_sp_triplet.current.lon)
&& PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
&& PX4_ISFINITE(_pos_sp_triplet.current.alt) && _pos_sp_triplet.current.valid) {
_pos_sp_triplet.current.valid = true;
} else {
_pos_sp_triplet.current.valid = false;
}
if (PX4_ISFINITE(_pos_sp_triplet.previous.lat) && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
&& PX4_ISFINITE(_pos_sp_triplet.previous.alt)) {
&& PX4_ISFINITE(_pos_sp_triplet.previous.alt) && _pos_sp_triplet.previous.valid) {
_pos_sp_triplet.previous.valid = true;
} else {
_pos_sp_triplet.previous.valid = false;
}
}
@@ -1466,6 +1470,9 @@ void MulticopterPositionControl::control_auto(float dt)
if (diff.length() > FLT_EPSILON) {
triplet_updated = true;
}
/* we need to update _curr_pos_sp always since navigator applies slew rate on z */
_curr_pos_sp = curr_pos_sp;
}
if (_pos_sp_triplet.previous.valid) {
@@ -1501,7 +1508,6 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
/* Auto logic:
* The vehicle should follow the line previous-current.
* - if there is no next setpoint or the current is a loiter point, then slowly approach the current along the line
@@ -1589,7 +1595,6 @@ void MulticopterPositionControl::control_auto(float dt)
}
/* if we already close to current, then just take over the velocity that
* we would have computed if going directly to the current setpoint
*/
@@ -1603,7 +1608,6 @@ void MulticopterPositionControl::control_auto(float dt)
vel_sp_z = (flying_upward) ? -vel_sp_z : vel_sp_z;
/* compute pos_sp(2) */
pos_sp(2) = _pos(2) + vel_sp_z / _params.pos_p(2);
}
/*