mavlink: return unsupported frame result when that's the case

msg/VehicleCommand.msg

@@ -165,7 +165,10 @@ int8 ARMING_ACTION_ARM = 1
 uint8 GRIPPER_ACTION_RELEASE = 0
 uint8 GRIPPER_ACTION_GRAB = 1
 
-uint8 ORB_QUEUE_LENGTH = 8
+uint8 frame # The coordinate system of the Command
+uint8 FRAME_UNKNOWN = 0 # Not specified
+uint8 FRAME_GLOBAL = 1 # Global WGS84 coordinate frame + altitude relative to mean sea level
+uint8 FRAME_GLOBAL_RELATIVE_ALTITUDE = 2 # Global WGS84 coordinate frame + altitude relative to the home position
 
 float32 param1			# Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
 float32 param2			# Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum.
@@ -184,4 +187,6 @@ bool from_external
 
 uint16 COMPONENT_MODE_EXECUTOR_START = 1000
 
+uint8 ORB_QUEUE_LENGTH = 8
+
 # TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor

msg/VehicleCommandAck.msg

@@ -5,13 +5,16 @@
 uint64 timestamp		# time since system start (microseconds)
 
 # Result cases. This follows the MAVLink MAV_RESULT enum definition
-uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
+uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command is valid (is supported and has valid parameters), and was executed
-uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
+uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command is valid, but cannot be executed at this time
-uint8 VEHICLE_CMD_RESULT_DENIED = 2			# Command PERMANENTLY DENIED |
+uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command is invalid (is supported but has invalid parameters)
-uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
+uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command is not supported (unknown)
-uint8 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
+uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command is valid, but execution has failed
-uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5		# Command being executed |
+uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command is valid and is being executed
-uint8 VEHICLE_CMD_RESULT_CANCELLED = 6			# Command Canceled
+uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command has been cancelled (as a result of receiving a COMMAND_CANCEL message)
+uint8 VEHICLE_CMD_RESULT_COMMAND_LONG_ONLY = 7 # Command is only accepted when sent as a COMMAND_LONG
+uint8 VEHICLE_CMD_RESULT_COMMAND_INT_ONLY = 8 # Command is only accepted when sent as a COMMAND_INT
+uint8 VEHICLE_CMD_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME = 9 # Command is invalid because a frame is required and the specified frame is not supported
 
 # Arming denied specific cases
 uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0

src/modules/mavlink/mavlink_receiver.cpp

@@ -469,6 +469,25 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 	vcmd.param3 = cmd_mavlink.param3;
 	vcmd.param4 = cmd_mavlink.param4;
 
+	switch (cmd_mavlink.frame) {
+	case MAV_FRAME_GLOBAL:
+	case MAV_FRAME_GLOBAL_INT:
+		vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
+		break;
+
+	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+	case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
+		vcmd.frame = vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALTITUDE;
+		break;
+
+	default:
+		vcmd.frame = vehicle_command_s::FRAME_UNKNOWN;
+		PX4_ERR("invalid frame %" PRIu8 " for command %" PRIu16, cmd_mavlink.frame, cmd_mavlink.command);
+		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command,
+			    vehicle_command_ack_s::VEHICLE_CMD_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME);
+		return;
+	}
+
 	if (cmd_mavlink.x == INT32_MAX && cmd_mavlink.y == INT32_MAX) {
 		// INT32_MAX for x and y means to ignore it.
 		vcmd.param5 = (double)NAN;

src/modules/navigator/land.cpp

@@ -108,6 +108,7 @@ Land::on_active()
 		vehicle_command_s vcmd = {};
 
 		vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
+		vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
 		vcmd.param1 = -1;
 		vcmd.param2 = 1;
 		vcmd.param5 = _navigator->get_global_position()->lat;

src/modules/navigator/mission_base.cpp

@@ -855,6 +855,7 @@ MissionBase::do_abort_landing()
 	vehicle_command_s vcmd = {};
 
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
+	vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
 	vcmd.param1 = -1;
 	vcmd.param2 = 1;
 	vcmd.param5 = _mission_item.lat;

src/modules/navigator/navigator_main.cpp

@@ -267,6 +267,7 @@ void Navigator::run()
 
 				vehicle_global_position_s position_setpoint{};
 
+				// latitude/longitude
 				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
 					position_setpoint.lat = cmd.param5;
 					position_setpoint.lon = cmd.param6;
@@ -276,7 +277,22 @@ void Navigator::run()
 					position_setpoint.lon = get_global_position()->lon;
 				}
 
-				position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
+				// altitude
+				if (PX4_ISFINITE(cmd.param7)) {
+					switch (cmd.frame) {
+					default:
+					case vehicle_command_s::FRAME_GLOBAL:
+						position_setpoint.alt = cmd.param7;
+						break;
+
+					case vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALTITUDE:
+						position_setpoint.alt = cmd.param7 + get_home_position()->alt;
+						break;
+					}
+
+				} else {
+					position_setpoint.alt = get_global_position()->alt;
+				}
 
 				if (geofence_allows_position(position_setpoint)) {
 					position_setpoint_triplet_s *rep = get_reposition_triplet();