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

mc_pos_control: set previous triplet point to invalid when switching to manual

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent 2ceb703613
commit c84c7cdcb4
1 changed files with 12 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+12 -5
View File
@@ -753,13 +753,19 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) { if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
//set current position setpoint invalid if none of them (lat, lon and alt) is finite /* we need either a valid position setpoint or a valid velocity setpoint */
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) && _pos_sp_triplet.current.valid = false;
!PX4_ISFINITE(_pos_sp_triplet.current.lon) && _pos_sp_triplet.previous.valid = false;
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false; if (PX4_ISFINITE(_pos_sp_triplet.current.lat) && PX4_ISFINITE(_pos_sp_triplet.current.lon)
&& PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = true;
} }
if (PX4_ISFINITE(_pos_sp_triplet.previous.lat) && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
&& PX4_ISFINITE(_pos_sp_triplet.previous.alt)) {
_pos_sp_triplet.previous.valid = true;
}
} }
orb_check(_home_pos_sub, &updated); orb_check(_home_pos_sub, &updated);
@@ -1999,6 +2005,7 @@ MulticopterPositionControl::do_control(float dt)
* controller will not use the old triplets but waits until triplets * controller will not use the old triplets but waits until triplets
* have been updated */ * have been updated */
_pos_sp_triplet.current.valid = false; _pos_sp_triplet.current.valid = false;
_pos_sp_triplet.previous.valid = false;
_hold_offboard_xy = false; _hold_offboard_xy = false;
_hold_offboard_z = false; _hold_offboard_z = false;