feat: add Course Hold mode

Signed-off-by: Silvan <silvan@auterion.com>

msg/PositionSetpoint.msg

@@ -33,6 +33,7 @@ uint8 	loiter_pattern		# loitern pattern to follow
 float32 acceptance_radius   # horizontal acceptance_radius (meters)
 float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters)
 
+float32 course			# [rad] desired course (bearing) over ground, NaN = unused
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover

msg/versioned/VehicleCommand.msg

@@ -127,6 +127,7 @@ uint8 FUSION_SOURCE_RNGBCN = 8 # Ranging Beacon
 uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehicle commands (> UINT16_MAX).
 uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Unused|Unused|Unused|Unused|Latitude (WGS-84)|Longitude (WGS-84)|[m] Altitude (AMSL from GNSS, positive above ground)|
 uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Unused|Unused|Unused|Unused|Unused|Unused|
+uint32 VEHICLE_CMD_DO_CHANGE_COURSE = 100002 # Change the course setpoint in Course Hold mode. |[rad] Course (bearing) [-PI..PI, NaN = no change]|Unused|Unused|Unused|Unused|Unused|Unused|
 
 uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization.
 uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory.

msg/versioned/VehicleStatus.msg

@@ -36,7 +36,7 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3         # Auto mission mode
 uint8 NAVIGATION_STATE_AUTO_LOITER = 4          # Auto loiter mode
 uint8 NAVIGATION_STATE_AUTO_RTL = 5             # Auto return to launch mode
 uint8 NAVIGATION_STATE_POSITION_SLOW = 6
-uint8 NAVIGATION_STATE_FREE5 = 7
+uint8 NAVIGATION_STATE_AUTO_COURSE_HOLD = 7 # Course hold mode (FW: maintain course/alt/speed)
 uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8      # Altitude with Cruise mode
 uint8 NAVIGATION_STATE_FREE3 = 9
 uint8 NAVIGATION_STATE_ACRO = 10                # Acro mode

src/lib/events/enums.json

@@ -592,6 +592,10 @@
                             "name": "altitude_cruise",
                             "description": "Altitude Cruise"
                         },
+                        "25": {
+                            "name": "auto_course_hold",
+                            "description": "Course Hold"
+                        },
                         "255": {
                             "name": "unknown",
                             "description": "[Unknown]"

src/lib/modes/ui.hpp

@@ -51,6 +51,7 @@ static inline uint32_t getValidNavStates()
 	       (1u << vehicle_status_s::NAVIGATION_STATE_POSCTL) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) |
@@ -75,7 +76,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"Hold",
 	"Return",
 	"Position Slow",
-	"7: unallocated",
+	"Course Hold",
 	"Altitude Cruise",
 	"9: unallocated",
 	"Acro",

src/modules/commander/Commander.cpp

@@ -489,6 +489,10 @@ int Commander::custom_command(int argc, char *argv[])
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_LOITER);
 
+			} else if (!strcmp(argv[1], "auto:course_hold")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
+						     PX4_CUSTOM_SUB_MODE_AUTO_COURSE_HOLD);
+
 			} else if (!strcmp(argv[1], "auto:rtl")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_RTL);
@@ -890,17 +894,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			// to not require navigator and command to receive / process
 			// the data at the exact same time.
 
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+			// If already in course hold, stay in course hold (navigator handles the altitude update)
+			uint8_t target_state = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD)
+					       ? vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD
+					       : vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
+			if (_user_mode_intention.change(target_state)) {
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+				printRejectMode(target_state);
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE: {
+			// DO_CHANGE_COURSE is only valid in course hold mode, navigator handles the actual course update
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
 			uint8_t base_mode = (uint8_t)cmd.param1;
 			uint8_t custom_main_mode = (uint8_t)cmd.param2;
@@ -964,6 +979,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 							break;
 
+						case PX4_CUSTOM_SUB_MODE_AUTO_COURSE_HOLD:
+							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD;
+							break;
+
 						case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
 							break;

src/modules/commander/ModeUtil/control_mode.cpp

@@ -89,6 +89,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
 		vehicle_control_mode.flag_control_auto_enabled = true;

src/modules/commander/ModeUtil/conversions.hpp

@@ -58,6 +58,8 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter;
 
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD: return navigation_mode_t::auto_course_hold;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl;
 
 	case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW: return navigation_mode_t::position_slow;

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -123,6 +123,22 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance);
 
+	// NAVIGATION_STATE_AUTO_COURSE_HOLD (same requirements as AUTO_LOITER)
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_attitude);
+
+	if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_global_position_relaxed);
+		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_local_position_relaxed);
+
+	} else {
+		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_global_position);
+		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_local_position);
+	}
+
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_wind_and_flight_time_compliance);
+
 	// NAVIGATION_STATE_AUTO_RTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude);

