fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink PARAM_ERROR support (#25699)

Browse Source 
Co-authored-by: PX4BuildBot <bot@pixhawk.org>
This commit is contained in:
Hamish Willee 2025-10-17 13:29:23 +11:00 committed by GitHub
parent faee1da630
commit f03131cfd3
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 90 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -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()

8
src/modules/mavlink/mavlink_parameters.h
View File

@ -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