From a51e58022f112ec10b9c5e826fcd28151e54bc36 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 29 Jul 2021 21:46:15 +0100 Subject: [PATCH] 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 | 11 +++++----- libraries/AP_HAL_ChibiOS/RCOutput.h | 22 ++++++++++++++++---- libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 19 +++++++++++++---- 3 files changed, 38 insertions(+), 14 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 17d4572b54..a271b80c8a 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/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) { diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 173667e2e2..9591a3cd44 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/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; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp index 6e3ed3a099..e6db93d673 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp +++ b/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; }