feat(course): use velocity estimate instead of GPS for course/heading mode

This changes the guard to v_xy_valid (horizontal velocity estimate), which also holds
during dead-reckoning.

- Replace gps_position_valid() with v_xy_valid in set_course() and on_activation()
- Initialise course from velocity vector (atan2(vy, vx)) instead of heading
- Guard lat/lon with xy_global in both course and heading branches
- Fall back to heading mode automatically if velocity estimate is lost
- Introduce ControlMode enum replacing the _use_heading bool
- Rename update_course_setpoint() to update_setpoint_triplet()

src/modules/commander/Commander.cpp

@@ -919,13 +919,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE: {
-			// DO_CHANGE_COURSE is only valid in course mode, navigator handles the actual course update
-			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			// Navigator handles this command: it acks ACCEPTED when
+			// the vehicle is in course mode with a valid position, DENIED otherwise.
 		}
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_CONDITION_YAW: {
-			// CONDITION_YAW in course mode sets the course, navigator handles it
+			// In course mode, CONDITION_YAW sets heading (switches to heading control); navigator handles it.
 			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -570,9 +570,7 @@ FixedWingModeManager::control_auto(const float control_interval, const Vector2d
 	position_setpoint_s current_sp = pos_sp_curr;
 	move_position_setpoint_for_vtol_transition(current_sp);
 
-	// Course and heading setpoints are handled directly before setpoint-type resolution.
-	// Dispatching here avoids handle_setpoint_type() misidentifying these as stationary
-	// position setpoints (dist_xy ≈ 0) and incorrectly downgrading them to LOITER.
+	// Course and heading setpoints are handled directly to avoid entering hold mode
 	if (PX4_ISFINITE(current_sp.course) || PX4_ISFINITE(current_sp.yaw)) {
 		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
 

src/modules/navigator/course.cpp

@@ -39,6 +39,8 @@
 #include "course.h"
 #include "navigator.h"
 
+#include <matrix/math.hpp>
+
 Course::Course(Navigator *navigator) :
 	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE)
 {
@@ -47,52 +49,65 @@ Course::Course(Navigator *navigator) :
 void
 Course::on_activation()
 {
-	// Capture current state
-	_heading = _navigator->get_local_position()->heading;
+	const vehicle_local_position_s *lpos = _navigator->get_local_position();
+
+	// Capture current heading (nose direction)
+	_heading = lpos->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;
+	if (lpos->v_xy_valid) {
+		_course = matrix::wrap_2pi(atan2f(lpos->vy, lpos->vx));
+		_control_mode = ControlMode::Course;
 
 	} else {
-		// No GPS: use heading mode (nose direction only)
-		_use_heading = true;
+		// No valid velocity estimate: fall back to heading mode (nose direction only).
+		_control_mode = ControlMode::Heading;
 	}
 
 	_navigator->reset_cruising_speed();
 
-	update_course_setpoint();
+	update_setpoint_triplet();
 }
 
 void
 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_heading(), set_altitude(), set_airspeed() calls from navigator_main.
+	// If horizontal velocity estimate becomes invalid while in course mode, fall back to heading mode
+	if (_control_mode == ControlMode::Course && !_navigator->get_local_position()->v_xy_valid) {
+		_heading = _navigator->get_local_position()->heading;
+		_control_mode = ControlMode::Heading;
+		update_setpoint_triplet();
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Course hold: velocity lost, holding current heading\t");
+		events::send(events::ID("navigator_course_gps_lost"), {events::Log::Error, events::LogInternal::Info},
+			     "Course hold: velocity estimate lost, switching to heading mode");
+	}
+}
+
+void
+Course::set_heading(float heading_rad)
+{
+	_heading = heading_rad;
+	_control_mode = ControlMode::Heading;
+	update_setpoint_triplet();
 }
 
 bool
 Course::set_course(float course_rad)
 {
-	if (!_navigator->gps_position_valid()) {
-		// No valid GPS - cannot use course mode
+	if (!_navigator->get_local_position()->v_xy_valid) {
+		// No valid velocity estimate - cannot compute or maintain a ground track
 		return false;
 	}
 
 	_course = course_rad;
-	_use_heading = false;
-	update_course_setpoint();
+	_control_mode = ControlMode::Course;
+	update_setpoint_triplet();
 	return true;
 }
 
 void
-Course::update_course_setpoint()
+Course::update_setpoint_triplet()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
@@ -102,16 +117,15 @@ Course::update_course_setpoint()
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 	pos_sp_triplet->current.alt = _altitude;
 
-	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 (_control_mode == ControlMode::Heading) {
+		// Heading mode: control nose direction (yaw).
+		// lat/lon must be finite for the setpoint to be valid;
+		// use current position if available, otherwise dummy values (not used by the controller in heading mode).
 		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;
 		}
@@ -120,9 +134,18 @@ Course::update_course_setpoint()
 		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;
+		// Course mode: control ground track.
+		// lat/lon must be finite for the setpoint to be valid;
+		// use current position if available (including during dead-reckoning), otherwise dummy values.
+		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 {
+			pos_sp_triplet->current.lat = 0.0;
+			pos_sp_triplet->current.lon = 0.0;
+		}
+
 		pos_sp_triplet->current.yaw = NAN;
 		pos_sp_triplet->current.course = _course;
 	}

src/modules/navigator/course.h

@@ -34,11 +34,12 @@
  * @file course.h
  *
  * Course/Heading mode: maintain constant course or heading, altitude, and airspeed.
- * On activation, tracks course if GPS available, otherwise heading.
+ * On activation, tracks course if position is available, otherwise heading.
+ * Falls back to heading hold automatically on position loss.
  *
- * 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).
+ * Accepts MAV_CMD_DO_CHANGE_ALTITUDE, VEHICLE_CMD_DO_REPOSITION, DO_CHANGE_COURSE, CONDITION_YAW and DO_CHANGE_SPEED commands.
+ * - DO_CHANGE_COURSE: sets course (ground track direction, requires valid position).
+ * - CONDITION_YAW: sets heading (airspeed direction), switches to heading control.
  */
 
 #pragma once
@@ -56,30 +57,32 @@ public:
 	void on_active() override;
 
 	/**
-	 * Set course (ground track direction). Requires GPS.
+	 * Set course (ground track direction). Requires valid horizontal velocity.
 	 * @param course_rad Course angle in radians, 0 = north
-	 * @return true if GPS available and course set, false otherwise
+	 * @return true if horizontal velocity available and course set, false otherwise
 	 */
 	bool set_course(float course_rad);
 
 	/**
-	 * Set heading (nose/airspeed direction). GPS-independent.
+	 * Set heading (nose/airspeed direction). position-independent. Switches to heading control.
 	 * @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_heading(float heading_rad);
 
-	void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_course_setpoint(); }
-	void set_airspeed(float airspeed) { _airspeed = airspeed; update_course_setpoint(); }
+	void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_setpoint_triplet(); }
+	void set_airspeed(float airspeed) { _airspeed = airspeed; update_setpoint_triplet(); }
 
 private:
 	/**
 	 * Update the position setpoint triplet to fly the current course or heading.
 	 */
-	void update_course_setpoint();
+	void update_setpoint_triplet();
 
+	enum class ControlMode { Course, Heading };
+
+	ControlMode _control_mode{ControlMode::Course};
 	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_main.cpp

@@ -281,7 +281,7 @@ void Navigator::run()
 				// which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too)
 
 				// In course mode with only an altitude change (no lat/lon target), update the course altitude directly.
-				// The reposition triplet is not used in course mode — the course mode publishes its own setpoint each tick.
+				// The reposition triplet is not used in course mode — the course mode publishes its own setpoint
 				if (_navigation_mode == &_course
 				    && !PX4_ISFINITE(cmd.param5) && !PX4_ISFINITE(cmd.param6)
 				    && PX4_ISFINITE(cmd.param7)) {