diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c7170ec490..15319a3af4 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -59,14 +59,7 @@ using matrix::wrap_pi; MissionBlock::MissionBlock(Navigator *navigator) : NavigatorMode(navigator) { - _mission_item.lat = (double)NAN; - _mission_item.lon = (double)NAN; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + } bool @@ -932,3 +925,16 @@ MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) return mission_item.altitude; } } + +void +MissionBlock::initialize() +{ + _mission_item.lat = (double)NAN; + _mission_item.lon = (double)NAN; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 544b5abc81..c2c05eb17d 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -71,6 +71,8 @@ public: MissionBlock(const MissionBlock &) = delete; MissionBlock &operator=(const MissionBlock &) = delete; + void initialize() override; + /** * Check if the mission item contains a navigation position * diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 775428a740..8bba5d4131 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -88,6 +88,13 @@ Navigator::Navigator() : _navigation_mode_array[5] = &_precland; _navigation_mode_array[6] = &_vtol_takeoff; + /* iterate through navigation modes and initialize _mission_item for each */ + for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + if (_navigation_mode_array[i]) { + _navigation_mode_array[i]->initialize(); + } + } + _handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS"); _handle_reverse_delay = param_find("VT_B_REV_DEL"); diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index a3a48a920a..3aa6f255f6 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -49,7 +49,8 @@ public: NavigatorMode(Navigator *navigator); virtual ~NavigatorMode() = default; NavigatorMode(const NavigatorMode &) = delete; - NavigatorMode operator=(const NavigatorMode &) = delete; + NavigatorMode &operator=(const NavigatorMode &) = delete; + virtual void initialize() = 0; void run(bool active);