From 605d4c47b96b1b7a225d87aa2da5f4552de5bd58 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 26 Oct 2022 13:50:23 +0200 Subject: [PATCH] Navigator: initialize _mission_item for all navigation modes in Navigator::Navigator() This fixes the issue where the init happended in the initializer list, at which point the params were not yet initialized and thus resulted in random values for the init values of _mission_item.loiter_radius and .acceptance_radius. Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission_block.cpp | 22 ++++++++++++++-------- src/modules/navigator/mission_block.h | 2 ++ src/modules/navigator/navigator_main.cpp | 7 +++++++ src/modules/navigator/navigator_mode.h | 3 ++- 4 files changed, 25 insertions(+), 9 deletions(-) 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);