feat(modes): make course and heading one mdoe

msg/versioned/VehicleCommand.msg

@@ -126,7 +126,8 @@ 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 mode. |[rad] Course (bearing) [-PI..PI, NaN = no change]|Unused|Unused|Unused|Unused|Unused|Unused|
+
+uint16 VEHICLE_CMD_DO_CHANGE_COURSE = 43007 # Change the course setpoint in Course mode. |[deg] Course (bearing) [0..360, 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.

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -123,19 +123,11 @@ 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 (same requirements as AUTO_LOITER)
+	// NAVIGATION_STATE_AUTO_COURSE
+	// Heading mode is GPS-independent, course mode internally checks for GPS availability.
+	// Only require attitude and altitude, not global position.
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_attitude);
-
-	if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_global_position_relaxed);
-		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_local_position_relaxed);
-
-	} else {
-		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_global_position);
-		setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_local_position);
-	}
-
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_wind_and_flight_time_compliance);
 

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -768,7 +768,7 @@ 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
+	// Course Hold: if a course is explicitly set, navigate along that bearing (ground track)
 	if (PX4_ISFINITE(pos_sp_curr.course)) {
 		const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN;
 
@@ -794,6 +794,28 @@ FixedWingModeManager::control_auto_position(const float control_interval, const
 		return;
 	}
 
+	// Heading Hold: if yaw is explicitly set (no course), control airspeed direction directly (GPS-independent)
+	if (PX4_ISFINITE(pos_sp_curr.yaw)) {
+		const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN;
+
+		fixed_wing_lateral_setpoint_s lateral_ctrl_sp{empty_lateral_control_setpoint};
+		lateral_ctrl_sp.timestamp = hrt_absolute_time();
+		lateral_ctrl_sp.airspeed_direction = pos_sp_curr.yaw;
+		_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/navigator/course.cpp

@@ -33,7 +33,7 @@
 /**
  * @file course.cpp
  *
- * Course mode: maintain constant course, altitude, and airspeed.
+ * Course/Heading mode: maintain constant course or heading, altitude, and airspeed.
  */
 
 #include "course.h"
@@ -48,10 +48,21 @@ void
 Course::on_activation()
 {
 	// Capture current state
-	_course = _navigator->get_local_position()->heading;
+	_heading = _navigator->get_local_position()->heading;
 	_altitude = _navigator->get_global_position()->alt;
 	_airspeed = -1.f; // default airspeed
 
+	// Select best mode based on GPS availability
+	if (_navigator->gps_position_valid()) {
+		// GPS available: use course mode (ground track control)
+		_course = _navigator->get_local_position()->heading;
+		_use_heading = false;
+
+	} else {
+		// No GPS: use heading mode (nose direction only)
+		_use_heading = true;
+	}
+
 	_navigator->reset_cruising_speed();
 
 	update_course_setpoint();
@@ -63,7 +74,21 @@ Course::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.
+	// triggered by set_course(), set_heading(), set_altitude(), set_airspeed() calls from navigator_main.
+}
+
+bool
+Course::set_course(float course_rad)
+{
+	if (!_navigator->gps_position_valid()) {
+		// No valid GPS - cannot use course mode
+		return false;
+	}
+
+	_course = course_rad;
+	_use_heading = false;
+	update_course_setpoint();
+	return true;
 }
 
 void
@@ -75,13 +100,35 @@ Course::update_course_setpoint()
 
 	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;
+
+	if (_use_heading) {
+		// Heading mode: control nose direction (yaw)
+		// Use dummy lat/lon if GPS not available - they're not used for heading control
+		// but need to be finite for the setpoint to be considered valid
+		if (_navigator->get_local_position()->xy_global) {
+			pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+			pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+
+		} else {
+			// No GPS - use dummy position (not used in heading mode anyway)
+			pos_sp_triplet->current.lat = 0.0;
+			pos_sp_triplet->current.lon = 0.0;
+		}
+
+		pos_sp_triplet->current.yaw = _heading;
+		pos_sp_triplet->current.course = NAN;
+
+	} else {
+		// Course mode: control ground track (requires GPS, checked by set_course())
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->current.yaw = NAN;
+		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();
@@ -90,3 +137,5 @@ Course::update_course_setpoint()
 
 	_navigator->set_position_setpoint_triplet_updated();
 }
+
+

src/modules/navigator/course.h

@@ -33,9 +33,12 @@
 /**
  * @file course.h
  *
- * Course mode: maintain constant course, altitude, and airspeed.
- * Accepts DO_CHANGE_COURSE, CONDITION_YAW, DO_CHANGE_ALTITUDE, and DO_CHANGE_SPEED commands.
- * For Condition YAW, only the Angle and relative/absolute fields are supported.
+ * Course/Heading mode: maintain constant course or heading, altitude, and airspeed.
+ * On activation, tracks course if GPS available, otherwise heading.
+ *
+ * Accepts MAV_CMD_DO_CHANGE_ALTITUDE, DO_CHANGE_COURSE, CONDITION_YAW and DO_CHANGE_SPEED commands.
+ * - DO_CHANGE_COURSE: sets course (ground track direction, requires GPS).
+ * - CONDITION_YAW: sets heading (airspeed direction).
  */
 
 #pragma once
@@ -52,18 +55,31 @@ public:
 	void on_activation() override;
 	void on_active() override;
 
-	void set_course(float course_rad) { _course = course_rad; update_course_setpoint(); }
+	/**
+	 * Set course (ground track direction). Requires GPS.
+	 * @param course_rad Course angle in radians, 0 = north
+	 * @return true if GPS available and course set, false otherwise
+	 */
+	bool set_course(float course_rad);
+
+	/**
+	 * Set heading (nose/airspeed direction). GPS-independent.
+	 * @param heading_rad Heading angle in radians, 0 = north
+	 */
+	void set_heading(float heading_rad) { _heading = heading_rad; _use_heading = true; 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.
+	 * Update the position setpoint triplet to fly the current course or heading.
 	 */
 	void update_course_setpoint();
 
-	float _course{0.f};		///< [rad] current course bearing
+	float _course{0.f};		///< [rad] current course bearing (ground track)
+	float _heading{0.f};		///< [rad] current heading setpoint (nose direction)
+	bool _use_heading{false};	///< true: control heading (yaw), false: control course
 	float _altitude{0.f};		///< [m] AMSL altitude setpoint
 	float _airspeed{-1.f};		///< [m/s] cruising speed setpoint (-1 = default)
 };

src/modules/navigator/navigator.h

@@ -189,6 +189,11 @@ public:
 
 	bool home_global_position_valid() { return (_home_pos.valid_alt && _home_pos.valid_hpos); }
 
+	/**
+	 * Check if GPS position is valid (has recent 3D fix)
+	 */
+	bool gps_position_valid() { return (_gps_pos.fix_type >= 3) && (hrt_elapsed_time(&_gps_pos.timestamp) < 2_s); }
+
 	Geofence &get_geofence() { return _geofence; }
 
 	float get_default_loiter_rad() { return fabsf(_param_nav_loiter_rad.get()); }

src/modules/navigator/navigator_main.cpp

@@ -514,29 +514,41 @@ void Navigator::run()
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE
 				   && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
+				uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED};
+
 				if (_navigation_mode == &_course && PX4_ISFINITE(cmd.param1)) {
-					_course.set_course(cmd.param1);
+					// param1: course angle [deg], 0 = north, converted to radians
+					float course_rad = cmd.param1 * M_DEG_TO_RAD_F;
+
+					if (_course.set_course(course_rad)) {
+						result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					}
+
+					// DENIED if no GPS available for course mode
 				}
 
-				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				publish_vehicle_command_ack(cmd, result);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_CONDITION_YAW
 				   && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
+				uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED};
+
 				if (_navigation_mode == &_course && PX4_ISFINITE(cmd.param1)) {
+					// CONDITION_YAW sets heading (airspeed direction), not course
 					// param1: target angle [deg], 0 = north
 					// param4: 0 = absolute, 1 = relative to current heading
-					float course_rad = cmd.param1 * M_DEG_TO_RAD_F;
+					float heading_rad = cmd.param1 * M_DEG_TO_RAD_F;
 
-					if (cmd.param4 > 0.5f) {
-						// Relative: add to current course
-						course_rad = matrix::wrap_2pi(get_local_position()->heading + course_rad);
+					if (static_cast<float>(cmd.param4) > 0.5f) {
+						// Relative: add to current heading
+						heading_rad = matrix::wrap_2pi(get_local_position()->heading + heading_rad);
 					}
 
-					_course.set_course(course_rad);
+					_course.set_heading(heading_rad);
 				}
 
-				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				publish_vehicle_command_ack(cmd, result);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_ORBIT &&
 				   get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {