From f03131cfd3a5b92dfa8beda6b53d679ea1ce5666 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Fri, 17 Oct 2025 13:29:23 +1100 Subject: [PATCH] MAVLink PARAM_ERROR support (#25699) Co-authored-by: PX4BuildBot --- src/modules/mavlink/mavlink_parameters.cpp | 87 ++++++++++++++++++++-- src/modules/mavlink/mavlink_parameters.h | 8 ++ 2 files changed, 90 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index a391b0e38e..60841a34e6 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/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 * @author Lorenz Meier * @author Beat Kueng + * @author Hamish Willee */ #include @@ -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() diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 478c487881..c43052c020 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/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