fix: allow altitude changes in course mode

src/modules/commander/Commander.cpp

@@ -876,11 +876,19 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 
 			} else {
-				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
+				// If already in course mode, stay in course mode (navigator handles any altitude update)
+				// Only switch to loiter if a specific lat/lon target is given, or we are not in course mode.
+				const bool has_position_target = PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6);
+				const bool in_course_mode = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE;
+				const uint8_t target_state = (in_course_mode && !has_position_target)
+							     ? vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE
+							     : vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
+				if (_user_mode_intention.change(target_state, getSourceFromCommand(cmd))) {
 					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;
 				}
 			}

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -570,6 +570,19 @@ 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.
+	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);
+
+		if (!_vehicle_status.in_transition_to_fw) {
+			publishLocalPositionSetpoint(current_sp);
+		}
+
+		return;
+	}
+
 	const uint8_t position_sp_type = handle_setpoint_type(current_sp, pos_sp_next);
 
 	_position_sp_type = position_sp_type;

src/modules/navigator/navigator_main.cpp

@@ -280,6 +280,16 @@ void Navigator::run()
 				// only update the reposition 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)
 
+				// 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.
+				if (_navigation_mode == &_course
+				    && !PX4_ISFINITE(cmd.param5) && !PX4_ISFINITE(cmd.param6)
+				    && PX4_ISFINITE(cmd.param7)) {
+					_course.set_altitude(cmd.param7);
+					// DO_REPOSITION is acknowledged by commander
+
+				} else {
+
 					// Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten
 					_wait_for_vehicle_status_timestamp = hrt_absolute_time();
 
@@ -427,6 +437,8 @@ void Navigator::run()
 
 					// CMD_DO_REPOSITION is acknowledged by commander
 
+				}
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_ALTITUDE
 				   && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 				// only update the setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter,
@@ -1272,6 +1284,7 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.lat = static_cast<double>(NAN);
 	sp.lon = static_cast<double>(NAN);
 	sp.yaw = NAN;
+	sp.course = NAN;
 	sp.loiter_radius = get_default_loiter_rad();
 	sp.acceptance_radius = get_default_acceptance_radius();
 	sp.cruising_speed = get_cruising_speed();