aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: allow testing of IMU failure

Browse Source
This commit is contained in:
Andrew Tridgell 2019-04-18 12:41:10 +10:00
parent e18f40cc08
commit c7ed4bfbcc
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -179,15 +179,19 @@ void AP_InertialSensor_SITL::timer_update(void)
#endif
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
if (now >= next_accel_sample[i]) {
generate_accel(i);
while (now >= next_accel_sample[i]) {
next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
if (((1U<<i) & sitl->accel_fail_mask) == 0) {
generate_accel(i);
while (now >= next_accel_sample[i]) {
next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
}
}
}
if (now >= next_gyro_sample[i]) {
generate_gyro(i);
while (now >= next_gyro_sample[i]) {
next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
if (((1U<<i) & sitl->gyro_fail_mask) == 0) {
generate_gyro(i);
while (now >= next_gyro_sample[i]) {
next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
}
}
}
}