Rio Harris / Madgwick_Algorithm

library for 9dof using madgwick's algorithm

Dependents:   NerfGun_nRF24L01P_TX_9d0f

MadgwickAHRS.cpp@0:756055ce357a, 2015-08-13 (annotated)

Committer:
b50559
Date:
Thu Aug 13 22:11:38 2015 +0000
Revision:
0:756055ce357a
created library for sensor fusion using madgwick's algorithm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
b50559 0:756055ce357a 1 //=====================================================================================================
b50559 0:756055ce357a 2 // MadgwickAHRS.c
b50559 0:756055ce357a 3 //=====================================================================================================
b50559 0:756055ce357a 4 //
b50559 0:756055ce357a 5 // Implementation of Madgwick's IMU and AHRS algorithms.
b50559 0:756055ce357a 6 // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
b50559 0:756055ce357a 7 //
b50559 0:756055ce357a 8 // Date Author Notes
b50559 0:756055ce357a 9 // 29/09/2011 SOH Madgwick Initial release
b50559 0:756055ce357a 10 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
b50559 0:756055ce357a 11 // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
b50559 0:756055ce357a 12 //
b50559 0:756055ce357a 13 //=====================================================================================================
b50559 0:756055ce357a 14
b50559 0:756055ce357a 15 //---------------------------------------------------------------------------------------------------
b50559 0:756055ce357a 16 // Header files
b50559 0:756055ce357a 17
b50559 0:756055ce357a 18 #include "mbed.h"
b50559 0:756055ce357a 19 #include "MadgwickAHRS.h"
b50559 0:756055ce357a 20 #include <math.h>
b50559 0:756055ce357a 21
b50559 0:756055ce357a 22 //---------------------------------------------------------------------------------------------------
b50559 0:756055ce357a 23 // Definitions
b50559 0:756055ce357a 24
b50559 0:756055ce357a 25 //#define sampleFreq 512.0f // sample frequency in Hz
b50559 0:756055ce357a 26 #define betaDef 0.2f //2* proportional gain
b50559 0:756055ce357a 27 #define PI 3.14159265359f
b50559 0:756055ce357a 28
b50559 0:756055ce357a 29 //---------------------------------------------------------------------------------------------------
b50559 0:756055ce357a 30
b50559 0:756055ce357a 31 MadgwickAHRS::MadgwickAHRS(float Freq){
b50559 0:756055ce357a 32
b50559 0:756055ce357a 33 sampleFreq = Freq;
b50559 0:756055ce357a 34
b50559 0:756055ce357a 35 }
b50559 0:756055ce357a 36
b50559 0:756055ce357a 37 float beta = betaDef; // 2 * proportional gain (Kp)
b50559 0:756055ce357a 38 float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
b50559 0:756055ce357a 39
b50559 0:756055ce357a 40 float invSqrt(float x);
b50559 0:756055ce357a 41
b50559 0:756055ce357a 42 //====================================================================================================
b50559 0:756055ce357a 43 // Functions
b50559 0:756055ce357a 44
b50559 0:756055ce357a 45 //---------------------------------------------------------------------------------------------------
b50559 0:756055ce357a 46 // AHRS algorithm update
b50559 0:756055ce357a 47
b50559 0:756055ce357a 48 void MadgwickAHRS::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
b50559 0:756055ce357a 49 float recipNorm;
b50559 0:756055ce357a 50 float s0, s1, s2, s3;
b50559 0:756055ce357a 51 float qDot1, qDot2, qDot3, qDot4;
b50559 0:756055ce357a 52 float hx, hy;
b50559 0:756055ce357a 53 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;
b50559 0:756055ce357a 54
b50559 0:756055ce357a 55 // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
b50559 0:756055ce357a 56 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
b50559 0:756055ce357a 57 MadgwickAHRS::updateIMU(gx, gy, gz, ax, ay, az);
b50559 0:756055ce357a 58 return;
b50559 0:756055ce357a 59 }
b50559 0:756055ce357a 60
b50559 0:756055ce357a 61 // Rate of change of quaternion from gyroscope
b50559 0:756055ce357a 62 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
b50559 0:756055ce357a 63 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
b50559 0:756055ce357a 64 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
b50559 0:756055ce357a 65 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
b50559 0:756055ce357a 66
b50559 0:756055ce357a 67 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
b50559 0:756055ce357a 68 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
b50559 0:756055ce357a 69
b50559 0:756055ce357a 70 // Normalise accelerometer measurement
b50559 0:756055ce357a 71 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
b50559 0:756055ce357a 72 ax *= recipNorm;
b50559 0:756055ce357a 73 ay *= recipNorm;
b50559 0:756055ce357a 74 az *= recipNorm;
b50559 0:756055ce357a 75
b50559 0:756055ce357a 76 // Normalise magnetometer measurement
b50559 0:756055ce357a 77 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
b50559 0:756055ce357a 78 mx *= recipNorm;
b50559 0:756055ce357a 79 my *= recipNorm;
b50559 0:756055ce357a 80 mz *= recipNorm;
b50559 0:756055ce357a 81
b50559 0:756055ce357a 82 // Auxiliary variables to avoid repeated arithmetic
b50559 0:756055ce357a 83 _2q0mx = 2.0f * q0 * mx;
b50559 0:756055ce357a 84 _2q0my = 2.0f * q0 * my;
b50559 0:756055ce357a 85 _2q0mz = 2.0f * q0 * mz;
b50559 0:756055ce357a 86 _2q1mx = 2.0f * q1 * mx;
b50559 0:756055ce357a 87 _2q0 = 2.0f * q0;
b50559 0:756055ce357a 88 _2q1 = 2.0f * q1;
b50559 0:756055ce357a 89 _2q2 = 2.0f * q2;
b50559 0:756055ce357a 90 _2q3 = 2.0f * q3;
b50559 0:756055ce357a 91 _2q0q2 = 2.0f * q0 * q2;
b50559 0:756055ce357a 92 _2q2q3 = 2.0f * q2 * q3;
b50559 0:756055ce357a 93 q0q0 = q0 * q0;
b50559 0:756055ce357a 94 q0q1 = q0 * q1;
b50559 0:756055ce357a 95 q0q2 = q0 * q2;
b50559 0:756055ce357a 96 q0q3 = q0 * q3;
b50559 0:756055ce357a 97 q1q1 = q1 * q1;
b50559 0:756055ce357a 98 q1q2 = q1 * q2;
b50559 0:756055ce357a 99 q1q3 = q1 * q3;
b50559 0:756055ce357a 100 q2q2 = q2 * q2;
b50559 0:756055ce357a 101 q2q3 = q2 * q3;
b50559 0:756055ce357a 102 q3q3 = q3 * q3;
b50559 0:756055ce357a 103
b50559 0:756055ce357a 104 // Reference direction of Earth's magnetic field
b50559 0:756055ce357a 105 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
b50559 0:756055ce357a 106 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
b50559 0:756055ce357a 107 _2bx = sqrt(hx * hx + hy * hy);
b50559 0:756055ce357a 108 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
b50559 0:756055ce357a 109 _4bx = 2.0f * _2bx;
b50559 0:756055ce357a 110 _4bz = 2.0f * _2bz;
b50559 0:756055ce357a 111
b50559 0:756055ce357a 112 // Gradient decent algorithm corrective step
b50559 0:756055ce357a 113 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);
b50559 0:756055ce357a 114 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);
b50559 0:756055ce357a 115 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);
b50559 0:756055ce357a 116 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);
b50559 0:756055ce357a 117 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
b50559 0:756055ce357a 118 s0 *= recipNorm;
b50559 0:756055ce357a 119 s1 *= recipNorm;
b50559 0:756055ce357a 120 s2 *= recipNorm;
b50559 0:756055ce357a 121 s3 *= recipNorm;
b50559 0:756055ce357a 122
b50559 0:756055ce357a 123 // Apply feedback step
b50559 0:756055ce357a 124 qDot1 -= beta * s0;
b50559 0:756055ce357a 125 qDot2 -= beta * s1;
b50559 0:756055ce357a 126 qDot3 -= beta * s2;
b50559 0:756055ce357a 127 qDot4 -= beta * s3;
b50559 0:756055ce357a 128 }
b50559 0:756055ce357a 129
b50559 0:756055ce357a 130 // Integrate rate of change of quaternion to yield quaternion
b50559 0:756055ce357a 131 q0 += qDot1 * (1.0f / sampleFreq);
b50559 0:756055ce357a 132 q1 += qDot2 * (1.0f / sampleFreq);
b50559 0:756055ce357a 133 q2 += qDot3 * (1.0f / sampleFreq);
b50559 0:756055ce357a 134 q3 += qDot4 * (1.0f / sampleFreq);
b50559 0:756055ce357a 135
b50559 0:756055ce357a 136 // Normalise quaternion
b50559 0:756055ce357a 137 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
b50559 0:756055ce357a 138 q0 *= recipNorm;
b50559 0:756055ce357a 139 q1 *= recipNorm;
b50559 0:756055ce357a 140 q2 *= recipNorm;
b50559 0:756055ce357a 141 q3 *= recipNorm;
b50559 0:756055ce357a 142 }
b50559 0:756055ce357a 143
b50559 0:756055ce357a 144 //---------------------------------------------------------------------------------------------------
b50559 0:756055ce357a 145 // IMU algorithm update
b50559 0:756055ce357a 146
b50559 0:756055ce357a 147 void MadgwickAHRS::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
b50559 0:756055ce357a 148 float recipNorm;
b50559 0:756055ce357a 149 float s0, s1, s2, s3;
b50559 0:756055ce357a 150 float qDot1, qDot2, qDot3, qDot4;
b50559 0:756055ce357a 151 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
b50559 0:756055ce357a 152
b50559 0:756055ce357a 153 // Rate of change of quaternion from gyroscope
b50559 0:756055ce357a 154 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
b50559 0:756055ce357a 155 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
b50559 0:756055ce357a 156 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
b50559 0:756055ce357a 157 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
b50559 0:756055ce357a 158
b50559 0:756055ce357a 159 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
b50559 0:756055ce357a 160 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
b50559 0:756055ce357a 161
b50559 0:756055ce357a 162 // Normalise accelerometer measurement
b50559 0:756055ce357a 163 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
b50559 0:756055ce357a 164 ax *= recipNorm;
b50559 0:756055ce357a 165 ay *= recipNorm;
b50559 0:756055ce357a 166 az *= recipNorm;
b50559 0:756055ce357a 167
b50559 0:756055ce357a 168 // Auxiliary variables to avoid repeated arithmetic
b50559 0:756055ce357a 169 _2q0 = 2.0f * q0;
b50559 0:756055ce357a 170 _2q1 = 2.0f * q1;
b50559 0:756055ce357a 171 _2q2 = 2.0f * q2;
b50559 0:756055ce357a 172 _2q3 = 2.0f * q3;
b50559 0:756055ce357a 173 _4q0 = 4.0f * q0;
b50559 0:756055ce357a 174 _4q1 = 4.0f * q1;
b50559 0:756055ce357a 175 _4q2 = 4.0f * q2;
b50559 0:756055ce357a 176 _8q1 = 8.0f * q1;
b50559 0:756055ce357a 177 _8q2 = 8.0f * q2;
b50559 0:756055ce357a 178 q0q0 = q0 * q0;
b50559 0:756055ce357a 179 q1q1 = q1 * q1;
b50559 0:756055ce357a 180 q2q2 = q2 * q2;
b50559 0:756055ce357a 181 q3q3 = q3 * q3;
b50559 0:756055ce357a 182
b50559 0:756055ce357a 183 // Gradient decent algorithm corrective step
b50559 0:756055ce357a 184 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
b50559 0:756055ce357a 185 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
b50559 0:756055ce357a 186 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
b50559 0:756055ce357a 187 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
b50559 0:756055ce357a 188 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
b50559 0:756055ce357a 189 s0 *= recipNorm;
b50559 0:756055ce357a 190 s1 *= recipNorm;
b50559 0:756055ce357a 191 s2 *= recipNorm;
b50559 0:756055ce357a 192 s3 *= recipNorm;
b50559 0:756055ce357a 193
b50559 0:756055ce357a 194 // Apply feedback step
b50559 0:756055ce357a 195 qDot1 -= beta * s0;
b50559 0:756055ce357a 196 qDot2 -= beta * s1;
b50559 0:756055ce357a 197 qDot3 -= beta * s2;
b50559 0:756055ce357a 198 qDot4 -= beta * s3;
b50559 0:756055ce357a 199 }
b50559 0:756055ce357a 200
b50559 0:756055ce357a 201 // Integrate rate of change of quaternion to yield quaternion
b50559 0:756055ce357a 202 q0 += qDot1 * (1.0f / sampleFreq);
b50559 0:756055ce357a 203 q1 += qDot2 * (1.0f / sampleFreq);
b50559 0:756055ce357a 204 q2 += qDot3 * (1.0f / sampleFreq);
b50559 0:756055ce357a 205 q3 += qDot4 * (1.0f / sampleFreq);
b50559 0:756055ce357a 206
b50559 0:756055ce357a 207 // Normalise quaternion
b50559 0:756055ce357a 208 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
b50559 0:756055ce357a 209 q0 *= recipNorm;
b50559 0:756055ce357a 210 q1 *= recipNorm;
b50559 0:756055ce357a 211 q2 *= recipNorm;
b50559 0:756055ce357a 212 q3 *= recipNorm;
b50559 0:756055ce357a 213 }
b50559 0:756055ce357a 214
b50559 0:756055ce357a 215 //---------------------------------------------------------------------------------------------------
b50559 0:756055ce357a 216 // Fast inverse square-root
b50559 0:756055ce357a 217 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
b50559 0:756055ce357a 218
b50559 0:756055ce357a 219 float invSqrt(float x) {
b50559 0:756055ce357a 220 float halfx = 0.5f * x;
b50559 0:756055ce357a 221 float y = x;
b50559 0:756055ce357a 222 long i = *(long*)&y;
b50559 0:756055ce357a 223 i = 0x5f3759df - (i>>1);
b50559 0:756055ce357a 224 y = *(float*)&i;
b50559 0:756055ce357a 225 y = y * (1.5f - (halfx * y * y));
b50559 0:756055ce357a 226 return y;
b50559 0:756055ce357a 227 //return 1.0/sqrt(x);
b50559 0:756055ce357a 228 }
b50559 0:756055ce357a 229
b50559 0:756055ce357a 230
b50559 0:756055ce357a 231 void MadgwickAHRS::getEuler(){
b50559 0:756055ce357a 232
b50559 0:756055ce357a 233 float gx = 2*(q1*q3 - q0*q2);
b50559 0:756055ce357a 234 float gy = 2 * (q0*q1 + q2*q3);
b50559 0:756055ce357a 235 float gz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
b50559 0:756055ce357a 236
b50559 0:756055ce357a 237 //roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
b50559 0:756055ce357a 238 //pitch = asin(2*(q0*q2 - q3*q1));
b50559 0:756055ce357a 239 //yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));;
b50559 0:756055ce357a 240
b50559 0:756055ce357a 241 roll = atan(gy / sqrt(gx*gx + gz*gz));
b50559 0:756055ce357a 242 pitch = atan(gx / sqrt(gy*gy + gz*gz));
b50559 0:756055ce357a 243 yaw = atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0*q0 + 2 * q1 * q1 - 1);
b50559 0:756055ce357a 244
b50559 0:756055ce357a 245 roll = roll*180/PI;
b50559 0:756055ce357a 246 pitch = pitch*180/PI;
b50559 0:756055ce357a 247 yaw = yaw*180/PI;
b50559 0:756055ce357a 248
b50559 0:756055ce357a 249 /*roll = roll*1000;
b50559 0:756055ce357a 250 pitch = pitch*1000;
b50559 0:756055ce357a 251 yaw = yaw*1000;*/
b50559 0:756055ce357a 252
b50559 0:756055ce357a 253 if (ceil(roll) - roll <= .5){
b50559 0:756055ce357a 254 roll = ceil(roll);
b50559 0:756055ce357a 255 }
b50559 0:756055ce357a 256 else{
b50559 0:756055ce357a 257 roll = floor(roll);
b50559 0:756055ce357a 258 }
b50559 0:756055ce357a 259
b50559 0:756055ce357a 260 if (ceil(pitch) - pitch <= .5){
b50559 0:756055ce357a 261 pitch = ceil(pitch);
b50559 0:756055ce357a 262 }
b50559 0:756055ce357a 263 else{
b50559 0:756055ce357a 264 pitch = floor(pitch);
b50559 0:756055ce357a 265 }
b50559 0:756055ce357a 266
b50559 0:756055ce357a 267 if (ceil(yaw) - yaw <= .5){
b50559 0:756055ce357a 268 yaw = ceil(yaw);
b50559 0:756055ce357a 269 }
b50559 0:756055ce357a 270 else{
b50559 0:756055ce357a 271 yaw = floor(yaw);
b50559 0:756055ce357a 272 }
b50559 0:756055ce357a 273
b50559 0:756055ce357a 274 //printf("Roll: %6.2f, Pitch: %6.2f, Yaw: %6.2f\r\n", roll, pitch, yaw);
b50559 0:756055ce357a 275 }
b50559 0:756055ce357a 276
b50559 0:756055ce357a 277 int16_t MadgwickAHRS::getRoll(){
b50559 0:756055ce357a 278 return (int16_t)roll;
b50559 0:756055ce357a 279 }
b50559 0:756055ce357a 280
b50559 0:756055ce357a 281 int16_t MadgwickAHRS::getPitch(){
b50559 0:756055ce357a 282 return (int16_t)pitch;
b50559 0:756055ce357a 283 }
b50559 0:756055ce357a 284
b50559 0:756055ce357a 285 int16_t MadgwickAHRS::getYaw(){
b50559 0:756055ce357a 286 return (int16_t)yaw;
b50559 0:756055ce357a 287 }
b50559 0:756055ce357a 288
b50559 0:756055ce357a 289 //====================================================================================================
b50559 0:756055ce357a 290 // END OF CODE
b50559 0:756055ce357a 291 //====================================================================================================