From 1fa878ad88d80e084f0b332b76677321376ff0ff Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 11:15:15 +0200 Subject: [PATCH] navigator: add navigation state ID to every mode class --- src/modules/navigator/land.cpp | 4 ++-- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/mission_base.cpp | 6 +++--- src/modules/navigator/mission_base.h | 2 +- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/mission_block.h | 2 +- src/modules/navigator/navigator_mode.cpp | 5 +++-- src/modules/navigator/navigator_mode.h | 7 ++++++- src/modules/navigator/precland.cpp | 2 +- src/modules/navigator/rtl.cpp | 2 +- src/modules/navigator/rtl_base.h | 2 +- src/modules/navigator/rtl_direct.cpp | 4 ++-- src/modules/navigator/takeoff.cpp | 4 ++-- src/modules/navigator/vtol_takeoff.cpp | 4 ++-- 15 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b7..74f54fbb07 100644 --- 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 index 15408baaff..db61030528 100644 --- 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 index 2949c88d62..ea7dae6752 100644 --- 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 index 6a64f46044..8963a0685d 100644 --- 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) { @@ -476,7 +476,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 index 35d4d37030..f819b5c0e5 100644 --- 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 index 1f0c9f0dd0..f1f2022318 100644 --- 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 index 13815b8602..edc1b0e843 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,7 +64,7 @@ public: /** * 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 index b0ced9006c..392ca6abb4 100644 --- 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 index 60abdcd165..ca418e5b2d 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -41,12 +41,14 @@ #pragma once +#include + 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 @@ public: 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 @@ protected: private: bool _active{false}; + uint8_t _navigator_state_id{0}; }; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37c..74c81569cf 100644 --- 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 index 2b6bd55b3f..6fef45f9df 100644 --- 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 index dc698687fc..a4c88e06b0 100644 --- 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 index 62a940f6c0..145b99d0d1 100644 --- 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(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 index c9e59fec1e..d9b22720f3 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -43,7 +43,7 @@ #include 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 index 7b36201856..f9a552ef58 100644 --- 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; }