Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: dshot commands for reverse/3D should be sent to IOMCU 

normalize servo/FMU channels for dshot commands and 3D mask
This commit is contained in:
Andy Piper 2024-08-20 18:11:06 +01:00 committed by Andrew Tridgell
parent 7f8e5aab85
commit 48624f189e
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -1649,7 +1649,9 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
bdshot_decode_telemetry_from_erpm(group.bdshot.erpm[i], chan); bdshot_decode_telemetry_from_erpm(group.bdshot.erpm[i], chan);
} }
#endif #endif
if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) { const uint32_t servo_chan_mask = 1U<<(chan+chan_offset);
if (safety_on && !(safety_mask & servo_chan_mask)) {
// safety is on, don't output anything // safety is on, don't output anything
continue; continue;
} }
@ -1661,12 +1663,10 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
continue; continue;
} }
const uint32_t chan_mask = (1U<<chan);
pwm = constrain_int16(pwm, 1000, 2000); pwm = constrain_int16(pwm, 1000, 2000);
uint16_t value = MIN(2 * (pwm - 1000), 1999); uint16_t value = MIN(2 * (pwm - 1000), 1999);
if ((chan_mask & _reversible_mask) != 0) { if ((servo_chan_mask & _reversible_mask) != 0) {
// this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed // this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed
if (value < 1000) { if (value < 1000) {
value = 1999 - value; value = 1999 - value;
@ -1689,6 +1689,7 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
} }
// according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc // according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc
const uint32_t chan_mask = (1U<<chan);
bool request_telemetry = telem_request_mask & chan_mask; bool request_telemetry = telem_request_mask & chan_mask;
uint16_t packet = create_dshot_packet(value, request_telemetry, uint16_t packet = create_dshot_packet(value, request_telemetry,
#ifdef HAL_WITH_BIDIR_DSHOT #ifdef HAL_WITH_BIDIR_DSHOT

10
libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
View File

@ -33,6 +33,8 @@ using namespace ChibiOS;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// send a dshot command to group
// chan is the FMU channel to send the command to
bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t chan) bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t chan)
{ {
if (!group.can_send_dshot_pulse()) { if (!group.can_send_dshot_pulse()) {
@ -112,10 +114,11 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
DshotCommandPacket pkt; DshotCommandPacket pkt;
pkt.command = command; pkt.command = command;
if (chan != ALL_CHANNELS) { if (chan != ALL_CHANNELS) {
pkt.chan = chan - chan_offset; pkt.chan = chan - chan_offset; // normalize to FMU channel
} else { } else {
pkt.chan = ALL_CHANNELS; pkt.chan = ALL_CHANNELS;
} }
if (command_timeout_ms == 0) { if (command_timeout_ms == 0) {
pkt.cycle = MAX(10, repeat_count); pkt.cycle = MAX(10, repeat_count);
} else { } else {
@ -156,8 +159,9 @@ void RCOutput::update_channel_masks() {
return; return;
} }
#if HAL_PWM_COUNT > 0 // The masks use servo channel numbering
for (uint8_t i=chan_offset; i<HAL_PWM_COUNT+chan_offset; i++) { #if NUM_SERVO_CHANNELS > 0
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
switch (_dshot_esc_type) { switch (_dshot_esc_type) {
case DSHOT_ESC_BLHELI: case DSHOT_ESC_BLHELI:
case DSHOT_ESC_BLHELI_S: case DSHOT_ESC_BLHELI_S: