Fix handling of higher-numbered channels in RC_CHANNELS_OVERRIDE

Tools/autotest/common.py

@@ -4221,7 +4221,7 @@ def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operato
             if comparator(m_value, value):
                 return m_value
 
-    def wait_rc_channel_value(self, channel, value, timeout=2):
+    def get_rc_channel_value(self, channel, timeout=2):
         """wait for channel to hit value"""
         channel_field = "chan%u_raw" % channel
         tstart = self.get_sim_time()
@@ -4235,12 +4235,22 @@ def wait_rc_channel_value(self, channel, value, timeout=2):
             if m is None:
                 continue
             m_value = getattr(m, channel_field)
-            self.progress("RC_CHANNELS.%s=%u want=%u time_boot_ms=%u" %
-                          (channel_field, m_value, value, m.time_boot_ms))
             if m_value is None:
                 raise ValueError("message (%s) has no field %s" %
                                  (str(m), channel_field))
-            if m_value == value:
+            return m_value
+
+    def wait_rc_channel_value(self, channel, value, timeout=2):
+        channel_field = "chan%u_raw" % channel
+        tstart = self.get_sim_time()
+        while True:
+            remaining = timeout - (self.get_sim_time_cached() - tstart)
+            if remaining <= 0:
+                raise NotAchievedException("Channel never achieved value")
+            m_value = self.get_rc_channel_value(channel, timeout=timeout)
+            self.progress("RC_CHANNELS.%s=%u want=%u" %
+                          (channel_field, m_value, value))
+            if value == m_value:
                 return
 
     def wait_location(self,

Tools/autotest/rover.py

@@ -908,6 +908,73 @@ def test_rc_overrides(self):
                     raise NotAchievedException("Value reverted after %f seconds when it should not have (got=%u) (want=%u)" % (delta, m_value, ch_override_value))
             self.set_parameter("RC_OVERRIDE_TIME", old)
 
+
+            self.delay_sim_time(10)
+
+            self.start_subtest("Checking higher-channel semantics")
+            self.context_push()
+            self.set_parameter("RC_OVERRIDE_TIME", 30)
+
+            ch = 11
+            rc_value = 1010
+            self.set_rc(ch, rc_value)
+
+            channels = [65535] * 18
+            ch_override_value = 1234
+            channels[ch-1] = ch_override_value
+            self.progress("Sending override message ch%u=%u" % (ch, ch_override_value))
+            self.mav.mav.rc_channels_override_send(
+                1, # target system
+                1, # targe component
+                *channels
+            )
+            self.progress("Wait for override value")
+            self.wait_rc_channel_value(ch, ch_override_value, timeout=10)
+
+            self.progress("Sending return-to-RC-input value")
+            channels[ch-1] = 65534
+            self.mav.mav.rc_channels_override_send(
+                1, # target system
+                1, # targe component
+                *channels
+            )
+            self.wait_rc_channel_value(ch, rc_value, timeout=10)
+
+
+            channels[ch-1] = ch_override_value
+            self.progress("Sending override message ch%u=%u" % (ch, ch_override_value))
+            self.mav.mav.rc_channels_override_send(
+                1, # target system
+                1, # targe component
+                *channels
+            )
+            self.progress("Wait for override value")
+            self.wait_rc_channel_value(ch, ch_override_value, timeout=10)
+
+            # make we keep the override vaue for at least 10 seconds:
+            tstart = self.get_sim_time()
+            while True:
+                if self.get_sim_time_cached() - tstart > 10:
+                    break
+                # try both ignore values:
+                ignore_value = 0
+                if self.get_sim_time_cached() - tstart > 5:
+                    ignore_value = 65535
+                self.progress("Sending ignore value %u" % ignore_value)
+                channels[ch-1] = ignore_value
+                self.mav.mav.rc_channels_override_send(
+                    1, # target system
+                    1, # targe component
+                    *channels
+                )
+                if self.get_rc_channel_value(ch) != ch_override_value:
+                    raise NotAchievedException("Did not maintain value")
+
+            self.context_pop()
+
+            self.end_subtest("Checking higher-channel semantics")
+
+
         except Exception as e:
             self.progress("Exception caught: %s" %
                           self.get_exception_stacktrace(e))

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3126,12 +3126,22 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg)
         packet.chan16_raw
     };
 
-    for (uint8_t i=0; i<ARRAY_SIZE(override_data); i++) {
+    for (uint8_t i=0; i<8; i++) {
         // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
         if (override_data[i] != UINT16_MAX) {
             RC_Channels::set_override(i, override_data[i], tnow);
         }
     }
+    for (uint8_t i=8; i<ARRAY_SIZE(override_data); i++) {
+        // Per MAVLink spec a value of zero or UINT16_MAX means to
+        // ignore this field.
+        if (override_data[i] != 0 && override_data[i] != UINT16_MAX) {
+            // per the mavlink spec, a value of UINT16_MAX-1 means
+            // return the field to RC radio values:
+            const uint16_t value = override_data[i] == (UINT16_MAX-1) ? 0 : override_data[i];
+            RC_Channels::set_override(i, value, tnow);
+        }
+    }
 }
 
 // allow override of RC channel values for HIL or for complete GCS