src/modules/commander/px4_custom_mode.h

@@ -66,6 +66,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
 	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
 	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_COURSE_HOLD,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL1,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL2,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL3,
@@ -192,6 +193,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_COURSE_HOLD;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL1;

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -768,6 +768,32 @@ void
 FixedWingModeManager::control_auto_position(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
+	// Course Hold: if a course is explicitly set, navigate along that bearing
+	if (PX4_ISFINITE(pos_sp_curr.course)) {
+		const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN;
+
+		const Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+		const DirectionalGuidanceOutput sp = navigateBearing(curr_pos_local, pos_sp_curr.course, ground_speed, _wind_vel);
+
+		fixed_wing_lateral_setpoint_s lateral_ctrl_sp{empty_lateral_control_setpoint};
+		lateral_ctrl_sp.timestamp = hrt_absolute_time();
+		lateral_ctrl_sp.course = sp.course_setpoint;
+		lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward;
+		_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
+
+		const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
+			.timestamp = hrt_absolute_time(),
+			.altitude = pos_sp_curr.alt,
+			.height_rate = NAN,
+			.equivalent_airspeed = target_airspeed,
+			.pitch_direct = NAN,
+			.throttle_direct = NAN
+		};
+
+		_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);
+		return;
+	}
+
 	const float acc_rad = _directional_guidance.switchDistance(500.0f);
 	const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN;
 

src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp

@@ -144,6 +144,7 @@ private:
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
+							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
 

src/modules/navigator/CMakeLists.txt

@@ -44,6 +44,7 @@ set(NAVIGATOR_SOURCES
 	mission_base.cpp
 	mission_block.cpp
 	mission.cpp
+	course_hold.cpp
 	loiter.cpp
 	rtl.cpp
 	rtl_direct.cpp

src/modules/navigator/course_hold.cpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file course_hold.cpp
+ *
+ * Course Hold mode: maintain constant course, altitude, and airspeed.
+ */
+
+#include "course_hold.h"
+#include "navigator.h"
+
+CourseHold::CourseHold(Navigator *navigator) :
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD)
+{
+}
+
+void
+CourseHold::on_activation()
+{
+	// Capture current state
+	_course = _navigator->get_local_position()->heading;
+	_altitude = _navigator->get_global_position()->alt;
+	_airspeed = -1.f; // default airspeed
+
+	_navigator->reset_cruising_speed();
+
+	update_course_setpoint();
+}
+
+void
+CourseHold::on_active()
+{
+	// Check for incoming vehicle commands
+	// Commands are dispatched from navigator_main, which sets fields on this object
+	// before calling on_active(). Nothing to poll here - setpoint updates are
+	// triggered by set_course(), set_altitude(), set_airspeed() calls from navigator_main.
+}
+
+void
+CourseHold::update_course_setpoint()
+{
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	pos_sp_triplet->previous.valid = false;
+
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+	pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+	pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+	pos_sp_triplet->current.alt = _altitude;
+	pos_sp_triplet->current.course = _course;
+	pos_sp_triplet->current.cruising_speed = _airspeed;
+	pos_sp_triplet->current.cruising_throttle = NAN;
+	pos_sp_triplet->current.yaw = NAN;
+	pos_sp_triplet->current.loiter_radius = NAN;
+	pos_sp_triplet->current.acceptance_radius = _navigator->get_acceptance_radius();
+	pos_sp_triplet->current.timestamp = hrt_absolute_time();
+
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}

