Justin Gensel / Mbed 2 deprecated FootSensor

Checks to see when someone is walking and in which direciton

Dependencies:   LSM9DS1_Library_cal mbed

Fork of LSM9DS1_Demo_wCal by jim hamblen

main.cpp@1:705baf131710, 2017-04-14 (annotated)

Committer:
jgensel3
Date:
Fri Apr 14 00:29:47 2017 +0000
Revision:
1:705baf131710
Parent:
0:e693d5bf0a25 
Direction checking code

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 1:705baf131710 3 #include "USBJoystick.h"
4180_1 0:e693d5bf0a25 4 #define PI 3.14159
4180_1 0:e693d5bf0a25 5 // Earth's magnetic field varies by location. Add or subtract
4180_1 0:e693d5bf0a25 6 // a declination to get a more accurate heading. Calculate
4180_1 0:e693d5bf0a25 7 // your's here:
4180_1 0:e693d5bf0a25 8 // http://www.ngdc.noaa.gov/geomag-web/#declination
4180_1 0:e693d5bf0a25 9 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
4180_1 0:e693d5bf0a25 10
4180_1 0:e693d5bf0a25 11 DigitalOut myled(LED1);
4180_1 0:e693d5bf0a25 12 Serial pc(USBTX, USBRX);
jgensel3 1:705baf131710 13 DigitalIn pb1(p17);
4180_1 0:e693d5bf0a25 14 // Calculate pitch, roll, and heading.
4180_1 0:e693d5bf0a25 15 // Pitch/roll calculations taken from this app note:
4180_1 0:e693d5bf0a25 16 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
4180_1 0:e693d5bf0a25 17 // Heading calculations taken from this app note:
4180_1 0:e693d5bf0a25 18 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
jgensel3 1:705baf131710 19 float correctHeading(float currHeading, float forward){
jgensel3 1:705baf131710 20 float newHeading = currHeading - forward;
jgensel3 1:705baf131710 21 if(newHeading < 0) newHeading = 360 + newHeading;
jgensel3 1:705baf131710 22 return newHeading;
jgensel3 1:705baf131710 23 }
jgensel3 1:705baf131710 24
jgensel3 1:705baf131710 25 float printAttitude(float ax, float ay, float az, float mx, float my, float mz)
4180_1 0:e693d5bf0a25 26 {
4180_1 0:e693d5bf0a25 27 float roll = atan2(ay, az);
4180_1 0:e693d5bf0a25 28 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
4180_1 0:e693d5bf0a25 29 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
4180_1 0:e693d5bf0a25 30 mx = -mx;
4180_1 0:e693d5bf0a25 31 float heading;
4180_1 0:e693d5bf0a25 32 if (my == 0.0)
4180_1 0:e693d5bf0a25 33 heading = (mx < 0.0) ? 180.0 : 0.0;
4180_1 0:e693d5bf0a25 34 else
4180_1 0:e693d5bf0a25 35 heading = atan2(mx, my)*360.0/(2.0*PI);
4180_1 0:e693d5bf0a25 36 //pc.printf("heading atan=%f \n\r",heading);
4180_1 0:e693d5bf0a25 37 heading -= DECLINATION; //correct for geo location
4180_1 0:e693d5bf0a25 38 if(heading>180.0) heading = heading - 360.0;
4180_1 0:e693d5bf0a25 39 else if(heading<-180.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 40 else if(heading<0.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 41
4180_1 0:e693d5bf0a25 42
4180_1 0:e693d5bf0a25 43 // Convert everything from radians to degrees:
4180_1 0:e693d5bf0a25 44 //heading *= 180.0 / PI;
4180_1 0:e693d5bf0a25 45 pitch *= 180.0 / PI;
4180_1 0:e693d5bf0a25 46 roll *= 180.0 / PI;
4180_1 0:e693d5bf0a25 47
jgensel3 1:705baf131710 48 //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
jgensel3 1:705baf131710 49 //pc.printf("Magnetic Heading: %f degress\n\r",heading);
jgensel3 1:705baf131710 50 return heading;
4180_1 0:e693d5bf0a25 51 }
4180_1 0:e693d5bf0a25 52
4180_1 0:e693d5bf0a25 53
4180_1 0:e693d5bf0a25 54
4180_1 0:e693d5bf0a25 55
4180_1 0:e693d5bf0a25 56 int main()
4180_1 0:e693d5bf0a25 57 {
4180_1 0:e693d5bf0a25 58 //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
4180_1 0:e693d5bf0a25 59 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
jgensel3 1:705baf131710 60 pb1.mode(PullUp);
4180_1 0:e693d5bf0a25 61 IMU.begin();
jgensel3 1:705baf131710 62 float forward;
4180_1 0:e693d5bf0a25 63 if (!IMU.begin()) {
4180_1 0:e693d5bf0a25 64 pc.printf("Failed to communicate with LSM9DS1.\n");
4180_1 0:e693d5bf0a25 65 }
4180_1 0:e693d5bf0a25 66 IMU.calibrate(1);
4180_1 0:e693d5bf0a25 67 IMU.calibrateMag(0);
jgensel3 1:705baf131710 68 pc.printf("Press button to set forward direction");
jgensel3 1:705baf131710 69 while(pb1 == 1){
jgensel3 1:705baf131710 70 IMU.readMag();
jgensel3 1:705baf131710 71 IMU.readAccel();
jgensel3 1:705baf131710 72 forward = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jgensel3 1:705baf131710 73 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));;
jgensel3 1:705baf131710 74 }
4180_1 0:e693d5bf0a25 75 while(1) {
4180_1 0:e693d5bf0a25 76 while(!IMU.tempAvailable());
4180_1 0:e693d5bf0a25 77 IMU.readTemp();
4180_1 0:e693d5bf0a25 78 while(!IMU.magAvailable(X_AXIS));
4180_1 0:e693d5bf0a25 79 IMU.readMag();
4180_1 0:e693d5bf0a25 80 while(!IMU.accelAvailable());
4180_1 0:e693d5bf0a25 81 IMU.readAccel();
4180_1 0:e693d5bf0a25 82 while(!IMU.gyroAvailable());
4180_1 0:e693d5bf0a25 83 IMU.readGyro();
jgensel3 1:705baf131710 84 /*pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
4180_1 0:e693d5bf0a25 85 pc.printf(" X axis Y axis Z axis\n\r");
4180_1 0:e693d5bf0a25 86 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
4180_1 0:e693d5bf0a25 87 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
4180_1 0:e693d5bf0a25 88 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
4180_1 0:e693d5bf0a25 89 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jgensel3 1:705baf131710 90 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));*/
jgensel3 1:705baf131710 91 if(abs(IMU.calcGyro(IMU.gy)) > 100){
jgensel3 1:705baf131710 92 // pc.printf("Forward:%f\n\r",forward);
jgensel3 1:705baf131710 93 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 1:705baf131710 94 // pc.printf("RawHeading:%f\n\r",currHeading);
jgensel3 1:705baf131710 95 currHeading = correctHeading(currHeading, forward) + 45;
jgensel3 1:705baf131710 96 // pc.printf("CorrectedHeading:%f\n\n\n\r",currHeading);
jgensel3 1:705baf131710 97 if(currHeading > 360 || currHeading < 90){
jgensel3 1:705baf131710 98 pc.printf("Forward\n\r");
jgensel3 1:705baf131710 99 }
jgensel3 1:705baf131710 100 else if(currHeading > 90 && currHeading < 180){
jgensel3 1:705baf131710 101 pc.printf("Right\n\r");
jgensel3 1:705baf131710 102 }
jgensel3 1:705baf131710 103 else if(currHeading > 180 && currHeading < 270){
jgensel3 1:705baf131710 104 pc.printf("Backwards\n\r");
jgensel3 1:705baf131710 105 }
jgensel3 1:705baf131710 106 else{
jgensel3 1:705baf131710 107 pc.printf("Left\n\r");
jgensel3 1:705baf131710 108 }
jgensel3 1:705baf131710 109 }
4180_1 0:e693d5bf0a25 110 }
4180_1 0:e693d5bf0a25 111 }
4180_1 0:e693d5bf0a25 112