diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp index e69ba4c18f..6f6e70d096 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp @@ -120,7 +120,7 @@ void MPU9250_AK8963::Run() CNTL1_BIT::BIT_16 | CNTL1_BIT::CONTINUOUS_MODE_2); _state = STATE::READ; - ScheduleOnInterval(10_ms, 100_ms); // 100 Hz + ScheduleDelayed(100_ms); } } else { @@ -175,8 +175,7 @@ void MPU9250_AK8963::Run() if (buffer.ST2 & ST2_BIT::HOFL) { perf_count(_magnetic_sensor_overflow_perf); - } else if ((buffer.ST1 & ST1_BIT::DRDY) && (buffer.ST2 & ST2_BIT::BITM)) { - + } else if (buffer.ST2 & ST2_BIT::BITM) { const int16_t x = combine(buffer.HXH, buffer.HXL); const int16_t y = combine(buffer.HYH, buffer.HYL); const int16_t z = combine(buffer.HZH, buffer.HZL); @@ -192,6 +191,9 @@ void MPU9250_AK8963::Run() success = true; _consecutive_failures = 0; + + ScheduleDelayed(20_ms); // ~50 Hz + return; } } @@ -199,7 +201,7 @@ void MPU9250_AK8963::Run() perf_count(_bad_transfer_perf); _consecutive_failures++; - if (_consecutive_failures > 10) { + if (_consecutive_failures > 100) { Reset(); return; } @@ -207,6 +209,7 @@ void MPU9250_AK8963::Run() // ensure mpu9250 slave sensor reading is configured _mpu9250.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ST1, sizeof(TransferBuffer)); + ScheduleDelayed(100_ms); } break;