Add handling for PARAM_ERROR error message

Tools/autotest/arducopter.py

@@ -6760,7 +6760,7 @@
 
             # double-notch should do better, but check for within 5%
             if peakdb2 * 1.05 > peakdb1:
                 raise NotAchievedException(
                     "Double-notch peak was higher than single-notch peak %fdB > %fdB" %
                     (peakdb2, peakdb1))
 
@@ -6797,7 +6797,7 @@
         self.context_pop()
 
         if ex is not None:
             raise ex
 
     def DynamicRpmNotches(self):
         """Use dynamic harmonic notch to control motor noise via ESC telemetry."""
@@ -14366,9 +14366,17 @@
         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 @@
         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()
 

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,

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", "<h" },
+   { "target_system", "<B" },
+   { "target_component", "<B" },
+   { "param_id", "<c16" },
+   { "error", "<B" },
+}
+
+function encode(msgid, message, messages_array)
+  local message_map = messages_array[msgid]
+  if not message_map then
+    -- we don't know how to encode this message, bail on it
+    error("Unknown MAVLink message " .. msgid)
+  end
+
+  local packString = "<"
+  local packedTable = {}
+  local packedIndex = 1
+  for i,v in ipairs(message_map) do
+    if v[3] then
+      packString = (packString .. string.rep(string.sub(v[2], 2), v[3]))
+      for j = 1, v[3] do
+        packedTable[packedIndex] = message[message_map[i][1]][j]
+        packedIndex = packedIndex + 1
+      end
+    else
+      packString = (packString .. string.sub(v[2], 2))
+      packedTable[packedIndex] = message[message_map[i][1]]
+      packedIndex = packedIndex + 1
+    end
+  end
+
+  return string.pack(packString, table.unpack(packedTable))
+end
+
+-- send PARAM_ERROR message to GCS
+function send_param_error_response(chan, target_system, target_component, param_id, param_error)
+  -- prepare message
+  local msg = {
+      target_system = target_system,
+      target_component = target_component,
+      param_id = param_id,
+      param_index = -1,
+      error = param_error
+  }
+
+  -- send PARAM_ERROR mavlink message
+  local encoded_msg = encode(param_error_msgid, msg, messages)
+  mavlink:send_chan(chan, param_error_msgid, encoded_msg)
+end
+-- end support for sending mavlink messages
+
 -- handle PARAM_SET message
 local parameters_which_can_be_set = {}
 parameters_which_can_be_set["MAV_OPTIONS"] = true
@@ -85,6 +150,8 @@ local function should_set_parameter_id(param_id)
     return parameters_which_can_be_set[param_id]
 end
 
+-- handle an attempt by a GCS to set name to value.  Returns a value
+-- from the MAV_ERROR enumeration, 0 on no error:
 local function handle_param_set(name, value)
     -- we will not receive packets in here for the wrong system ID /
     --   component ID; this is handled by ArduPilot's MAVLink routing
@@ -93,11 +160,13 @@ local function handle_param_set(name, value)
     -- check for this specific ID:
     if not should_set_parameter_id(name) then
         gcs:send_text(MAV_SEVERITY.WARNING, string.format("%s: param set denied (%s)", TEXT_PREFIX_STR, name))
-        return
+        return MAV_PARAM_ERROR.PERMISSION_DENIED
     end
 
     param:set_and_save(name, value)
     gcs:send_text(MAV_SEVERITY.WARNING, string.format("%s: param set applied", TEXT_PREFIX_STR))
+
+    return MAV_PARAM_ERROR.NO_ERROR
 end
 
 -- display welcome message
@@ -136,15 +205,19 @@ local function update()
 
     -- consume all available mavlink messages
     while true do
-        local msg, _ = mavlink:receive_chan()
+        local msg, chan, _ = mavlink:receive_chan()
         if msg == nil then
             break
         end
 
         local param_value, _, _, param_id, _ = string.unpack("<fBBc16B", string.sub(msg, 13, 36))
         param_id = string.gsub(param_id, string.char(0), "")
 
-        handle_param_set(param_id, param_value)
+        param_error = handle_param_set(param_id, param_value)
+        if param_error ~= 0 then
+           sysid, compid = string.unpack("<BBB", msg, 8)
+           send_param_error_response(chan, sysid, compid, param_id, param_error)
+        end
     end
 end
 

libraries/GCS_MAVLink/GCS.h

@@ -957,6 +957,8 @@ class GCS_MAVLINK
         mavlink_channel_t chan;
         int16_t param_index;
         char param_name[AP_MAX_NAME_SIZE+1];
+        uint8_t src_system_id;
+        uint8_t src_component_id;
     };
 
     struct pending_param_reply {
@@ -966,6 +968,9 @@ class GCS_MAVLINK
         int16_t param_index;
         uint16_t count;
         char param_name[AP_MAX_NAME_SIZE+1];
+        uint8_t src_system_id;
+        uint8_t src_component_id;
+        MAV_PARAM_ERROR param_error;
     };
 
     // queue of pending parameter requests and replies
@@ -978,6 +983,9 @@ class GCS_MAVLINK
     // IO timer callback for parameters
     void param_io_timer(void);
 
+    // support for returning explicit error for parameter protocol
+    void send_param_error(const mavlink_message_t &msg, const mavlink_param_set_t &param_set, MAV_PARAM_ERROR error);
+    void send_param_error(const pending_param_reply &msg, MAV_PARAM_ERROR error);
     uint8_t send_parameter_async_replies();
 
     void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const;