navigator: add navigation state ID to every mode class

PX4 · Aug 9, 2024 · 4e7bb6a · 4e7bb6a
1 parent 6cf0bf5
commit 4e7bb6a
Show file tree

Hide file tree

Showing 15 changed files with 29 additions and 23 deletions.
diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp
@@ -42,7 +42,7 @@
 #include "navigator.h"
 
 Land::Land(Navigator *navigator) :
-	MissionBlock(navigator)
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
 {
 }
 
@@ -93,7 +93,7 @@ Land::on_active()
 	if (_navigator->get_land_detected()->landed) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
-		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND);
+		_navigator->mode_completed(getNavigatorStateId());
 		set_idle_item(&_mission_item);
 
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
@@ -43,7 +43,7 @@
 #include "navigator.h"
 
 Loiter::Loiter(Navigator *navigator) :
-	MissionBlock(navigator),
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER),
 	ModuleParams(navigator)
 {
 }

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
@@ -65,7 +65,7 @@ using namespace time_literals;
 static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10;
 
 Mission::Mission(Navigator *navigator) :
-	MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE)
+	MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
 {
 }
 

diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp
@@ -46,8 +46,8 @@
 #include "mission_feasibility_checker.h"
 #include "navigator.h"
 
-MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) :
-	MissionBlock(navigator),
+MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id) :
+	MissionBlock(navigator, navigator_state_id),
 	ModuleParams(navigator),
 	_dataman_cache_size_signed(dataman_cache_size_signed)
 {
@@ -465,7 +465,7 @@ MissionBase::set_mission_items()
 
 	if (set_end_of_mission) {
 		setEndOfMissionItems();
-		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
+		_navigator->mode_completed(getNavigatorStateId());
 	}
 }
 

diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h
@@ -65,7 +65,7 @@ class Navigator;
 class MissionBase : public MissionBlock, public ModuleParams
 {
 public:
-	MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed);
+	MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id);
 	~MissionBase() override = default;
 
 	virtual void on_inactive() override;

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
@@ -55,8 +55,8 @@
 
 using matrix::wrap_pi;
 
-MissionBlock::MissionBlock(Navigator *navigator) :
-	NavigatorMode(navigator)
+MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) :
+	NavigatorMode(navigator, navigator_state_id)
 {
 
 }

diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
@@ -64,7 +64,7 @@ class MissionBlock : public NavigatorMode
 	/**
 	 * Constructor
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, uint8_t navigator_state_id);
 	virtual ~MissionBlock() = default;
 
 	MissionBlock(const MissionBlock &) = delete;

diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
@@ -42,8 +42,9 @@
 #include "navigator_mode.h"
 #include "navigator.h"
 
-NavigatorMode::NavigatorMode(Navigator *navigator) :
-	_navigator(navigator)
+NavigatorMode::NavigatorMode(Navigator *navigator, uint8_t navigator_state_id) :
+	_navigator(navigator),
+	_navigator_state_id(navigator_state_id)
 {
 	/* set initial mission items */
 	on_inactivation();

diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
@@ -41,12 +41,14 @@
 
 #pragma once
 
+#include <cstdint>
+
 class Navigator;
 
 class NavigatorMode
 {
 public:
-	NavigatorMode(Navigator *navigator);
+	NavigatorMode(Navigator *navigator, uint8_t navigator_state_id);
 	virtual ~NavigatorMode() = default;
 	NavigatorMode(const NavigatorMode &) = delete;
 	NavigatorMode &operator=(const NavigatorMode &) = delete;
@@ -56,6 +58,8 @@ class NavigatorMode
 
 	bool isActive() {return _active;};
 
+	uint8_t getNavigatorStateId() const { return _navigator_state_id; }
+
 	/**
 	 * This function is called while the mode is inactive
 	 */
@@ -81,4 +85,5 @@ class NavigatorMode
 
 private:
 	bool _active{false};
+	uint8_t _navigator_state_id{0};
 };
diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp
@@ -62,7 +62,7 @@
 static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
 
 PrecLand::PrecLand(Navigator *navigator) :
-	MissionBlock(navigator),
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND),
 	ModuleParams(navigator)
 {
 	_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
@@ -55,7 +55,7 @@ static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We
 static constexpr float MIN_DIST_THRESHOLD = 2.f;
 
 RTL::RTL(Navigator *navigator) :
-	NavigatorMode(navigator),
+	NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL),
 	ModuleParams(navigator),
 	_rtl_direct(navigator)
 {

diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h
@@ -46,7 +46,7 @@ class RtlBase : public MissionBase
 {
 public:
 	RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed):
-		MissionBase(navigator, dataman_cache_size_signed) {};
+		MissionBase(navigator, dataman_cache_size_signed, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {};
 	virtual ~RtlBase() = default;
 
 	virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0;

diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp
@@ -51,7 +51,7 @@
 using namespace math;
 
 RtlDirect::RtlDirect(Navigator *navigator) :
-	MissionBlock(navigator),
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL),
 	ModuleParams(navigator)
 {
 	_destination.lat = static_cast<double>(NAN);
@@ -318,7 +318,7 @@ void RtlDirect::set_rtl_item()
 
 	case RTLState::IDLE: {
 			set_idle_item(&_mission_item);
-			_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
+			_navigator->mode_completed(getNavigatorStateId());
 			break;
 		}
 

diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp
@@ -43,7 +43,7 @@
 #include <px4_platform_common/events.h>
 
 Takeoff::Takeoff(Navigator *navigator) :
-	MissionBlock(navigator)
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
 {
 }
 
@@ -68,7 +68,7 @@ Takeoff::on_active()
 	} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
-		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF);
+		_navigator->mode_completed(getNavigatorStateId());
 
 		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 

diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp
@@ -43,7 +43,7 @@
 using matrix::wrap_pi;
 
 VtolTakeoff::VtolTakeoff(Navigator *navigator) :
-	MissionBlock(navigator),
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF),
 	ModuleParams(navigator)
 {
 }
@@ -151,7 +151,7 @@ VtolTakeoff::on_active()
 				// the VTOL takeoff is done
 				_navigator->get_mission_result()->finished = true;
 				_navigator->set_mission_result_updated();
-				_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);
+				_navigator->mode_completed(getNavigatorStateId());
 
 				break;
 			}