src/modules/navigator/course_hold.h

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file course_hold.h
+ *
+ * Course Hold mode: maintain constant course, altitude, and airspeed.
+ * Accepts DO_CHANGE_COURSE, DO_CHANGE_ALTITUDE, and DO_CHANGE_SPEED commands.
+ */
+
+#pragma once
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class CourseHold : public MissionBlock
+{
+public:
+	CourseHold(Navigator *navigator);
+	~CourseHold() = default;
+
+	void on_activation() override;
+	void on_active() override;
+
+	void set_course(float course_rad) { _course = course_rad; update_course_setpoint(); }
+	void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_course_setpoint(); }
+	void set_airspeed(float airspeed) { _airspeed = airspeed; update_course_setpoint(); }
+
+private:
+	/**
+	 * Update the position setpoint triplet to fly the current course.
+	 * Places a waypoint far ahead along the course bearing from the current position.
+	 */
+	void update_course_setpoint();
+
+	float _course{0.f};		///< [rad] current course bearing
+	float _altitude{0.f};		///< [m] AMSL altitude setpoint
+	float _airspeed{-1.f};		///< [m/s] cruising speed setpoint (-1 = default)
+};

src/modules/navigator/navigator.h

@@ -41,6 +41,7 @@
 
 #pragma once
 
+#include "course_hold.h"
 #include "geofence.h"
 #include "land.h"
 #include "precland.h"
@@ -98,7 +99,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 9
 
 class Navigator : public ModuleBase, public ModuleParams
 {
@@ -176,6 +177,7 @@ public:
 	vehicle_status_s            *get_vstatus() { return &_vstatus; }
 
 	PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
+	CourseHold *get_course_hold() { return &_course_hold; }
 
 	const PositionYawSetpoint &get_last_pos_with_gcs_heartbeat() const { return _last_pos_with_gcs_heartbeat; }
 
@@ -386,6 +388,7 @@ private:
 	Land		_land;			/**< class for handling land commands */
 	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
+	CourseHold	_course_hold;			/**< class that handles course hold */
 #if CONFIG_NAVIGATOR_ADSB
 	AdsbConflict 	_adsb_conflict;			/**< class that handles ADSB conflict avoidance */
 	traffic_buffer_s _traffic_buffer{};

src/modules/navigator/navigator_main.cpp

@@ -80,7 +80,8 @@ Navigator::Navigator() :
 #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
 	_land(this),
 	_precland(this),
-	_rtl(this)
+	_rtl(this),
+	_course_hold(this)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -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] = &_course_hold;
 
 	/* iterate through navigation modes and initialize _mission_item for each */
 	for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
@@ -430,75 +432,93 @@ void Navigator::run()
 				// only update the setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter,
 				// which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too)
 
