- // Rate of change of quaternion from gyroscope\r
- qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
- qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
- qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
- qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
-\r
- // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
- if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
-\r
- // Normalise accelerometer measurement\r
- recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
- ax *= recipNorm;\r
- ay *= recipNorm;\r
- az *= recipNorm;\r
-\r
- // Normalise magnetometer measurement\r
- recipNorm = invSqrt(mx * mx + my * my + mz * mz);\r
- mx *= recipNorm;\r
- my *= recipNorm;\r
- mz *= recipNorm;\r
-\r
- // Auxiliary variables to avoid repeated arithmetic\r
+ /* Rate of change of quaternion from gyroscope */\r
+ qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz);\r
+ qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy);\r
+ qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx);\r
+ qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx);\r
+\r
+ /* Compute feedback only if accelerometer measurement valid (avoids NaN\r
+ * in accelerometer normalisation) */\r
+ if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) {\r
+\r
+ /* Normalise accelerometer measurement */\r
+ recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay +\r
+ imu->az * imu->az);\r
+ ax = imu->ax * recipNorm;\r
+ ay = imu->ay * recipNorm;\r
+ az = imu->az * recipNorm;\r
+\r
+ /* Normalise magnetometer measurement */\r
+ recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my +\r
+ imu->mz * imu->mz);\r
+ mx = imu->mx * recipNorm;\r
+ my = imu->my * recipNorm;\r
+ mz = imu->mz * recipNorm;\r
+\r
+ /* Auxiliary variables to avoid repeated arithmetic */\r