diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 34 |
1 files changed, 25 insertions, 9 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 44c1075c1..fe1f4893e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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; |