MadgwickAHRS.c

   1 //=====================================================================================================\r
   2 // MadgwickAHRS.c\r
   3 //=====================================================================================================\r
   4 //\r
   5 // Implementation of Madgwick's IMU and AHRS algorithms.\r
   6 // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms\r
   7 //\r
   8 // Date                 Author          Notes\r
   9 // 29/09/2011   SOH Madgwick    Initial release\r
  10 // 02/10/2011   SOH Madgwick    Optimised for reduced CPU load\r
  11 // 19/02/2012   SOH Madgwick    Magnetometer measurement is normalised\r
  12 //\r
  13 //=====================================================================================================\r
  14 \r
  15 //---------------------------------------------------------------------------------------------------\r
  16 // Header files\r
  17 \r
  18 #include "MadgwickAHRS.h"\r
  19 #include <math.h>\r
  20 \r
  21 #include <f32.h>\r
  22 \r
  23 \r
  24 //---------------------------------------------------------------------------------------------------\r
  25 // Definitions\r
  26 \r
  27 //#define sampleFreq    512.0f          // sample frequency in Hz\r
  28 //#define sampleFreq    46.0f           // sample frequency in Hz\r
  29 #define sampleFreq      85.0f           // sample frequency in Hz\r
  30 #define betaDef         0.1f            // 2 * proportional gain\r
  31 \r
  32 //---------------------------------------------------------------------------------------------------\r
  33 // Variable definitions\r
  34 \r
  35 volatile float beta = betaDef;                                                          // 2 * proportional gain (Kp)\r
  36 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;      // quaternion of sensor frame relative to auxiliary frame\r
  37 \r
  38 \r
  39 //---------------------------------------------------------------------------------------------------\r
  40 // Function declarations\r
  41 \r
  42 float invSqrt(float x);\r
  43 \r
  44 //====================================================================================================\r
  45 // Functions\r
  46 \r
  47 //---------------------------------------------------------------------------------------------------\r
  48 // AHRS algorithm update\r
  49 \r
  50 void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {\r
  51         float recipNorm;\r
  52         float s0, s1, s2, s3;\r
  53         float qDot1, qDot2, qDot3, qDot4;\r
  54         float hx, hy;\r
  55         float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;\r
  56 \r
  57         // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)\r
  58         if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {\r
  59                 MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);\r
  60                 return;\r
  61         }\r
  62 \r
  63         // Rate of change of quaternion from gyroscope\r
  64         qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
  65         qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
  66         qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
  67         qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
  68 \r
  69         // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
  70         if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
  71 \r
  72                 // Normalise accelerometer measurement\r
  73                 recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
  74                 ax *= recipNorm;\r
  75                 ay *= recipNorm;\r
  76                 az *= recipNorm;\r
  77 \r
  78                 // Normalise magnetometer measurement\r
  79                 recipNorm = invSqrt(mx * mx + my * my + mz * mz);\r
  80                 mx *= recipNorm;\r
  81                 my *= recipNorm;\r
  82                 mz *= recipNorm;\r
  83 \r
  84                 // Auxiliary variables to avoid repeated arithmetic\r
  85                 _2q0mx = 2.0f * q0 * mx;\r
  86                 _2q0my = 2.0f * q0 * my;\r
  87                 _2q0mz = 2.0f * q0 * mz;\r
  88                 _2q1mx = 2.0f * q1 * mx;\r
  89                 _2q0 = 2.0f * q0;\r
  90                 _2q1 = 2.0f * q1;\r
  91                 _2q2 = 2.0f * q2;\r
  92                 _2q3 = 2.0f * q3;\r
  93                 _2q0q2 = 2.0f * q0 * q2;\r
  94                 _2q2q3 = 2.0f * q2 * q3;\r
  95                 q0q0 = q0 * q0;\r
  96                 q0q1 = q0 * q1;\r
  97                 q0q2 = q0 * q2;\r
  98                 q0q3 = q0 * q3;\r
  99                 q1q1 = q1 * q1;\r
 100                 q1q2 = q1 * q2;\r
 101                 q1q3 = q1 * q3;\r
 102                 q2q2 = q2 * q2;\r
 103                 q2q3 = q2 * q3;\r
 104                 q3q3 = q3 * q3;\r
 105 \r
 106                 // Reference direction of Earth's magnetic field\r
 107                 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;\r
 108                 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;\r
 109                 _2bx = sqrt(hx * hx + hy * hy);\r
 110                 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;\r
 111                 _4bx = 2.0f * _2bx;\r
 112                 _4bz = 2.0f * _2bz;\r
 113 \r
 114                 // Gradient decent algorithm corrective step\r
 115                 s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
 116                 s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
 117                 s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
 118                 s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
 119                 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
 120                 s0 *= recipNorm;\r
 121                 s1 *= recipNorm;\r
 122                 s2 *= recipNorm;\r
 123                 s3 *= recipNorm;\r
 124 \r
 125                 // Apply feedback step\r
 126                 qDot1 -= beta * s0;\r
 127                 qDot2 -= beta * s1;\r
 128                 qDot3 -= beta * s2;\r
 129                 qDot4 -= beta * s3;\r
 130         }\r
 131 \r
 132         // Integrate rate of change of quaternion to yield quaternion\r
 133         q0 += qDot1 * (1.0f / sampleFreq);\r
 134         q1 += qDot2 * (1.0f / sampleFreq);\r
 135         q2 += qDot3 * (1.0f / sampleFreq);\r
 136         q3 += qDot4 * (1.0f / sampleFreq);\r
 137 \r
 138         // Normalise quaternion\r
 139         recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
 140         q0 *= recipNorm;\r
 141         q1 *= recipNorm;\r
 142         q2 *= recipNorm;\r
 143         q3 *= recipNorm;\r
 144 }\r
 145 \r
 146 //---------------------------------------------------------------------------------------------------\r
 147 // IMU algorithm update\r
 148 \r
 149 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {\r
 150         float recipNorm;\r
 151         float s0, s1, s2, s3;\r
 152         float qDot1, qDot2, qDot3, qDot4;\r
 153         float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;\r
 154 \r
 155         // Rate of change of quaternion from gyroscope\r
 156         qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
 157         qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
 158         qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
 159         qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
 160 \r
 161 \r
 162         // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
 163         if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
 164 \r
 165                 // Normalise accelerometer measurement\r
 166                 recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
 167                 ax *= recipNorm;\r
 168                 ay *= recipNorm;\r
 169                 az *= recipNorm;\r
 170 \r
 171                 // Auxiliary variables to avoid repeated arithmetic\r
 172                 _2q0 = 2.0f * q0;\r
 173                 _2q1 = 2.0f * q1;\r
 174                 _2q2 = 2.0f * q2;\r
 175                 _2q3 = 2.0f * q3;\r
 176                 _4q0 = 4.0f * q0;\r
 177                 _4q1 = 4.0f * q1;\r
 178                 _4q2 = 4.0f * q2;\r
 179                 _8q1 = 8.0f * q1;\r
 180                 _8q2 = 8.0f * q2;\r
 181                 q0q0 = q0 * q0;\r
 182                 q1q1 = q1 * q1;\r
 183                 q2q2 = q2 * q2;\r
 184                 q3q3 = q3 * q3;\r
 185 \r
 186 \r
 187                 // Gradient decent algorithm corrective step\r
 188                 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;\r
 189                 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;\r
 190                 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;\r
 191                 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;\r
 192                 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
 193 \r
 194 \r
 195 \r
 196                 s0 *= recipNorm;\r
 197                 s1 *= recipNorm;\r
 198                 s2 *= recipNorm;\r
 199                 s3 *= recipNorm;\r
 200 \r
 201 \r
 202                 // Apply feedback step\r
 203                 qDot1 -= beta * s0;\r
 204                 qDot2 -= beta * s1;\r
 205                 qDot3 -= beta * s2;\r
 206                 qDot4 -= beta * s3;\r
 207         }\r
 208 \r
 209         // Integrate rate of change of quaternion to yield quaternion\r
 210         q0 += qDot1 * (1.0f / sampleFreq);\r
 211         q1 += qDot2 * (1.0f / sampleFreq);\r
 212         q2 += qDot3 * (1.0f / sampleFreq);\r
 213         q3 += qDot4 * (1.0f / sampleFreq);\r
 214 \r
 215 \r
 216         // Normalise quaternion\r
 217         recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
 218         q0 *= recipNorm;\r
 219         q1 *= recipNorm;\r
 220         q2 *= recipNorm;\r
 221         q3 *= recipNorm;\r
 222 \r
 223         //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2);\r
 224 \r
 225 }\r
 226 \r
 227 \r
 228 //---------------------------------------------------------------------------------------------------\r
 229 // IMU algorithm update\r
 230 \r
 231 f32 f_q0;\r
 232 f32 f_q1;\r
 233 f32 f_q2;\r
 234 f32 f_q3;\r
 235 \r
 236 \r
 237 void Mad_f32_init()\r
 238 {\r
 239         f_q0 = f32_from_double((double)1.0);\r
 240         f_q1 = f32_from_double((double)0.0);\r
 241         f_q2 = f32_from_double((double)0.0);\r
 242         f_q3 = f32_from_double((double)0.0);\r
 243 \r
 244 }\r
 245 void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az) {\r
 246         f32 f_recipNorm;\r
 247         f32 f_s0, f_s1, f_s2, f_s3;\r
 248         f32 f_qDot1, f_qDot2, f_qDot3, f_qDot4;\r
 249         f32 f__2q0, f__2q1, f__2q2, f__2q3, f__4q0, f__4q1, f__4q2 ,f__8q1, f__8q2, f_q0q0, f_q1q1, f_q2q2, f_q3q3;\r
 250 \r
 251         f32 f_gx, f_gy, f_gz;\r
 252         f32 f_ax, f_ay, f_az;\r
 253 \r
 254         f32 f_beta;\r
 255         f32 f_sampleFreq;\r
 256 \r
 257 \r
 258         f_gx = f32_from_double((double)gx);\r
 259         f_gy = f32_from_double((double)gy);\r
 260         f_gz = f32_from_double((double)gz);\r
 261 \r
 262         f_ax = f32_from_double((double)ax);\r
 263         f_ay = f32_from_double((double)ay);\r
 264         f_az = f32_from_double((double)az);\r
 265 \r
 266         f_beta = f32_from_double((double)beta);\r
 267         f_sampleFreq = f32_from_double((double)sampleFreq);\r
 268 \r
 269 \r
 270         // Rate of change of quaternion from gyroscope\r
 271         /*\r
 272         qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
 273         qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
 274         qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
 275         qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
 276         */\r
 277 \r
 278         f_qDot1 = f32_mul(f32_sub(f32_sub(f32_neg(f32_mul(f_gx, f_q1)), f32_mul(f_gy, f_q2)), f32_mul(f_gz, f_q3)), f32_from_double(0.5));\r
 279         f_qDot2 = f32_mul(f32_sub(f32_add(f32_mul(f_gx, f_q0), f32_mul(f_gz, f_q2)), f32_mul(f_gy, f_q3)), f32_from_double(0.5));\r
 280         f_qDot3 = f32_mul(f32_add(f32_mul(f_gx, f_q3), f32_sub(f32_mul(f_gy, f_q0), f32_mul(f_gz, f_q1))), f32_from_double(0.5));\r
 281         f_qDot4 = f32_mul(f32_sub(f32_add(f32_mul(f_gy, f_q1), f32_mul(f_gz, f_q0)), f32_mul(f_gx, f_q2)), f32_from_double(0.5));\r
 282 \r
 283 \r
 284 \r
 285 \r
 286         // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
 287         if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
 288 \r
 289                 // Normalise accelerometer measurement\r
 290                 //recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
 291                 f_recipNorm = f32_inv(f32_sqrt((f32_add(f32_mul(f_ax, f_ax), f32_add(f32_mul(f_ay, f_ay), f32_mul(f_az, f_az))))));\r
 292                 /*\r
 293                 ax *= recipNorm;\r
 294                 ay *= recipNorm;\r
 295                 az *= recipNorm;\r
 296                 */\r
 297 \r
 298                 f_ax = f32_mul(f_ax, f_recipNorm);\r
 299                 f_ay = f32_mul(f_ay, f_recipNorm);\r
 300                 f_az = f32_mul(f_az, f_recipNorm);\r
 301 \r
 302                 // Auxiliary variables to avoid repeated arithmetic\r
 303 \r
 304                 /*\r
 305                 _2q0 = 2.0f * q0;\r
 306                 _2q1 = 2.0f * q1;\r
 307                 _2q2 = 2.0f * q2;\r
 308                 _2q3 = 2.0f * q3;\r
 309                 _4q0 = 4.0f * q0;\r
 310                 _4q1 = 4.0f * q1;\r
 311                 _4q2 = 4.0f * q2;\r
 312                 _8q1 = 8.0f * q1;\r
 313                 _8q2 = 8.0f * q2;\r
 314                 q0q0 = q0 * q0;\r
 315                 q1q1 = q1 * q1;\r
 316                 q2q2 = q2 * q2;\r
 317                 q3q3 = q3 * q3;\r
 318                 */\r
 319 \r
 320                 f__2q0 = f32_mul(f32_from_double(2.0f), f_q0);\r
 321                 f__2q1 = f32_mul(f32_from_double(2.0f), f_q1);\r
 322                 f__2q2 = f32_mul(f32_from_double(2.0f), f_q2);\r
 323                 f__2q3 = f32_mul(f32_from_double(2.0f), f_q3);\r
 324                 f__4q0 = f32_mul(f32_from_double(4.0f), f_q0);\r
 325                 f__4q1 = f32_mul(f32_from_double(4.0f), f_q1);\r
 326                 f__4q2 = f32_mul(f32_from_double(4.0f), f_q2);\r
 327                 f__8q1 = f32_mul(f32_from_double(8.0f), f_q1);\r
 328                 f__8q2 = f32_mul(f32_from_double(8.0f), f_q2);\r
 329                 f_q0q0 = f32_mul(f_q0, f_q0);\r
 330                 f_q1q1 = f32_mul(f_q1, f_q1);\r
 331                 f_q2q2 = f32_mul(f_q2, f_q2);\r
 332                 f_q3q3 = f32_mul(f_q3, f_q3);\r
 333 \r
 334 \r
 335                 // Gradient decent algorithm corrective step\r
 336 \r
 337                 /*\r
 338                 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;\r
 339                 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;\r
 340                 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;\r
 341                 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;\r
 342                 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
 343                 */\r
 344 \r
 345                 f_s0 = f32_sub(f32_add(f32_mul(f__2q2, f_ax), f32_add(f32_mul(f__4q0, f_q1q1), f32_mul(f__4q0, f_q2q2))), f32_mul(f__2q1, f_ay));\r
 346                 f_s1 = f32_add(f32_mul(f__4q1, f_az), f32_add(f32_mul(f__8q1, f_q1q1), f32_add(f32_mul(f__8q1, f_q2q2), f32_sub(f32_sub(f32_add(f32_mul(f_q0q0, f32_mul(f_q1, f32_from_double(4.0f))), f32_sub(f32_mul(f__4q1, f_q3q3), f32_mul(f__2q3, f_ax))), f32_mul(f__2q0, f_ay)), f__4q1))));\r
 347                 f_s2 = f32_add(f32_mul(f__4q2, f_az), f32_add(f32_mul(f__8q2, f_q1q1), f32_add(f32_mul(f__8q2, f_q2q2), f32_sub(f32_sub(f32_add(f32_mul(f__2q0, f_ax), f32_add(f32_mul(f__4q2, f_q3q3), f32_mul(f_q0q0, f32_mul(f_q2, f32_from_double(4.0))))), f32_mul(f__2q3, f_ay)), f__4q2))));\r
 348                 f_s3 = f32_sub(f32_add(f32_mul(f_q2q2, f32_mul(f_q3, f32_from_double(4.0))), f32_sub(f32_mul(f_q1q1, f32_mul(f_q3, f32_from_double(4.0))), f32_mul(f__2q1, f_ax))), f32_mul(f__2q2, f_ay));\r
 349                 f_recipNorm = f32_inv(f32_sqrt(f32_add(f32_mul(f_s0, f_s0), f32_add(f32_mul(f_s1, f_s1), f32_add(f32_mul(f_s2, f_s2), f32_mul(f_s3, f_s3))))));\r
 350 \r
 351                 /*\r
 352                 s0 *= recipNorm;\r
 353                 s1 *= recipNorm;\r
 354                 s2 *= recipNorm;\r
 355                 s3 *= recipNorm;\r
 356                 */\r
 357 \r
 358                 f_s0 = f32_mul(f_s0, f_recipNorm);\r
 359                 f_s1 = f32_mul(f_s1, f_recipNorm);\r
 360                 f_s2 = f32_mul(f_s2, f_recipNorm);\r
 361                 f_s3 = f32_mul(f_s3, f_recipNorm);\r
 362 \r
 363 \r
 364                 // Apply feedback step\r
 365 \r
 366                 /*\r
 367                 qDot1 -= beta * s0;\r
 368                 qDot2 -= beta * s1;\r
 369                 qDot3 -= beta * s2;\r
 370                 qDot4 -= beta * s3;\r
 371                 */\r
 372 \r
 373                 f_qDot1 = f32_sub(f_qDot1, f32_mul(f_beta, f_s0));\r
 374                 f_qDot2 = f32_sub(f_qDot2, f32_mul(f_beta, f_s1));\r
 375                 f_qDot3 = f32_sub(f_qDot3, f32_mul(f_beta, f_s2));\r
 376                 f_qDot4 = f32_sub(f_qDot4, f32_mul(f_beta, f_s3));\r
 377 \r
 378         }\r
 379 \r
 380         // Integrate rate of change of quaternion to yield quaternion\r
 381 \r
 382         /*\r
 383         q0 += qDot1 * (1.0f / sampleFreq);\r
 384         q1 += qDot2 * (1.0f / sampleFreq);\r
 385         q2 += qDot3 * (1.0f / sampleFreq);\r
 386         q3 += qDot4 * (1.0f / sampleFreq);\r
 387         */\r
 388 \r
 389         f_q0 = f32_add(f_q0, f32_mul(f_qDot1, f32_inv(f_sampleFreq)));\r
 390         f_q1 = f32_add(f_q1, f32_mul(f_qDot2, f32_inv(f_sampleFreq)));\r
 391         f_q2 = f32_add(f_q2, f32_mul(f_qDot3, f32_inv(f_sampleFreq)));\r
 392         f_q3 = f32_add(f_q3, f32_mul(f_qDot4, f32_inv(f_sampleFreq)));\r
 393 \r
 394         // Normalise quaternion\r
 395         //recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
 396 \r
 397         f_recipNorm = f32_inv(f32_sqrt(f32_add(f32_mul(f_q0, f_q0), f32_add(f32_mul(f_q1, f_q1), f32_add(f32_mul(f_q2, f_q2), f32_mul(f_q3, f_q3))))));\r
 398 \r
 399         /*\r
 400         q0 *= recipNorm;\r
 401         q1 *= recipNorm;\r
 402         q2 *= recipNorm;\r
 403         q3 *= recipNorm;\r
 404         */\r
 405 \r
 406         f_q0 = f32_mul(f_q0, f_recipNorm);\r
 407         f_q1 = f32_mul(f_q1, f_recipNorm);\r
 408         f_q2 = f32_mul(f_q2, f_recipNorm);\r
 409         f_q3 = f32_mul(f_q3, f_recipNorm);\r
 410 \r
 411 \r
 412         q0 = f32_to_double(f_q0);\r
 413         q1 = f32_to_double(f_q1);\r
 414         q2 = f32_to_double(f_q2);\r
 415         q3 = f32_to_double(f_q3);\r
 416 \r
 417         //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2);\r
 418 \r
 419 }\r
 420 \r
 421 \r
 422 //---------------------------------------------------------------------------------------------------\r
 423 // Fast inverse square-root\r
 424 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root\r
 425 /*\r
 426 float invSqrt(float x) {\r
 427         float halfx = 0.5f * x;\r
 428         float y = x;\r
 429         long i = *(long*)&y;\r
 430         i = 0x5f3759df - (i>>1);\r
 431         y = *(float*)&i;\r
 432         y = y * (1.5f - (halfx * y * y));\r
 433         return y;\r
 434 }\r
 435 */\r
 436 float invSqrt(float x) {\r
 437         return 1.0f / sqrtf(x);\r
 438 }\r
 439 //====================================================================================================\r
 440 // END OF CODE\r
 441 //====================================================================================================\r