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

AP_RPM: fixed SITL RPM backend for new motor mask

This commit is contained in:
Andrew Tridgell 2022-10-15 17:55:08 +11:00
parent 1138f9e624
commit 1c5c1a566f
1 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_RPM/RPM_SITL.cpp
View File

@ -34,10 +34,17 @@ void AP_RPM_SITL::update(void)
if (sitl == nullptr) {
return;
}
if (instance == 0) {
state.rate_rpm = sitl->state.rpm[0];
} else {
state.rate_rpm = sitl->state.rpm[1];
const uint32_t motor_mask = sitl->state.motor_mask;
uint8_t count = 0;
// find the motor with the corresponding index
for (uint8_t i=0; i<32; i++) {
if (motor_mask & (1U<<i)) {
if (count == instance) {
state.rate_rpm = sitl->state.rpm[i];
break;
}
count++;
}
}
state.rate_rpm *= ap_rpm._params[state.instance].scaling;
state.signal_quality = 0.5f;