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

navigator: new class structure, loiter and mission working

Browse Source
This commit is contained in:
Julian Oes
2014-06-06 17:17:41 +02:00
parent 9bfae10b73
commit d78c3a2242
13 changed files with 565 additions and 243 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+25 -9
View File
@@ -118,6 +118,7 @@ Navigator::Navigator() :
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
_navigation_mode(nullptr),
_mission(this, "MIS"),
_loiter(this, "LOI"),
_rtl(this, "RTL"),
@@ -321,32 +322,47 @@ Navigator::task_main()
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
_mission.reset();
_loiter.reset();
_rtl.reset();
_navigation_mode = nullptr;
_is_in_loiter = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
_update_triplet = _mission.update(&_pos_sp_triplet);
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
_update_triplet = _loiter.update(&_pos_sp_triplet);
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC:
case NAVIGATION_STATE_AUTO_RTL_DL:
_update_triplet = _rtl.update(&_pos_sp_triplet);
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
default:
_mission.reset();
_loiter.reset();
_rtl.reset();
_navigation_mode = nullptr;
_is_in_loiter = false;
break;
}
/* TODO: make list of modes and loop through it */
if (_navigation_mode == &_mission) {
_update_triplet = _mission.update(&_pos_sp_triplet);
} else {
_mission.reset();
}
if (_navigation_mode == &_rtl) {
_update_triplet = _rtl.update(&_pos_sp_triplet);
} else {
_rtl.reset();
}
if (_navigation_mode == &_loiter) {
_update_triplet = _loiter.update(&_pos_sp_triplet);
} else {
_loiter.reset();
}
if (_update_triplet ) {
publish_position_setpoint_triplet();
_update_triplet = false;