MAVLink PARAM_ERROR support (#25699)

Co-authored-by: PX4BuildBot <bot@pixhawk.org>

src/modules/mavlink/mavlink_parameters.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2025 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +38,7 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Beat Kueng <beat@px4.io>
+ * @author Hamish Willee <hamishwillee@gmail.com>
  */
 
 #include <stdio.h>
@@ -116,6 +117,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 						_send_all_index = -1;
 					}
 
+					/* Error report that _HASH_CHECK parameter is read-only */
+					send_error(MAV_PARAM_ERROR_READ_ONLY, name, -1, msg->sysid, msg->compid);
+
 					/* No other action taken, return */
 					return;
 				}
@@ -125,10 +129,17 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				if (param == PARAM_INVALID) {
 					PX4_ERR("unknown param: %s", name);
+					send_error(MAV_PARAM_ERROR_DOES_NOT_EXIST, name, -1, msg->sysid, msg->compid);
+
+				} else if (!((set.param_type == MAV_PARAM_TYPE_INT32) || (set.param_type == MAV_PARAM_TYPE_REAL32))) {
+					PX4_ERR("param type unsupported: %s", name);
+					send_error(MAV_PARAM_ERROR_TYPE_UNSUPPORTED, name, -1, msg->sysid, msg->compid);
 
 				} else if (!((param_type(param) == PARAM_TYPE_INT32 && set.param_type == MAV_PARAM_TYPE_INT32) ||
 					     (param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32))) {
 					PX4_ERR("param types mismatch param: %s", name);
+					send_error(MAV_PARAM_ERROR_TYPE_MISMATCH, name, -1, msg->sysid, msg->compid);
+
 
 				} else {
 					// According to the mavlink spec we should always acknowledge a write operation.
@@ -200,18 +211,32 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 						/* enforce null termination */
 						name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
 						/* attempt to find parameter and send it */
-						send_param(param_find_no_notification(name));
+
+						const int result = send_param(param_find_no_notification(name));
+
+						if (result == 1) {
+							PX4_ERR("Unknown param name: %s", name);
+							send_error(MAV_PARAM_ERROR_DOES_NOT_EXIST, name, -1, msg->sysid, msg->compid);
+
+						} else if (result == 2) {
+							PX4_ERR("Failed loading param from storage: %s", name);
+							send_error(MAV_PARAM_ERROR_READ_FAIL, name, -1, msg->sysid, msg->compid);
+						}
 					}
 
 				} else {
+
 					/* when index is >= 0, send this parameter again */
 					int ret = send_param(param_for_used_index(req_read.param_index));
 
 					if (ret == 1) {
-						PX4_ERR("unknown param ID: %i", req_read.param_index);
+						PX4_ERR("Unknown param index: %i", req_read.param_index);
+						send_error(MAV_PARAM_ERROR_DOES_NOT_EXIST, nullptr, req_read.param_index, msg->sysid, msg->compid);
 
 					} else if (ret == 2) {
-						PX4_ERR("failed loading param from storage ID: %i", req_read.param_index);
+						PX4_ERR("Failed loading param from storage index: %i", req_read.param_index);
+						send_error(MAV_PARAM_ERROR_READ_FAIL, nullptr, req_read.param_index, msg->sysid, msg->compid);
+
 					}
 				}
 			}
@@ -474,6 +499,7 @@ MavlinkParametersManager::send_one()
 int
 MavlinkParametersManager::send_param(param_t param, int component_id)
 {
+
 	if (param == PARAM_INVALID) {
 		return 1;
 	}
@@ -549,7 +575,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 		mavlink_msg_param_value_send_struct(_mavlink.get_channel(), &msg);
 
 	} else {
-		// Re-pack the message with a different component ID
+		// Re-pack the message with a passed component ID
 		mavlink_message_t mavlink_packet;
 		mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink.get_channel(), &mavlink_packet, &msg);
 		_mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet);
@@ -560,6 +586,57 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 	return 0;
 }
 
+
+int MavlinkParametersManager:: send_error(MAV_PARAM_ERROR error, const char *param_id, const int param_index,
+		const int target_sysid, const int target_compid,
+		int component_id)
+{
+	/* no free TX buf to send this param error message */
+	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_ERROR_LEN) {
+		return 1;
+	}
+
+	mavlink_param_error_t msg;
+	msg.target_system = target_sysid;
+	msg.target_component = target_compid;
+	msg.error = error;
+
+	if (param_index > -1) { // param_id is not used
+
+		msg.param_index = param_index;
+
+	} else {
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstringop-truncation"
+#endif
+		/*
+		 * coverity[buffer_size_warning : FALSE]
+		 *
+		 * The MAVLink spec does not require the string to be NUL-terminated if it
+		 * has length 16. In this case the receiving end needs to terminate it
+		 * when copying it.
+		 */
+		strncpy(msg.param_id, param_id, MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN);
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic pop
+#endif/* code */
+	}
+
+	/* default component ID */
+	if (component_id < 0) {
+		mavlink_msg_param_error_send_struct(_mavlink.get_channel(), &msg);
+
+	} else {
+		// Re-pack the message with a different component ID
+		mavlink_message_t mavlink_packet;
+		mavlink_msg_param_error_encode_chan(mavlink_system.sysid, component_id, _mavlink.get_channel(), &mavlink_packet, &msg);
+		_mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet);
+	}
+
+	return 0;
+}
+
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 
 bool MavlinkParametersManager::send_uavcan()

src/modules/mavlink/mavlink_parameters.h

@@ -112,6 +112,14 @@ protected:
 
 	int send_param(param_t param, int component_id = -1);
 
+	/**
+	 * Send error message.
+	 * /// @return true if a error message was sent
+	 */
+	int send_error(MAV_PARAM_ERROR error, const char *param_id = nullptr,
+		       const int param_index = -1, int target_system = -1,
+		       int target_component = -1, int component_id = -1);
+
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 	/**
 	 * Send UAVCAN params