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

AP_ESC_TELEM: allow for non-continguous ESC telem motor sets 

this fixes ESC telem for quadplanes with motors at 5-8 or 9-12
This commit is contained in:
Andrew Tridgell 2022-08-18 17:24:56 +10:00
parent 9ffdf2511f
commit 3dad2a0987
1 changed files with 22 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
View File

@ -37,7 +37,26 @@ void AP_ESC_Telem_SITL::update()
}
#if HAL_WITH_ESC_TELEM
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
if (AP_HAL::millis64() < 6000) {
// this prevents us sending blank data at startup, which triggers
// ESC telem messages for all channels
return;
}
uint32_t mask = sitl->state.motor_mask;
uint8_t bit;
while ((bit = __builtin_ffs(mask)) != 0) {
uint8_t motor = bit-1;
mask &= ~(1U<<motor);
if (is_zero(sitl->throttle)) {
if (!is_zero(sitl->esc_rpm_armed) && hal.util->get_soft_armed()) {
update_rpm(motor, sitl->esc_rpm_armed);
}
} else {
update_rpm(motor, sitl->state.rpm[motor]);
}
// some fake values so that is_telemetry_active() returns true
TelemetryData t {
.temperature_cdeg = 32,
@ -46,27 +65,13 @@ void AP_ESC_Telem_SITL::update()
.consumption_mah = 1.0f,
};
update_telem_data(i, t,
update_telem_data(motor, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
| AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
}
if (is_zero(sitl->throttle)) {
if (!is_zero(sitl->esc_rpm_armed) && hal.util->get_soft_armed()) {
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
update_rpm(i, sitl->esc_rpm_armed);
}
}
return;
}
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
update_rpm(i, MAX(sitl->state.rpm[sitl->state.vtol_motor_start+i], sitl->esc_rpm_armed));
}
#endif
}
#endif
#endif