Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: add support for Extended DShot Telemetry v2

Browse Source
This commit is contained in:
Maxim Buzdalov 2024-05-14 09:16:23 +10:00 committed by Andrew Tridgell
parent 1232e810d4
commit ada091f1fc
3 changed files with 27 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -453,7 +453,11 @@ void AP_IOMCU::read_telem()
TelemetryData t {
.temperature_cdeg = int16_t(telem->temperature_cdeg[i]),
.voltage = float(telem->voltage_cvolts[i]) * 0.01,
.current = float(telem->current_camps[i]) * 0.01
.current = float(telem->current_camps[i]) * 0.01,
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
.edt2_status = telem->edt2_status[i],
.edt2_stress = telem->edt2_stress[i],
#endif
};
update_telem_data(esc_group * 4 + i, t, telem->types[i]);
}

25
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -635,25 +635,34 @@ void AP_IOMCU_FW::telem_update()
uint32_t now_ms = AP_HAL::millis();
for (uint8_t i = 0; i < IOMCU_MAX_TELEM_CHANNELS/4; i++) {
struct page_dshot_telem &dshot_i = dshot_telem[i];
for (uint8_t j = 0; j < 4; j++) {
const uint8_t esc_id = (i * 4 + j);
if (esc_id >= IOMCU_MAX_TELEM_CHANNELS) {
continue;
}
dshot_telem[i].error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0));
dshot_i.error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0));
#if HAL_WITH_ESC_TELEM
const volatile AP_ESC_Telem_Backend::TelemetryData& telem = esc_telem.get_telem_data(esc_id);
// if data is stale then set to zero to avoid phantom data appearing in mavlink
if (now_ms - telem.last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS) {
dshot_telem[i].voltage_cvolts[j] = 0;
dshot_telem[i].current_camps[j] = 0;
dshot_telem[i].temperature_cdeg[j] = 0;
dshot_i.voltage_cvolts[j] = 0;
dshot_i.current_camps[j] = 0;
dshot_i.temperature_cdeg[j] = 0;
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
dshot_i.edt2_status[j] = 0;
dshot_i.edt2_stress[j] = 0;
#endif
continue;
}
dshot_telem[i].voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100));
dshot_telem[i].current_camps[j] = uint16_t(roundf(telem.current * 100));
dshot_telem[i].temperature_cdeg[j] = telem.temperature_cdeg;
dshot_telem[i].types[j] = telem.types;
dshot_i.voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100));
dshot_i.current_camps[j] = uint16_t(roundf(telem.current * 100));
dshot_i.temperature_cdeg[j] = telem.temperature_cdeg;
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
dshot_i.edt2_status[j] = uint8_t(telem.edt2_status);
dshot_i.edt2_stress[j] = uint8_t(telem.edt2_stress);
#endif
dshot_i.types[j] = telem.types;
#endif
}
}

5
libraries/AP_IOMCU/iofirmware/ioprotocol.h
View File

@ -223,4 +223,9 @@ struct page_dshot_telem {
uint16_t current_camps[4];
uint16_t temperature_cdeg[4];
uint16_t types[4];
// if EDTv2 needs to be disabled, IOMCU firmware should be recompiled too, this is the reason
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
uint8_t edt2_status[4];
uint8_t edt2_stress[4];
#endif
};