AP_HAL_ChibiOS: add accessor for reversed mask

allow mask updates to be disabled
send dshot commands even if armed - they will be accepted as long as throttle is at zero
only accept low-priority dshot commands while disarmed
apply reversed and reversible mask as servo channels

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -1149,16 +1149,14 @@ void RCOutput::dshot_send_groups(uint32_t time_out_us)
 
     bool command_sent = false;
     // queue up a command if there is one
-    if (!hal.util->get_soft_armed()
-        && _dshot_current_command.cycle == 0
+    if (_dshot_current_command.cycle == 0
         && _dshot_command_queue.pop(_dshot_current_command)) {
         // got a new command
     }
 
     for (auto &group : pwm_group_list) {
         // send a dshot command
-        if (!hal.util->get_soft_armed()
-            && is_dshot_protocol(group.current_mode)
+        if (is_dshot_protocol(group.current_mode)
             && dshot_command_is_active(group)) {
             command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
         // actually do a dshot send
@@ -1383,9 +1381,10 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
                     value = 0;
                 }
             }
+
+            // dshot values are from 48 to 2047. 48 means off.
             if (value != 0) {
-                // dshot values are from 48 to 2047. Zero means off.
-                value += 47;
+                value += DSHOT_ZERO_THROTTLE;
             }
 
             if (!armed) {

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -148,6 +148,7 @@ public:
     /*
       enable telemetry request for a mask of channels. This is used
       with Dshot to get telemetry feedback
+      The mask uses servo channel numbering
      */
     void set_telem_request_mask(uint16_t mask) override { telem_request_mask = (mask >> chan_offset); }
 
@@ -155,6 +156,7 @@ public:
     /*
       enable bi-directional telemetry request for a mask of channels. This is used
       with Dshot to get telemetry feedback
+      The mask uses servo channel numbering
      */
     void set_bidir_dshot_mask(uint16_t mask) override;
 
@@ -193,19 +195,22 @@ public:
 #ifndef DISABLE_DSHOT
     /*
      * mark the channels in chanmask as reversible. This is needed for some ESC types (such as Dshot)
-     * so that output scaling can be performed correctly. The chanmask passed is added (ORed) into
-     * any existing mask.
+     * so that output scaling can be performed correctly. The chanmask passed is added (ORed) into any existing mask.
+     * The mask uses servo channel numbering
      */
     void set_reversible_mask(uint16_t chanmask) override;
 
     /*
-     * mark the channels in chanmask as reversed. The chanmask passed is added (ORed) into
-     * any existing mask.
+     * mark the channels in chanmask as reversed.
+     * The chanmask passed is added (ORed) into any existing mask.
+     * The mask uses servo channel numbering
      */
     void set_reversed_mask(uint16_t chanmask) override;
+    uint16_t get_reversed_mask() override { return _reversed_mask; }
 
     /*
       mark escs as active for the purpose of sending dshot commands
+      The mask uses servo channel numbering
      */
     void set_active_escs_mask(uint16_t chanmask) override { _active_escs_mask |= (chanmask >> chan_offset); }
 
@@ -219,6 +224,12 @@ public:
      * Update channel masks at 1Hz allowing for actions such as dshot commands to be sent
      */
     void update_channel_masks() override;
+
+    /*
+     * Allow channel mask updates to be temporarily suspended
+     */
+    void disable_channel_mask_updates() override { _disable_channel_mask_updates = true; }
+    void enable_channel_mask_updates() override { _disable_channel_mask_updates = false; }
 #endif
 
     /*
@@ -495,6 +506,9 @@ private:
 
     DshotEscType _dshot_esc_type;
 
+    // control updates to channel masks
+    bool _disable_channel_mask_updates;
+
     bool dshot_command_is_active(const pwm_group& group) const {
       return (_dshot_current_command.chan == RCOutput::ALL_CHANNELS || (group.ch_mask & (1UL << _dshot_current_command.chan)))
                 && _dshot_current_command.cycle > 0;

libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp

@@ -85,9 +85,18 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
 // chan is the servo channel to send the command to
 void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority)
 {
+    // once armed only priority commands will be accepted
+    if (hal.util->get_soft_armed() && !priority) {
+        return;
+    }
+    // not an FMU channel
+    if (chan < chan_offset) {
+        return;
+    }
+
     DshotCommandPacket pkt;
     pkt.command = command;
-    pkt.chan = chan;
+    pkt.chan = chan + chan_offset;
     if (command_timeout_ms == 0) {
         pkt.cycle = MAX(10, repeat_count);
     } else {
@@ -102,21 +111,23 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
 
 // Set the dshot outputs that should be reversed (as opposed to 3D)
 // The chanmask passed is added (ORed) into any existing mask.
+// The mask uses servo channel numbering
 void RCOutput::set_reversed_mask(uint16_t chanmask) {
-    _reversed_mask |= chanmask;
+    _reversed_mask |= (chanmask >> chan_offset);
 }
 
 // Set the dshot outputs that should be reversible/3D
 // The chanmask passed is added (ORed) into any existing mask.
+// The mask uses servo channel numbering
 void RCOutput::set_reversible_mask(uint16_t chanmask) {
-    _reversible_mask |= chanmask;
+    _reversible_mask |= (chanmask >> chan_offset);
 }
 
 // Update the dshot outputs that should be reversible/3D at 1Hz
 void RCOutput::update_channel_masks() {
 
     // post arming dshot commands will not be accepted
-    if (hal.util->get_soft_armed()) {
+    if (hal.util->get_soft_armed() || _disable_channel_mask_updates) {
         return;
     }