From 65d22120b2fc9c97b435b3945e3b7545c11f137a 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 03be49bcc5..032b507976 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 @@ -838,3 +831,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 a6e0045a7b..2a4ddd1487 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -67,6 +67,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 1be889b9d8..c17d38e24b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -92,6 +92,13 @@ Navigator::Navigator() : _navigation_mode_array[7] = &_vtol_takeoff; _navigation_mode_array[8] = &_follow_target; + /* 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);