Justin Gensel / Mbed 2 deprecated FootModuleCode

Code to detect walking and convert to input for video game

Dependencies:   LSM9DS1_Library_cal2 XBee mbed

Fork of FootModule by Justin Gensel

main.cpp@4:43a6ec1af346, 2017-04-30 (annotated)

Committer:
jgensel3
Date:
Sun Apr 30 19:48:22 2017 +0000
Revision:
4:43a6ec1af346
Parent:
3:2d6ff72599f1
Child:
5:c4ae0656a736
Updated LSM9DS1_Library and added better heading detection

Who changed what in which revision?

UserRevisionLine numberNew contents of line
4180_1 0:e693d5bf0a25 1 #include "mbed.h"
4180_1 0:e693d5bf0a25 2 #include "LSM9DS1.h"
jgensel3 3:2d6ff72599f1 3 #include "Wireless.h"
jgensel3 2:ab7b95fb52aa 4 //#include "USBKeyboard.h"
jgensel3 4:43a6ec1af346 5 //#include "MahonyAHRS.h"
4180_1 0:e693d5bf0a25 6 #define PI 3.14159
4180_1 0:e693d5bf0a25 7 // Earth's magnetic field varies by location. Add or subtract
4180_1 0:e693d5bf0a25 8 // a declination to get a more accurate heading. Calculate
4180_1 0:e693d5bf0a25 9 // your's here:
4180_1 0:e693d5bf0a25 10 // http://www.ngdc.noaa.gov/geomag-web/#declination
4180_1 0:e693d5bf0a25 11 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
4180_1 0:e693d5bf0a25 12
jgensel3 2:ab7b95fb52aa 13 DigitalOut led1(LED1);
jgensel3 2:ab7b95fb52aa 14 DigitalOut led2(LED2);
jgensel3 2:ab7b95fb52aa 15 DigitalOut led3(LED3);
jgensel3 2:ab7b95fb52aa 16 DigitalOut led4(LED4);
4180_1 0:e693d5bf0a25 17 Serial pc(USBTX, USBRX);
jgensel3 3:2d6ff72599f1 18 DigitalIn pb1(p17);
jgensel3 4:43a6ec1af346 19 //USBKeyboard keyboard;
4180_1 0:e693d5bf0a25 20 // Calculate pitch, roll, and heading.
4180_1 0:e693d5bf0a25 21 // Pitch/roll calculations taken from this app note:
4180_1 0:e693d5bf0a25 22 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
4180_1 0:e693d5bf0a25 23 // Heading calculations taken from this app note:
4180_1 0:e693d5bf0a25 24 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
jgensel3 4:43a6ec1af346 25 #include <math.h>
jgensel3 4:43a6ec1af346 26
jgensel3 4:43a6ec1af346 27 //---------------------------------------------------------------------------------------------------
jgensel3 4:43a6ec1af346 28 // Definitions
jgensel3 4:43a6ec1af346 29
jgensel3 4:43a6ec1af346 30 #define sampleFreq 952.0f // sample frequency in Hz
jgensel3 4:43a6ec1af346 31 #define betaDef 0.1f // 2 * proportional gain
jgensel3 4:43a6ec1af346 32
jgensel3 4:43a6ec1af346 33 //---------------------------------------------------------------------------------------------------
jgensel3 4:43a6ec1af346 34 // Variable definitions
jgensel3 4:43a6ec1af346 35
jgensel3 4:43a6ec1af346 36 volatile float beta = betaDef; // 2 * proportional gain (Kp)
jgensel3 4:43a6ec1af346 37 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
jgensel3 4:43a6ec1af346 38
jgensel3 4:43a6ec1af346 39 //---------------------------------------------------------------------------------------------------
jgensel3 4:43a6ec1af346 40 // Function declarations
jgensel3 4:43a6ec1af346 41
jgensel3 4:43a6ec1af346 42 float invSqrt(float x);
jgensel3 4:43a6ec1af346 43
jgensel3 4:43a6ec1af346 44 //====================================================================================================
jgensel3 4:43a6ec1af346 45 // Functions
jgensel3 4:43a6ec1af346 46
jgensel3 4:43a6ec1af346 47 //---------------------------------------------------------------------------------------------------
jgensel3 4:43a6ec1af346 48 // AHRS algorithm update
jgensel3 4:43a6ec1af346 49 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
jgensel3 4:43a6ec1af346 50 void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
jgensel3 4:43a6ec1af346 51 {
jgensel3 4:43a6ec1af346 52 float recipNorm;
jgensel3 4:43a6ec1af346 53 float s0, s1, s2, s3;
jgensel3 4:43a6ec1af346 54 float qDot1, qDot2, qDot3, qDot4;
jgensel3 4:43a6ec1af346 55 float hx, hy;
jgensel3 4:43a6ec1af346 56 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;
jgensel3 4:43a6ec1af346 57
jgensel3 4:43a6ec1af346 58 // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
jgensel3 4:43a6ec1af346 59 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
jgensel3 4:43a6ec1af346 60 MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
jgensel3 4:43a6ec1af346 61 return;
jgensel3 4:43a6ec1af346 62 }
jgensel3 4:43a6ec1af346 63
jgensel3 4:43a6ec1af346 64 // Rate of change of quaternion from gyroscope
jgensel3 4:43a6ec1af346 65 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
jgensel3 4:43a6ec1af346 66 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
jgensel3 4:43a6ec1af346 67 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
jgensel3 4:43a6ec1af346 68 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
jgensel3 4:43a6ec1af346 69
jgensel3 4:43a6ec1af346 70 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
jgensel3 4:43a6ec1af346 71 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
jgensel3 4:43a6ec1af346 72
jgensel3 4:43a6ec1af346 73 // Normalise accelerometer measurement
jgensel3 4:43a6ec1af346 74 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
jgensel3 4:43a6ec1af346 75 ax *= recipNorm;
jgensel3 4:43a6ec1af346 76 ay *= recipNorm;
jgensel3 4:43a6ec1af346 77 az *= recipNorm;
jgensel3 4:43a6ec1af346 78
jgensel3 4:43a6ec1af346 79 // Normalise magnetometer measurement
jgensel3 4:43a6ec1af346 80 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
jgensel3 4:43a6ec1af346 81 mx *= recipNorm;
jgensel3 4:43a6ec1af346 82 my *= recipNorm;
jgensel3 4:43a6ec1af346 83 mz *= recipNorm;
jgensel3 4:43a6ec1af346 84
jgensel3 4:43a6ec1af346 85 // Auxiliary variables to avoid repeated arithmetic
jgensel3 4:43a6ec1af346 86 _2q0mx = 2.0f * q0 * mx;
jgensel3 4:43a6ec1af346 87 _2q0my = 2.0f * q0 * my;
jgensel3 4:43a6ec1af346 88 _2q0mz = 2.0f * q0 * mz;
jgensel3 4:43a6ec1af346 89 _2q1mx = 2.0f * q1 * mx;
jgensel3 4:43a6ec1af346 90 _2q0 = 2.0f * q0;
jgensel3 4:43a6ec1af346 91 _2q1 = 2.0f * q1;
jgensel3 4:43a6ec1af346 92 _2q2 = 2.0f * q2;
jgensel3 4:43a6ec1af346 93 _2q3 = 2.0f * q3;
jgensel3 4:43a6ec1af346 94 _2q0q2 = 2.0f * q0 * q2;
jgensel3 4:43a6ec1af346 95 _2q2q3 = 2.0f * q2 * q3;
jgensel3 4:43a6ec1af346 96 q0q0 = q0 * q0;
jgensel3 4:43a6ec1af346 97 q0q1 = q0 * q1;
jgensel3 4:43a6ec1af346 98 q0q2 = q0 * q2;
jgensel3 4:43a6ec1af346 99 q0q3 = q0 * q3;
jgensel3 4:43a6ec1af346 100 q1q1 = q1 * q1;
jgensel3 4:43a6ec1af346 101 q1q2 = q1 * q2;
jgensel3 4:43a6ec1af346 102 q1q3 = q1 * q3;
jgensel3 4:43a6ec1af346 103 q2q2 = q2 * q2;
jgensel3 4:43a6ec1af346 104 q2q3 = q2 * q3;
jgensel3 4:43a6ec1af346 105 q3q3 = q3 * q3;
jgensel3 4:43a6ec1af346 106
jgensel3 4:43a6ec1af346 107 // Reference direction of Earth's magnetic field
jgensel3 4:43a6ec1af346 108 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
jgensel3 4:43a6ec1af346 109 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
jgensel3 4:43a6ec1af346 110 _2bx = sqrt(hx * hx + hy * hy);
jgensel3 4:43a6ec1af346 111 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
jgensel3 4:43a6ec1af346 112 _4bx = 2.0f * _2bx;
jgensel3 4:43a6ec1af346 113 _4bz = 2.0f * _2bz;
jgensel3 4:43a6ec1af346 114
jgensel3 4:43a6ec1af346 115 // Gradient decent algorithm corrective step
jgensel3 4:43a6ec1af346 116 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);
jgensel3 4:43a6ec1af346 117 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);
jgensel3 4:43a6ec1af346 118 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);
jgensel3 4:43a6ec1af346 119 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);
jgensel3 4:43a6ec1af346 120 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
jgensel3 4:43a6ec1af346 121 s0 *= recipNorm;
jgensel3 4:43a6ec1af346 122 s1 *= recipNorm;
jgensel3 4:43a6ec1af346 123 s2 *= recipNorm;
jgensel3 4:43a6ec1af346 124 s3 *= recipNorm;
jgensel3 4:43a6ec1af346 125
jgensel3 4:43a6ec1af346 126 // Apply feedback step
jgensel3 4:43a6ec1af346 127 qDot1 -= beta * s0;
jgensel3 4:43a6ec1af346 128 qDot2 -= beta * s1;
jgensel3 4:43a6ec1af346 129 qDot3 -= beta * s2;
jgensel3 4:43a6ec1af346 130 qDot4 -= beta * s3;
jgensel3 4:43a6ec1af346 131 }
jgensel3 4:43a6ec1af346 132
jgensel3 4:43a6ec1af346 133 // Integrate rate of change of quaternion to yield quaternion
jgensel3 4:43a6ec1af346 134 q0 += qDot1 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 135 q1 += qDot2 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 136 q2 += qDot3 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 137 q3 += qDot4 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 138
jgensel3 4:43a6ec1af346 139 // Normalise quaternion
jgensel3 4:43a6ec1af346 140 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
jgensel3 4:43a6ec1af346 141 q0 *= recipNorm;
jgensel3 4:43a6ec1af346 142 q1 *= recipNorm;
jgensel3 4:43a6ec1af346 143 q2 *= recipNorm;
jgensel3 4:43a6ec1af346 144 q3 *= recipNorm;
jgensel3 4:43a6ec1af346 145 }
jgensel3 4:43a6ec1af346 146
jgensel3 4:43a6ec1af346 147 //---------------------------------------------------------------------------------------------------
jgensel3 4:43a6ec1af346 148 // IMU algorithm update
jgensel3 4:43a6ec1af346 149
jgensel3 4:43a6ec1af346 150 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
jgensel3 4:43a6ec1af346 151 {
jgensel3 4:43a6ec1af346 152 float recipNorm;
jgensel3 4:43a6ec1af346 153 float s0, s1, s2, s3;
jgensel3 4:43a6ec1af346 154 float qDot1, qDot2, qDot3, qDot4;
jgensel3 4:43a6ec1af346 155 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
jgensel3 4:43a6ec1af346 156
jgensel3 4:43a6ec1af346 157 // Rate of change of quaternion from gyroscope
jgensel3 4:43a6ec1af346 158 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
jgensel3 4:43a6ec1af346 159 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
jgensel3 4:43a6ec1af346 160 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
jgensel3 4:43a6ec1af346 161 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
jgensel3 4:43a6ec1af346 162
jgensel3 4:43a6ec1af346 163 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
jgensel3 4:43a6ec1af346 164 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
jgensel3 4:43a6ec1af346 165
jgensel3 4:43a6ec1af346 166 // Normalise accelerometer measurement
jgensel3 4:43a6ec1af346 167 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
jgensel3 4:43a6ec1af346 168 ax *= recipNorm;
jgensel3 4:43a6ec1af346 169 ay *= recipNorm;
jgensel3 4:43a6ec1af346 170 az *= recipNorm;
jgensel3 4:43a6ec1af346 171
jgensel3 4:43a6ec1af346 172 // Auxiliary variables to avoid repeated arithmetic
jgensel3 4:43a6ec1af346 173 _2q0 = 2.0f * q0;
jgensel3 4:43a6ec1af346 174 _2q1 = 2.0f * q1;
jgensel3 4:43a6ec1af346 175 _2q2 = 2.0f * q2;
jgensel3 4:43a6ec1af346 176 _2q3 = 2.0f * q3;
jgensel3 4:43a6ec1af346 177 _4q0 = 4.0f * q0;
jgensel3 4:43a6ec1af346 178 _4q1 = 4.0f * q1;
jgensel3 4:43a6ec1af346 179 _4q2 = 4.0f * q2;
jgensel3 4:43a6ec1af346 180 _8q1 = 8.0f * q1;
jgensel3 4:43a6ec1af346 181 _8q2 = 8.0f * q2;
jgensel3 4:43a6ec1af346 182 q0q0 = q0 * q0;
jgensel3 4:43a6ec1af346 183 q1q1 = q1 * q1;
jgensel3 4:43a6ec1af346 184 q2q2 = q2 * q2;
jgensel3 4:43a6ec1af346 185 q3q3 = q3 * q3;
jgensel3 4:43a6ec1af346 186
jgensel3 4:43a6ec1af346 187 // Gradient decent algorithm corrective step
jgensel3 4:43a6ec1af346 188 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
jgensel3 4:43a6ec1af346 189 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
jgensel3 4:43a6ec1af346 190 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
jgensel3 4:43a6ec1af346 191 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
jgensel3 4:43a6ec1af346 192 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
jgensel3 4:43a6ec1af346 193 s0 *= recipNorm;
jgensel3 4:43a6ec1af346 194 s1 *= recipNorm;
jgensel3 4:43a6ec1af346 195 s2 *= recipNorm;
jgensel3 4:43a6ec1af346 196 s3 *= recipNorm;
jgensel3 4:43a6ec1af346 197
jgensel3 4:43a6ec1af346 198 // Apply feedback step
jgensel3 4:43a6ec1af346 199 qDot1 -= beta * s0;
jgensel3 4:43a6ec1af346 200 qDot2 -= beta * s1;
jgensel3 4:43a6ec1af346 201 qDot3 -= beta * s2;
jgensel3 4:43a6ec1af346 202 qDot4 -= beta * s3;
jgensel3 4:43a6ec1af346 203 }
jgensel3 4:43a6ec1af346 204
jgensel3 4:43a6ec1af346 205 // Integrate rate of change of quaternion to yield quaternion
jgensel3 4:43a6ec1af346 206 q0 += qDot1 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 207 q1 += qDot2 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 208 q2 += qDot3 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 209 q3 += qDot4 * (1.0f / sampleFreq);
jgensel3 4:43a6ec1af346 210
jgensel3 4:43a6ec1af346 211 // Normalise quaternion
jgensel3 4:43a6ec1af346 212 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
jgensel3 4:43a6ec1af346 213 q0 *= recipNorm;
jgensel3 4:43a6ec1af346 214 q1 *= recipNorm;
jgensel3 4:43a6ec1af346 215 q2 *= recipNorm;
jgensel3 4:43a6ec1af346 216 q3 *= recipNorm;
jgensel3 4:43a6ec1af346 217 }
jgensel3 4:43a6ec1af346 218
jgensel3 4:43a6ec1af346 219 //---------------------------------------------------------------------------------------------------
jgensel3 4:43a6ec1af346 220 // Fast inverse square-root
jgensel3 4:43a6ec1af346 221 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
jgensel3 4:43a6ec1af346 222
jgensel3 4:43a6ec1af346 223 float invSqrt(float x)
jgensel3 4:43a6ec1af346 224 {
jgensel3 4:43a6ec1af346 225 float halfx = 0.5f * x;
jgensel3 4:43a6ec1af346 226 float y = x;
jgensel3 4:43a6ec1af346 227 long i = *(long*)&y;
jgensel3 4:43a6ec1af346 228 i = 0x5f3759df - (i>>1);
jgensel3 4:43a6ec1af346 229 y = *(float*)&i;
jgensel3 4:43a6ec1af346 230 y = y * (1.5f - (halfx * y * y));
jgensel3 4:43a6ec1af346 231 return y;
jgensel3 4:43a6ec1af346 232 }
jgensel3 4:43a6ec1af346 233
jgensel3 4:43a6ec1af346 234 //====================================================================================================
jgensel3 4:43a6ec1af346 235 // END OF CODE
jgensel3 4:43a6ec1af346 236 //====================================================================================================
jgensel3 2:ab7b95fb52aa 237
jgensel3 2:ab7b95fb52aa 238
jgensel3 4:43a6ec1af346 239
jgensel3 2:ab7b95fb52aa 240
jgensel3 4:43a6ec1af346 241
jgensel3 2:ab7b95fb52aa 242 float correctHeading(float currHeading, float forward)
jgensel3 2:ab7b95fb52aa 243 {
jgensel3 1:705baf131710 244 float newHeading = currHeading - forward;
jgensel3 1:705baf131710 245 if(newHeading < 0) newHeading = 360 + newHeading;
jgensel3 1:705baf131710 246 return newHeading;
jgensel3 1:705baf131710 247 }
jgensel3 1:705baf131710 248
jgensel3 1:705baf131710 249 float printAttitude(float ax, float ay, float az, float mx, float my, float mz)
4180_1 0:e693d5bf0a25 250 {
4180_1 0:e693d5bf0a25 251 float roll = atan2(ay, az);
4180_1 0:e693d5bf0a25 252 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
4180_1 0:e693d5bf0a25 253 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
4180_1 0:e693d5bf0a25 254 mx = -mx;
4180_1 0:e693d5bf0a25 255 float heading;
4180_1 0:e693d5bf0a25 256 if (my == 0.0)
4180_1 0:e693d5bf0a25 257 heading = (mx < 0.0) ? 180.0 : 0.0;
4180_1 0:e693d5bf0a25 258 else
4180_1 0:e693d5bf0a25 259 heading = atan2(mx, my)*360.0/(2.0*PI);
4180_1 0:e693d5bf0a25 260 //pc.printf("heading atan=%f \n\r",heading);
4180_1 0:e693d5bf0a25 261 heading -= DECLINATION; //correct for geo location
4180_1 0:e693d5bf0a25 262 if(heading>180.0) heading = heading - 360.0;
4180_1 0:e693d5bf0a25 263 else if(heading<-180.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 264 else if(heading<0.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 265
4180_1 0:e693d5bf0a25 266
4180_1 0:e693d5bf0a25 267 // Convert everything from radians to degrees:
4180_1 0:e693d5bf0a25 268 //heading *= 180.0 / PI;
4180_1 0:e693d5bf0a25 269 pitch *= 180.0 / PI;
4180_1 0:e693d5bf0a25 270 roll *= 180.0 / PI;
4180_1 0:e693d5bf0a25 271
jgensel3 1:705baf131710 272 //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
jgensel3 1:705baf131710 273 //pc.printf("Magnetic Heading: %f degress\n\r",heading);
jgensel3 4:43a6ec1af346 274 return abs(heading);
jgensel3 4:43a6ec1af346 275 }
jgensel3 4:43a6ec1af346 276
jgensel3 4:43a6ec1af346 277 bool isWalking = false;
jgensel3 4:43a6ec1af346 278
jgensel3 4:43a6ec1af346 279 Ticker walkingTimer;
jgensel3 4:43a6ec1af346 280 Ticker resetStart;
jgensel3 4:43a6ec1af346 281 WirelessModule wireless(p9, p10, FOOT_STEP);
jgensel3 4:43a6ec1af346 282 float ax ;
jgensel3 4:43a6ec1af346 283 float ay ;
jgensel3 4:43a6ec1af346 284 float az ;
jgensel3 4:43a6ec1af346 285 float gx ;
jgensel3 4:43a6ec1af346 286 float gy ;
jgensel3 4:43a6ec1af346 287 float gz ;
jgensel3 4:43a6ec1af346 288 float mx ;
jgensel3 4:43a6ec1af346 289 float my ;
jgensel3 4:43a6ec1af346 290 float mz ;
jgensel3 4:43a6ec1af346 291 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
jgensel3 4:43a6ec1af346 292
jgensel3 4:43a6ec1af346 293 void printStop()
jgensel3 4:43a6ec1af346 294 {
jgensel3 4:43a6ec1af346 295 // pc.printf("stop\n\r");
jgensel3 4:43a6ec1af346 296 wireless.sendDirection(DIR_NONE);
jgensel3 4:43a6ec1af346 297 isWalking = false;
jgensel3 4:43a6ec1af346 298 }
jgensel3 4:43a6ec1af346 299 void resetForward()
jgensel3 4:43a6ec1af346 300 {
jgensel3 4:43a6ec1af346 301 q0 = 1.0f;
jgensel3 4:43a6ec1af346 302 q1 = 0.0f;
jgensel3 4:43a6ec1af346 303 q2 = 0.0f;
jgensel3 4:43a6ec1af346 304 q3 = 0.0f;
jgensel3 4:43a6ec1af346 305 MadgwickAHRSupdate(IMU.calcGyro(gx), IMU.calcGyro(gy), IMU.calcGyro(gz), IMU.calcAccel(ax), IMU.calcAccel(ay), IMU.calcAccel(az), IMU.calcMag(mx), IMU.calcMag(my), IMU.calcMag(mz));
4180_1 0:e693d5bf0a25 306 }
4180_1 0:e693d5bf0a25 307
4180_1 0:e693d5bf0a25 308
4180_1 0:e693d5bf0a25 309
4180_1 0:e693d5bf0a25 310
4180_1 0:e693d5bf0a25 311 int main()
4180_1 0:e693d5bf0a25 312 {
4180_1 0:e693d5bf0a25 313 //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
jgensel3 4:43a6ec1af346 314
jgensel3 1:705baf131710 315 pb1.mode(PullUp);
4180_1 0:e693d5bf0a25 316 IMU.begin();
jgensel3 1:705baf131710 317 float forward;
4180_1 0:e693d5bf0a25 318 if (!IMU.begin()) {
4180_1 0:e693d5bf0a25 319 pc.printf("Failed to communicate with LSM9DS1.\n");
4180_1 0:e693d5bf0a25 320 }
jgensel3 4:43a6ec1af346 321 led4 = 1;
4180_1 0:e693d5bf0a25 322 IMU.calibrate(1);
jgensel3 4:43a6ec1af346 323 led4 = 0;
jgensel3 2:ab7b95fb52aa 324 wait(0.5);
jgensel3 4:43a6ec1af346 325 led1 = 1;
jgensel3 2:ab7b95fb52aa 326 led4 = 1;
4180_1 0:e693d5bf0a25 327 IMU.calibrateMag(0);
jgensel3 2:ab7b95fb52aa 328 led4 = 0;
jgensel3 2:ab7b95fb52aa 329 led2 = 1;
jgensel3 1:705baf131710 330 pc.printf("Press button to set forward direction");
jgensel3 2:ab7b95fb52aa 331 while(pb1 == 1) {
jgensel3 1:705baf131710 332 IMU.readMag();
jgensel3 1:705baf131710 333 IMU.readAccel();
jgensel3 4:43a6ec1af346 334 ax = IMU.calcAccel(IMU.ax);
jgensel3 4:43a6ec1af346 335 ay = IMU.calcAccel(IMU.ay);
jgensel3 4:43a6ec1af346 336 az = IMU.calcAccel(IMU.az);
jgensel3 4:43a6ec1af346 337 gx = IMU.calcGyro(IMU.gx);
jgensel3 4:43a6ec1af346 338 gy = IMU.calcGyro(IMU.gy);
jgensel3 4:43a6ec1af346 339 gz = IMU.calcGyro(IMU.gz);
jgensel3 4:43a6ec1af346 340 mx = IMU.calcMag(IMU.mx);
jgensel3 4:43a6ec1af346 341 my = IMU.calcMag(IMU.my);
jgensel3 4:43a6ec1af346 342 mz = IMU.calcMag(IMU.mz);
jgensel3 1:705baf131710 343 forward = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jgensel3 2:ab7b95fb52aa 344 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));;
jgensel3 2:ab7b95fb52aa 345 }
jgensel3 4:43a6ec1af346 346 led3 = 1;
jgensel3 4:43a6ec1af346 347 resetStart.attach(resetForward, 0.1);
4180_1 0:e693d5bf0a25 348 while(1) {
4180_1 0:e693d5bf0a25 349 while(!IMU.tempAvailable());
4180_1 0:e693d5bf0a25 350 IMU.readTemp();
4180_1 0:e693d5bf0a25 351 while(!IMU.magAvailable(X_AXIS));
4180_1 0:e693d5bf0a25 352 IMU.readMag();
4180_1 0:e693d5bf0a25 353 while(!IMU.accelAvailable());
4180_1 0:e693d5bf0a25 354 IMU.readAccel();
4180_1 0:e693d5bf0a25 355 while(!IMU.gyroAvailable());
4180_1 0:e693d5bf0a25 356 IMU.readGyro();
jgensel3 2:ab7b95fb52aa 357
jgensel3 4:43a6ec1af346 358 MadgwickAHRSupdate(IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz), IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jgensel3 4:43a6ec1af346 359 float Yaw_m=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1)*180/PI;
jgensel3 4:43a6ec1af346 360 float Pitch_m=-1*asin(2*q1*q3+2*q0*q2)*180/PI;
jgensel3 4:43a6ec1af346 361 float Roll_m=atan2(2*q2*q3-2*q0*q1,2*q0*q0+2*q3*q3-1)*180/PI;
jgensel3 2:ab7b95fb52aa 362
jgensel3 4:43a6ec1af346 363 if( Yaw_m < 0 ) Yaw_m += 360.0;
jgensel3 4:43a6ec1af346 364 //pc.printf("yaw: %f\n\r", Yaw_m);
jgensel3 4:43a6ec1af346 365 //pc.printf("Yaw: %f\n\r Roll: %f\n\r Pitch: %f\n\n\n\r", Yaw_m, Roll_m, Pitch_m);
jgensel3 2:ab7b95fb52aa 366
jgensel3 4:43a6ec1af346 367 if(abs(IMU.calcGyro(IMU.gy)) > 100) {
jgensel3 2:ab7b95fb52aa 368
jgensel3 2:ab7b95fb52aa 369 //Calculate heading relative to forward direction
jgensel3 1:705baf131710 370 float currHeading = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jgensel3 4:43a6ec1af346 371 currHeading = correctHeading(currHeading, forward);
jgensel3 4:43a6ec1af346 372 pc.printf("heading: %f\n\r", currHeading);
jgensel3 4:43a6ec1af346 373 //pc.printf("corrected heading: %f\n\r", currHeading);
jgensel3 2:ab7b95fb52aa 374 //Start timeout to detect when stopped walking
jgensel3 2:ab7b95fb52aa 375 walkingTimer.attach(printStop, 0.3);
jgensel3 2:ab7b95fb52aa 376
jgensel3 2:ab7b95fb52aa 377 //Detect direction and send command to main mbed
jgensel3 4:43a6ec1af346 378 if((currHeading > 225 && currHeading < 315) && !isWalking) {
jgensel3 4:43a6ec1af346 379 pc.printf("left\n\r");
jgensel3 3:2d6ff72599f1 380 wireless.sendDirection(DIR_LEFT);
jgensel3 2:ab7b95fb52aa 381
jgensel3 2:ab7b95fb52aa 382 isWalking = true;
jgensel3 4:43a6ec1af346 383 } else if((currHeading > 45 && currHeading < 135) && !isWalking) {
jgensel3 4:43a6ec1af346 384 pc.printf("right\n\r");
jgensel3 4:43a6ec1af346 385 wireless.sendDirection(DIR_RIGHT);
jgensel3 2:ab7b95fb52aa 386 isWalking = true;
jgensel3 4:43a6ec1af346 387 } else if((currHeading > 135 && currHeading < 225) && !isWalking) {
jgensel3 4:43a6ec1af346 388 pc.printf("down\n\r");
jgensel3 3:2d6ff72599f1 389 wireless.sendDirection(DIR_DOWN);
jgensel3 2:ab7b95fb52aa 390 isWalking = true;
jgensel3 4:43a6ec1af346 391 } else if((currHeading > 315 || currHeading < 45) && !isWalking) {
jgensel3 4:43a6ec1af346 392 pc.printf("up\n\r");
jgensel3 4:43a6ec1af346 393 wireless.sendDirection(DIR_UP);
jgensel3 2:ab7b95fb52aa 394
jgensel3 2:ab7b95fb52aa 395 isWalking = true;
jgensel3 1:705baf131710 396 }
jgensel3 1:705baf131710 397 }
4180_1 0:e693d5bf0a25 398 }
4180_1 0:e693d5bf0a25 399 }
4180_1 0:e693d5bf0a25 400