feat: make course change possible

src/modules/commander/Commander.cpp

@@ -916,6 +916,12 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_CONDITION_YAW: {
+			// CONDITION_YAW in course mode sets the course, navigator handles it
+			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;

src/modules/navigator/course.h

@@ -34,7 +34,8 @@
  * @file course.h
  *
  * Course mode: maintain constant course, altitude, and airspeed.
- * Accepts DO_CHANGE_COURSE, DO_CHANGE_ALTITUDE, and DO_CHANGE_SPEED commands.
+ * 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.
  */
 
 #pragma once

src/modules/navigator/navigator_main.cpp

@@ -520,6 +520,24 @@ void Navigator::run()
 
 				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_CONDITION_YAW
+				   && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+
+				if (_navigation_mode == &_course && PX4_ISFINITE(cmd.param1)) {
+					// param1: target angle [deg], 0 = north
+					// param4: 0 = absolute, 1 = relative to current heading
+					float course_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);
+					}
+
+					_course.set_course(course_rad);
+				}
+
+				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) {