-				// A VEHICLE_CMD_DO_CHANGE_ALTITUDE has the exact same effect as a VEHICLE_CMD_DO_REPOSITION with only the altitude
-				// field populated, this logic is copied from above.
-
-				// only supports MAV_FRAME_GLOBAL and MAV_FRAMEs with absolute altitude amsl
-
-				vehicle_global_position_s position_setpoint{};
-				position_setpoint.lat = get_global_position()->lat;
-				position_setpoint.lon = get_global_position()->lon;
-				position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
-
-				// Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten
-				_wait_for_vehicle_status_timestamp = hrt_absolute_time();
-
-				if (geofence_allows_position(position_setpoint)) {
-					position_setpoint_triplet_s *rep = get_reposition_triplet();
-					position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
-
-					// store current position as previous position and goal as next
-					rep->previous.yaw = get_local_position()->heading;
-					rep->previous.lat = get_global_position()->lat;
-					rep->previous.lon = get_global_position()->lon;
-					rep->previous.alt = get_global_position()->alt;
-
-					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
-
-					// on entering Loiter mode, reset speed setpoint to default
-					if (_navigation_mode != &_loiter) {
-						rep->current.cruising_speed = -1.f;
-
-					} else {
-						rep->current.cruising_speed = get_cruising_speed();
-					}
-
-					rep->current.cruising_throttle = get_cruising_throttle();
-					rep->current.acceptance_radius = get_acceptance_radius();
-					rep->current.yaw = NAN;
-
-					// Position is not changing, thus we keep the setpoint
-					rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat;
-					rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon;
-
-					// set the altitude corresponding to command
-					rep->current.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
-
-					if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
-						rep->current.loiter_radius = curr->current.loiter_radius;
-
-					} else {
-						rep->current.loiter_radius = get_default_loiter_rad();
-					}
-
-					rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise;
-
-					rep->previous.timestamp = hrt_absolute_time();
-
-					rep->current.valid = true;
-					rep->current.timestamp = hrt_absolute_time();
-
-					rep->next.valid = false;
-
-					_time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases
+				if (_navigation_mode == &_course_hold) {
+					// In course hold mode, update altitude directly
+					float new_alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
+					_course_hold.set_altitude(new_alt);
+					// DO_CHANGE_ALTITUDE is acknowledged by commander
 
 				} else {
-					mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t");
-					events::send(events::ID("navigator_change_altitude_outside_geofence"), {events::Log::Error, events::LogInternal::Info},
-						     "Altitude change is outside geofence");
+
+					// A VEHICLE_CMD_DO_CHANGE_ALTITUDE has the exact same effect as a VEHICLE_CMD_DO_REPOSITION with only the altitude
+					// field populated, this logic is copied from above.
+
+					// only supports MAV_FRAME_GLOBAL and MAV_FRAMEs with absolute altitude amsl
+
+					vehicle_global_position_s position_setpoint{};
+					position_setpoint.lat = get_global_position()->lat;
+					position_setpoint.lon = get_global_position()->lon;
+					position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
+
+					// Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten
+					_wait_for_vehicle_status_timestamp = hrt_absolute_time();
+
+					if (geofence_allows_position(position_setpoint)) {
+						position_setpoint_triplet_s *rep = get_reposition_triplet();
+						position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
+
+						// store current position as previous position and goal as next
+						rep->previous.yaw = get_local_position()->heading;
+						rep->previous.lat = get_global_position()->lat;
+						rep->previous.lon = get_global_position()->lon;
+						rep->previous.alt = get_global_position()->alt;
+
+						rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+
+						// on entering Loiter mode, reset speed setpoint to default
+						if (_navigation_mode != &_loiter) {
+							rep->current.cruising_speed = -1.f;
+
+						} else {
+							rep->current.cruising_speed = get_cruising_speed();
+						}
+
+						rep->current.cruising_throttle = get_cruising_throttle();
+						rep->current.acceptance_radius = get_acceptance_radius();
+						rep->current.yaw = NAN;
+
+						// Position is not changing, thus we keep the setpoint
+						rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat;
+						rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon;
+
+						// set the altitude corresponding to command
+						rep->current.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
+
+						if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
+							rep->current.loiter_radius = curr->current.loiter_radius;
+
+						} else {
+							rep->current.loiter_radius = get_default_loiter_rad();
+						}
+
+						rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise;
+
+						rep->previous.timestamp = hrt_absolute_time();
+
+						rep->current.valid = true;
+						rep->current.timestamp = hrt_absolute_time();
+
+						rep->next.valid = false;
+
+						_time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases
+
+					} else {
+						mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t");
+						events::send(events::ID("navigator_change_altitude_outside_geofence"), {events::Log::Error, events::LogInternal::Info},
+							     "Altitude change is outside geofence");
+					}
+
+					// DO_CHANGE_ALTITUDE is acknowledged by commander
+				} // else (not course hold)
+
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE
+				   && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+
+				if (_navigation_mode == &_course_hold && PX4_ISFINITE(cmd.param1)) {
+					_course_hold.set_course(cmd.param1);
 				}
 
-				// DO_CHANGE_ALTITUDE is acknowledged by commander
+				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_ORBIT &&
 				   get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
@@ -703,9 +723,17 @@ void Navigator::run()
 					// XXX not differentiating ground and airspeed yet
 					set_cruising_speed(cmd.param2);
 
+					if (_navigation_mode == &_course_hold) {
+						_course_hold.set_airspeed(cmd.param2);
+					}
+
 				} else {
 					reset_cruising_speed();
 
+					if (_navigation_mode == &_course_hold) {
+						_course_hold.set_airspeed(-1.f);
+					}
+
 					/* if no speed target was given try to set throttle */
 					if (cmd.param3 > FLT_EPSILON) {
 						set_cruising_throttle(cmd.param3 / 100);
@@ -807,6 +835,11 @@ void Navigator::run()
 			navigation_mode_new = &_loiter;
 			break;
 
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD:
+			_pos_sp_triplet_published_invalid_once = false;
+			navigation_mode_new = &_course_hold;
+			break;
+
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 
 			// If we are already in mission landing, do not switch.