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

navigator: reset triplets if navigation mode changes (#8285)

Browse Source
This commit is contained in:
Dennis Mannhart
2017-11-14 11:32:21 -05:00
committed by Daniel Agar
parent ef906d08d3
commit 05c00855c4
1 changed files with 21 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+21 -12
View File
@@ -610,60 +610,62 @@ Navigator::task_main()
} }
/* Do stuff according to navigation state set by commander */ /* Do stuff according to navigation state set by commander */
NavigatorMode *navigation_mode_new{nullptr};
switch (_vstatus.nav_state) { switch (_vstatus.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission; navigation_mode_new = &_mission;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter; navigation_mode_new = &_loiter;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rcLoss; navigation_mode_new = &_rcLoss;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rtl; navigation_mode_new = &_rtl;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff; navigation_mode_new = &_takeoff;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land; navigation_mode_new = &_land;
break; break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND: case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land; navigation_mode_new = &_land;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_dataLinkLoss; navigation_mode_new = &_dataLinkLoss;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure; navigation_mode_new = &_engineFailure;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure; navigation_mode_new = &_gpsFailure;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_follow_target; navigation_mode_new = &_follow_target;
break; break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_MANUAL:
@@ -675,11 +677,18 @@ Navigator::task_main()
case vehicle_status_s::NAVIGATION_STATE_STAB: case vehicle_status_s::NAVIGATION_STATE_STAB:
default: default:
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = nullptr; navigation_mode_new = nullptr;
_can_loiter_at_sp = false; _can_loiter_at_sp = false;
break; break;
} }
/* we have a new navigation mode: reset triplet */
if (_navigation_mode != navigation_mode_new) {
reset_triplets();
}
_navigation_mode = navigation_mode_new;
/* iterate through navigation modes and set active/inactive for each */ /* iterate through navigation modes and set active/inactive for each */
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);