Navigator: initialize _mission_item for all navigation modes in Navig…

…ator::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 <silvan@auterion.com>

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;
+}

src/modules/navigator/mission_block.h

@@ -67,6 +67,8 @@ class MissionBlock : public NavigatorMode
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
+	void initialize() override;
+
 	/**
 	 * Check if the mission item contains a navigation position
 	 *

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");
 

src/modules/navigator/navigator_mode.h

@@ -49,7 +49,8 @@ class NavigatorMode
 	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);