Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: implement extended dshot telemetry (EDT)

Browse Source
This commit is contained in:
Andy Piper 2023-05-29 20:45:47 +01:00 committed by Andrew Tridgell
parent 79bd29e9a7
commit 76b7566cd1
4 changed files with 87 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -539,6 +539,7 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type)
_dshot_esc_type = dshot_esc_type;
switch (_dshot_esc_type) {
case DSHOT_ESC_BLHELI_S:
case DSHOT_ESC_BLHELI_EDT_S:
DSHOT_BIT_WIDTH_TICKS = DSHOT_BIT_WIDTH_TICKS_S;
DSHOT_BIT_0_TICKS = DSHOT_BIT_0_TICKS_S;
DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_S;
@ -1531,15 +1532,9 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
uint8_t chan = group.chan[i];
if (group.is_chan_enabled(i)) {
#ifdef HAL_WITH_BIDIR_DSHOT
// retrieve the last erpm values
const uint16_t erpm = group.bdshot.erpm[i];
#if HAL_WITH_ESC_TELEM
// update the ESC telemetry data
if (erpm < 0xFFFF && group.bdshot.enabled) {
update_rpm(chan, erpm * 200 / _bdshot.motor_poles, get_erpm_error_rate(chan));
if (group.bdshot.enabled) {
bdshot_decode_telemetry_from_erpm(group.bdshot.erpm[i], chan);
}
#endif
_bdshot.erpm[chan] = erpm;
#endif
if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
// safety is on, don't output anything

3
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -222,7 +222,7 @@ public:
Send a dshot command, if command timout is 0 then 10 commands are sent
chan is the servo channel to send the command to
*/
void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override;
void send_dshot_command(uint8_t command, uint8_t chan = ALL_CHANNELS, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override;
/*
* Update channel masks at 1Hz allowing for actions such as dshot commands to be sent
@ -644,6 +644,7 @@ private:
void bdshot_ic_dma_allocate(Shared_DMA *ctx);
void bdshot_ic_dma_deallocate(Shared_DMA *ctx);
static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count);
bool bdshot_decode_telemetry_from_erpm(uint16_t erpm, uint8_t chan);
bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
static uint8_t bdshot_find_next_ic_channel(const pwm_group& group);
static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags);

85
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -595,17 +595,84 @@ uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t cou
}
decodedValue >>= 4;
if (decodedValue == 0x0fff) {
return 0;
}
decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
if (!decodedValue) {
return 0xffff;
}
uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
return ret;
return decodedValue;
}
#pragma GCC pop_options
// update ESC telemetry information. Returns true if valid eRPM data was decoded.
bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t chan)
{
if (encodederpm == 0xFFFF) {
return false;
}
// eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry)
uint8_t expo = ((encodederpm & 0xfffffe00) >> 9) & 0xFF;
uint16_t value = (encodederpm & 0x000001ff);
if (!(value & 0x100) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) {
switch (expo) {
case 0b001: { // Temperature C
#if HAL_WITH_ESC_TELEM
TelemetryData t {
.temperature_cdeg = int16_t(value * 100)
};
update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
#endif
return false;
}
break;
case 0b010: { // Voltage 0.25v
#if HAL_WITH_ESC_TELEM
TelemetryData t {
.voltage = 0.25f * value
};
update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE);
#endif
return false;
}
break;
case 0b011: { // Current A
#if HAL_WITH_ESC_TELEM
TelemetryData t {
.current = float(value)
};
update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT);
#endif
return false;
}
break;
case 0b100: // Debug 1
case 0b101: // Debug 2
case 0b110: // Stress level
case 0b111: // Status
return false;
break;
default: // eRPM
break;
}
}
uint16_t erpm = value << expo;
if (!erpm) { // decoded as 0 is an error
return false;
}
erpm = (1000000 * 60 / 100 + erpm / 2) / erpm;
if (encodederpm == 0x0fff) { // the special 0 encoding
erpm = 0;
}
// update the ESC telemetry data
if (erpm < 0xFFFF) {
_bdshot.erpm[chan] = erpm;
#if HAL_WITH_ESC_TELEM
update_rpm(chan, erpm * 200 / _bdshot.motor_poles, get_erpm_error_rate(chan));
#endif
}
return erpm < 0xFFFF;
}
#endif // HAL_WITH_BIDIR_DSHOT

6
libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
View File

@ -147,6 +147,8 @@ void RCOutput::update_channel_masks() {
switch (_dshot_esc_type) {
case DSHOT_ESC_BLHELI:
case DSHOT_ESC_BLHELI_S:
case DSHOT_ESC_BLHELI_EDT:
case DSHOT_ESC_BLHELI_EDT_S:
if (_reversible_mask & (1U<<i)) {
send_dshot_command(DSHOT_3D_ON, i + chan_offset, 0, 10, true);
}
@ -158,6 +160,10 @@ void RCOutput::update_channel_masks() {
break;
}
}
if (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S) {
send_dshot_command(DSHOT_EXTENDED_TELEMETRY_ENABLE, ALL_CHANNELS, 0, 10, true);
}
#endif
}