add precloiter

asimopunov · Apr 12, 2024 · 3fa4d8a · 3fa4d8a
1 parent 926e787
commit 3fa4d8a
Show file tree

Hide file tree

Showing 15 changed files with 794 additions and 6 deletions.
diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
@@ -1601,6 +1601,11 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_DO_SET_ACTUATOR:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 			break;
+		case MAV_CMD_DO_PRECISION_HOLD:
+
+			mission_item->nav_cmd = NAV_CMD_DO_PRECISION_HOLD;
+			mission_item->params[0] = mavlink_mission_item->param1; // Height above target
+			break;
 
 		default:
 			mission_item->nav_cmd = NAV_CMD_INVALID;
@@ -1691,6 +1696,8 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		case NAV_CMD_SET_CAMERA_ZOOM:
 		case NAV_CMD_SET_CAMERA_FOCUS:
 		case NAV_CMD_DO_VTOL_TRANSITION:
+		:
+		case MAV_CMD_DO_PRECISION_HOLD:
 			break;
 
 		default:

diff --git a/src/modules/muorb/apps/libfc-sensor-api b/src/modules/muorb/apps/libfc-sensor-api
diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt
@@ -49,6 +49,7 @@ set(NAVIGATOR_SOURCES
 	takeoff.cpp
 	land.cpp
 	precland.cpp
+	precloiter.cpp
 	mission_feasibility_checker.cpp
 	geofence.cpp)
 

diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
@@ -295,7 +295,8 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item,
 	    mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 	    mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
 	    mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
-	    mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
+	    mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION &&
+	    mission_item.nav_cmd != NAV_CMD_DO_PRECISION_HOLD) {
 
 		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: item %i: unsupported cmd: %d\t",
 				     (int)(current_index + 1),

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
@@ -223,7 +223,7 @@ void Mission::setActiveMissionItems()
 		handleLanding(new_work_item_type, next_mission_items, num_found_items);
 
 		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
-		if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) {
+		if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND && new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LOITER) {
 			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		}
 

diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp
@@ -187,6 +187,11 @@ MissionBase::on_inactivation()
 		_navigator->get_precland()->on_inactivation();
 	}
 
+	:
+	if (_navigator->get_precloiter()->is_activated()) {
+		_navigator->get_precloiter()->on_inactivation();
+	}
+
 	/* reset so current mission item gets restarted if mission was paused */
 	_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT;
 
@@ -352,6 +357,14 @@ MissionBase::on_active()
 	} else if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
 	}
+
+	:
+	if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_PRECISION_LOITER) {
+		_navigator->get_precloiter()->on_active();
+
+	} else if (_navigator->get_precloiter()->is_activated()) {
+		_navigator->get_precloiter()->on_inactivation();
+	}
 }
 
 void MissionBase::update_mission()

diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h
@@ -89,7 +89,8 @@ class MissionBase : public MissionBlock, public ModuleParams
 		WORK_ITEM_TYPE_ALIGN_HEADING,		/**< align for next waypoint */
 		WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF,
 		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
-		WORK_ITEM_TYPE_PRECISION_LAND
+		WORK_ITEM_TYPE_PRECISION_LAND,
+		WORK_ITEM_TYPE_PRECISION_LOITER
 	} _work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};	/**< current type of work to do (sub mission item) */
 
 	enum class MissionType {

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
@@ -325,7 +325,14 @@ MissionBlock::is_mission_item_reached_or_completed()
 					_time_wp_reached = now;
 				}
 			}
-
+		:
+		} else if (_mission_item.nav_cmd == NAV_CMD_DO_PRECISION_HOLD) {
+			// Check if the precloiter item has finished
+			if (!_navigator->get_precloiter()->is_activated()) {
+				_waypoint_position_reached = true;
+				_waypoint_yaw_reached = true;
+				_time_wp_reached = now;
+			}
 		} else {
 
 			float acceptance_radius = _navigator->get_acceptance_radius();

diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h
@@ -88,6 +88,7 @@ enum NAV_CMD {
 	NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
 	NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
 	NAV_CMD_OBLIQUE_SURVEY = 260,
+	NAV_CMD_DO_PRECISION_HOLD = 303,
 	NAV_CMD_SET_CAMERA_MODE = 530,
 	NAV_CMD_SET_CAMERA_ZOOM = 531,
 	NAV_CMD_SET_CAMERA_FOCUS = 532,

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
@@ -44,6 +44,7 @@
 #include "geofence.h"
 #include "land.h"
 #include "precland.h"
+#include "precloiter.h"
 #include "loiter.h"
 #include "mission.h"
 #include "navigator_mode.h"
@@ -90,7 +91,7 @@ using namespace time_literals;
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 8
+#define NAVIGATOR_MODE_ARRAY_SIZE 8 //TODO fix
 
 class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
@@ -163,6 +164,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 	vehicle_status_s            *get_vstatus() { return &_vstatus; }
 
 	PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
+	PrecLoiter *get_precloiter() { return &_precloiter; } /**< allow others, e.g. Mission, to use the precision loiter block */
 
 	const vehicle_roi_s &get_vroi() { return _vroi; }
 
@@ -350,6 +352,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
 	Land		_land;			/**< class for handling land commands */
 	PrecLand	_precland;			/**< class for handling precision land commands */
+	PrecLoiter	_precloiter;			/**< class for handling precision loiter commands */
 	RTL 		_rtl;				/**< class that handles RTL */
 	AdsbConflict 	_adsb_conflict;			/**< class that handles ADSB conflict avoidance */
 

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
@@ -80,6 +80,7 @@ Navigator::Navigator() :
 #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
 	_land(this),
 	_precland(this),
+	_precloiter(this),
 	_rtl(this)
 {
 	/* Create a list of our possible navigation types */
@@ -92,6 +93,7 @@ Navigator::Navigator() :
 #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
 	_navigation_mode_array[6] = &_vtol_takeoff;
 #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
+	_navigation_mode_array[7] = &_precloiter;
 
 	/* iterate through navigation modes and initialize _mission_item for each */
 	for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+1 −1		.github/workflows/test_and_deploy.yml
+26 −0		.github/workflows/upstream_sync.yml
+1 −1		README.md
+1 −1		examples/c/udp_example.c
+3 −1		message_definitions/v1.0/all.xml
+246 −0		message_definitions/v1.0/auterion.xml
+39 −89		message_definitions/v1.0/common.xml
+49 −94		message_definitions/v1.0/development.xml
+544 −0		message_definitions/v1.0/ras_a.xml
+47 −1		message_definitions/v1.0/storm32.xml
+1 −1		pymavlink
+6 −21		scripts/update_c_library.sh
+4 −4		scripts/update_generated_repos.sh