diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 79177cb576c3e..11f0b82af1c60 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -14366,9 +14366,17 @@ def LuaParamSet(self): self.start_subtest("Unable to set DISARM_DELAY freely") self.context_push() self.context_collect('STATUSTEXT') + self.context_collect('PARAM_ERROR') old_disarm_delay_value = self.get_parameter('DISARM_DELAY') self.send_set_parameter_direct('DISARM_DELAY', 78) self.wait_statustext('param-set: param set denied (DISARM_DELAY)', check_context=True) + self.assert_received_message_field_values('PARAM_ERROR', { + "target_system": 250, + "target_component": 250, + "param_id": 'DISARM_DELAY', + "param_index": -1, + "error": mavutil.mavlink.MAV_PARAM_ERROR_PERMISSION_DENIED, + }, check_context=True, very_verbose=True) self.assert_parameter_value('DISARM_DELAY', old_disarm_delay_value) self.context_pop() @@ -14379,10 +14387,18 @@ def LuaParamSet(self): self.start_subtest("Re-enabling applet via parameter should stop freely setting DISARM_DELAY") self.context_push() self.context_collect('STATUSTEXT') + self.context_collect('PARAM_ERROR') self.set_parameter("PARAM_SET_ENABLE", 1) old_disarm_delay_value = self.get_parameter('DISARM_DELAY') self.send_set_parameter_direct('DISARM_DELAY', 78) self.wait_statustext('param-set: param set denied (DISARM_DELAY)', check_context=True) + self.assert_received_message_field_values('PARAM_ERROR', { + "target_system": 250, + "target_component": 250, + "param_id": 'DISARM_DELAY', + "param_index": -1, + "error": mavutil.mavlink.MAV_PARAM_ERROR_PERMISSION_DENIED, + }, check_context=True, very_verbose=True) self.assert_parameter_value('DISARM_DELAY', old_disarm_delay_value) self.context_pop() diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index b0858b6f9d8e6..8272235327a54 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -202,6 +202,68 @@ def ThrottleFailsafe(self): self.disarm_vehicle() self.progress("Loiter or Hold as throttle failsafe OK") + def PARAM_ERROR(self): + '''test PARAM_ERROR mavlink message''' + self.start_subtest("Non-existent parameter (get)") + self.context_push() + self.context_collect('PARAM_ERROR') + self.install_messageprinter_handlers_context(['PARAM_ERROR']) + self.send_get_parameter_direct("BOB") + self.assert_received_message_field_values('PARAM_ERROR', { + "target_system": 250, + "target_component": 250, + "param_id": 'BOB', + "param_index": -1, + "error": mavutil.mavlink.MAV_PARAM_ERROR_DOES_NOT_EXIST, + }, check_context=True, very_verbose=True) + self.context_pop() + + self.start_subtest("Non-existent parameter (get-by-id)") + self.context_push() + self.context_collect('PARAM_ERROR') + self.install_messageprinter_handlers_context(['PARAM_ERROR']) + non_existent_param_index = 32764 # hopefully, anyway.... + self.mav.mav.param_request_read_send( + self.sysid_thismav(), + 1, + bytes("", 'ascii'), + non_existent_param_index + ) + self.assert_received_message_field_values('PARAM_ERROR', { + "target_system": 250, + "target_component": 250, + "param_id": '', + "param_index": non_existent_param_index, + "error": mavutil.mavlink.MAV_PARAM_ERROR_DOES_NOT_EXIST, + }, check_context=True, very_verbose=True) + self.context_pop() + + self.start_subtest("Non-existent parameter (set)") + self.context_push() + self.context_collect('PARAM_ERROR') + self.send_set_parameter_direct("BOB", 1) + self.assert_received_message_field_values('PARAM_ERROR', { + "target_system": 250, + "target_component": 250, + "param_id": 'BOB', + "param_index": -1, + "error": mavutil.mavlink.MAV_PARAM_ERROR_DOES_NOT_EXIST, + }, check_context=True, very_verbose=True) + self.context_pop() + + self.start_subtest("Try to set a bad parameter value") + self.context_push() + self.context_collect('PARAM_ERROR') + self.send_set_parameter_direct("SPRAY_ENABLE", float("nan")) + self.assert_received_message_field_values('PARAM_ERROR', { + "target_system": 250, + "target_component": 250, + "param_id": 'SPRAY_ENABLE', + "param_index": -1, + "error": mavutil.mavlink.MAV_PARAM_ERROR_VALUE_OUT_OF_RANGE, + }, check_context=True, very_verbose=True) + self.context_pop() + def Sprayer(self): """Test sprayer functionality.""" rc_ch = 5 @@ -7168,6 +7230,7 @@ def tests(self): self.RoverInitialMode, self.DriveMaxRCIN, self.NoArmWithoutMissionItems, + self.PARAM_ERROR, self.CompassPrearms, self.ManyMAVLinkConnections, self.MAV_CMD_DO_SET_REVERSE, diff --git a/libraries/AP_Scripting/applets/param-set.lua b/libraries/AP_Scripting/applets/param-set.lua index 8400497f21ff4..f046776dc4f72 100644 --- a/libraries/AP_Scripting/applets/param-set.lua +++ b/libraries/AP_Scripting/applets/param-set.lua @@ -50,6 +50,71 @@ mavlink:init(5, 1) local PARAM_SET_ID = 23 mavlink:register_rx_msgid(PARAM_SET_ID) +-- support for sending mavlink messages: + +local MAV_PARAM_ERROR = { + DOES_NOT_EXIST = 1, + VALUE_OUT_OF_RANGE = 2, + PERMISSION_DENIED = 3, + COMPONENT_NOT_FOUND = 4, + READ_ONLY = 5 +} + +-- mavlink message definition +local param_error_msgid = 345 +local messages = {} +messages[param_error_msgid] = { -- PARAM_ERROR + { "param_index", "copy_name_token(token, reply.param_name, AP_MAX_NAME_SIZE, true); + } else { + memset(reply.param_name, '\0', sizeof(reply.param_name)); } - vp->copy_name_token(token, reply.param_name, AP_MAX_NAME_SIZE, true); } else { strncpy(reply.param_name, req.param_name, AP_MAX_NAME_SIZE+1); vp = AP_Param::find(req.param_name, &reply.p_type); - if (vp == nullptr) { - return; - } } reply.chan = req.chan; + reply.src_system_id = req.src_system_id; + reply.src_component_id = req.src_component_id; reply.param_name[AP_MAX_NAME_SIZE] = 0; - reply.value = vp->cast_to_float(reply.p_type); + if (vp != nullptr) { + reply.value = vp->cast_to_float(reply.p_type); + reply.param_error = MAV_PARAM_ERROR_NO_ERROR; + } else { + reply.value = NaNf; + reply.param_error = MAV_PARAM_ERROR_DOES_NOT_EXIST; + } reply.param_index = req.param_index; reply.count = AP_Param::count_parameters(); @@ -410,6 +424,47 @@ void GCS_MAVLINK::param_io_timer(void) param_replies.push(reply); } +/* + A variant of send_param_error which sends based off the received mavlink message information + */ +void GCS_MAVLINK::send_param_error(const mavlink_message_t &msg, const mavlink_param_set_t ¶m_set, MAV_PARAM_ERROR error) +{ + if (!HAVE_PAYLOAD_SPACE(chan, PARAM_ERROR)) { + return; + } + char param_id[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN] {}; + strncpy_noterm(param_id, param_set.param_id, ARRAY_SIZE(param_id)); + mavlink_msg_param_error_send( + chan, + msg.sysid, + msg.compid, + param_id, + -1, + error + ); +} + +/* + A variant of send_param_error which is sent based on what the + parameter set thread returns + */ +void GCS_MAVLINK::send_param_error(const GCS_MAVLINK::pending_param_reply &reply, MAV_PARAM_ERROR error) +{ + if (!HAVE_PAYLOAD_SPACE(chan, PARAM_ERROR)) { + return; + } + char param_id[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN] {}; + strncpy_noterm(param_id, reply.param_name, ARRAY_SIZE(param_id)); + mavlink_msg_param_error_send( + chan, + reply.src_system_id, + reply.src_component_id, + param_id, + reply.param_index, + error + ); +} + /* send replies to PARAM_REQUEST_READ */ @@ -423,25 +478,37 @@ uint8_t GCS_MAVLINK::send_parameter_async_replies() return async_replies_sent_count; } + uint16_t required_space; + if (reply.param_error == MAV_PARAM_ERROR_NO_ERROR) { + required_space = PAYLOAD_SIZE(chan, PARAM_VALUE); + } else { + required_space = PAYLOAD_SIZE(chan, PARAM_ERROR); + } + /* we reserve some space for sending parameters if the client ever fails to get a parameter due to lack of space */ uint32_t saved_reserve_param_space_start_ms = reserve_param_space_start_ms; reserve_param_space_start_ms = 0; // bypass packet_overhead_chan reservation checking - if (!HAVE_PAYLOAD_SPACE(reply.chan, PARAM_VALUE)) { + if (txspace() < required_space) { + out_of_space_to_send(); reserve_param_space_start_ms = AP_HAL::millis(); return async_replies_sent_count; } reserve_param_space_start_ms = saved_reserve_param_space_start_ms; - mavlink_msg_param_value_send( - reply.chan, - reply.param_name, - reply.value, - mav_param_type(reply.p_type), - reply.count, - reply.param_index); + if (reply.param_error == MAV_PARAM_ERROR_NO_ERROR) { + mavlink_msg_param_value_send( + reply.chan, + reply.param_name, + reply.value, + mav_param_type(reply.p_type), + reply.count, + reply.param_index); + } else { + send_param_error(reply, reply.param_error); + } _queued_parameter_send_time_ms = AP_HAL::millis(); async_replies_sent_count++; diff --git a/modules/mavlink b/modules/mavlink index 430715591a7a1..4924617b8ddd9 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 430715591a7a14a996f56d4c724f2e0335d74546 +Subproject commit 4924617b8ddd9871506a01e16675a41464fdba49