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) { if (sitl == nullptr) {
return; return;
} }
if (instance == 0) { const uint32_t motor_mask = sitl->state.motor_mask;
state.rate_rpm = sitl->state.rpm[0]; uint8_t count = 0;
} else { // find the motor with the corresponding index
state.rate_rpm = sitl->state.rpm[1]; 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.rate_rpm *= ap_rpm._params[state.instance].scaling;
state.signal_quality = 0.5f; state.signal_quality = 0.5f;