Update src/modules/mavlink/mavlink_receiver.cpp

src/modules/mavlink/mavlink_receiver.cpp

@@ -618,108 +618,118 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 
 	} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {
 
-		bool has_module = true;
-		autotune_attitude_control_status_s status{};
-		_autotune_attitude_control_status_sub.copy(&status);
+		// Check command validity
+		if ((int)roundf(vehicle_command.param1) != 1 || (int)roundf(vehicle_command.param2) != 0) {
+			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
+			send_ack = true;
 
-		// if not busy enable via the parameter
-		// do not check the return value of the uORB copy above because the module
-		// starts publishing only when MC_AT_START is set
-		if (status.state == autotune_attitude_control_status_s::STATE_IDLE) {
-			vehicle_status_s vehicle_status{};
-			_vehicle_status_sub.copy(&vehicle_status);
+		} else {
 
-			if (!vehicle_status.in_transition_mode) {
-				param_t atune_start;
+			bool has_module = true;
+			autotune_attitude_control_status_s status{};
+			_autotune_attitude_control_status_sub.copy(&status);
 
-				switch (vehicle_status.vehicle_type) {
-				case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
-					atune_start = param_find("FW_AT_START");
+			// if not busy enable via the parameter
+			// do not check the return value of the uORB copy above because the module
+			// starts publishing only when MC_AT_START is set
+			if (status.state == autotune_attitude_control_status_s::STATE_IDLE) {
+				vehicle_status_s vehicle_status{};
+				_vehicle_status_sub.copy(&vehicle_status);
 
-					break;
+				if (!vehicle_status.in_transition_mode) {
+					param_t atune_start;
 
-				case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
-					atune_start = param_find("MC_AT_START");
+					switch (vehicle_status.vehicle_type) {
+					case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
+						atune_start = param_find("FW_AT_START");
 
-					break;
+						break;
 
-				default:
-					atune_start = PARAM_INVALID;
-					break;
-				}
+					case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
+						atune_start = param_find("MC_AT_START");
 
-				if (atune_start == PARAM_INVALID) {
-					has_module = false;
+						break;
+
+					default:
+						atune_start = PARAM_INVALID;
+						break;
+					}
+
+					if (atune_start == PARAM_INVALID) {
+						has_module = false;
+
+					} else {
+						int32_t start = 1;
+						param_set(atune_start, &start);
+					}
 
 				} else {
-					int32_t start = 1;
-					param_set(atune_start, &start);
+					has_module = false;
+				}
+			}
+
+			if (has_module) {
+
+				// most are in progress
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
+
+				switch (status.state) {
+				case autotune_attitude_control_status_s::STATE_IDLE:
+				case autotune_attitude_control_status_s::STATE_INIT:
+					progress = 0;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_ROLL:
+				case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
+					progress = 20;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_PITCH:
+				case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
+					progress = 40;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_YAW:
+				case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
+					progress = 60;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_VERIFICATION:
+					progress = 80;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_APPLY:
+					progress = 85;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_TEST:
+					progress = 90;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
+					progress = 95;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_COMPLETE:
+					progress = 100;
+					// ack it properly with an ACCEPTED once we're done
+					result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_FAIL:
+					progress = 0;
+					result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+					break;
 				}
 
 			} else {
-				has_module = false;
-			}
-		}
-
-		if (has_module) {
-
-			// most are in progress
-			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
-
-			switch (status.state) {
-			case autotune_attitude_control_status_s::STATE_IDLE:
-			case autotune_attitude_control_status_s::STATE_INIT:
-				progress = 0;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_ROLL:
-			case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
-				progress = 20;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_PITCH:
-			case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
-				progress = 40;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_YAW:
-			case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
-				progress = 60;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_VERIFICATION:
-				progress = 80;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_APPLY:
-				progress = 85;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_TEST:
-				progress = 90;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
-				progress = 95;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_COMPLETE:
-				progress = 100;
-				// ack it properly with an ACCEPTED once we're done
-				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_FAIL:
-				progress = 0;
-				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
-				break;
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 			}
 
-		} else {
-			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+			send_ack = true;
+
 		}
 
-		send_ack = true;
 
 	} else {
 		send_ack = false;