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

AP_ESC_Telem: use SIM_CAN_SRV_MSK and fixed throttle dependency 

RPM should not depend on "throttle", it is per motor
This commit is contained in:
Andrew Tridgell 2023-08-20 08:50:35 +10:00 committed by Peter Barker
parent 4e71023018
commit 3a8fd367ad
1 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
View File

@ -44,18 +44,26 @@ void AP_ESC_Telem_SITL::update()
return;
}
uint32_t mask = sitl->state.motor_mask;
/*
mask out motors we should not be providing telemetry for. On
AP_Periph SIM_CAN_SRV_MSK are the outputs we will provide
telemetry for, on the main firmware it is the ones we don't
provide telemetry for
*/
#if defined(HAL_BUILD_AP_PERIPH)
mask &= uint32_t(sitl->can_servo_mask);
#else
mask &= ~uint32_t(sitl->can_servo_mask);
#endif
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]);
}
const float min_rpm = hal.util->get_soft_armed()? sitl->esc_rpm_armed : 0;
update_rpm(motor, MAX(min_rpm, sitl->state.rpm[motor]));
// some fake values so that is_telemetry_active() returns true
TelemetryData t {