/* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
* magnetometer normalisation) */
- if ((imu->mx == 0.0f) && (imu->my == 0.0f) && (imu->mz == 0.0f)) {
+ //if ((imu->mx == 0.0f) && (imu->my == 0.0f) && (imu->mz == 0.0f)) {
+ if (1) {
MadgwickAHRSupdateIMU(imu, quat);
return;
}