Saad Khan / Mbed 2 deprecated SpeedDetectionSystem

Code for mbed running GPS and IMU to calculate speed and notify if too high or acceleration too high. By Ibrahim Khan and Saad Khan

Dependencies:   4DGL-uLCD-SE GPS LSM9DS1_Library_cal mbed wave_player SDFileSystem

By Ibrahim Khan and Saad Khan. Part of ECE4180 final project.

main.cpp@2:95d21fb43f6f, 2016-12-11 (annotated)

Committer:
gotmilk
Date:
Sun Dec 11 03:50:17 2016 +0000
Revision:
2:95d21fb43f6f
Parent:
1:40becbadfee3 
one mbed - working version alhamdulillah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gotmilk 0:c0e8fdd254a9 1 #include "mbed.h"
gotmilk 0:c0e8fdd254a9 2 #include "math.h"
gotmilk 0:c0e8fdd254a9 3 #include "GPS.h"
gotmilk 0:c0e8fdd254a9 4 #include "uLCD_4DGL.h"
gotmilk 0:c0e8fdd254a9 5 #include "LSM9DS1.h"
gotmilk 2:95d21fb43f6f 6 #include "SDFileSystem.h"
gotmilk 2:95d21fb43f6f 7 #include "wave_player.h"
gotmilk 0:c0e8fdd254a9 8
gotmilk 0:c0e8fdd254a9 9 #define M_PI 3.141592653589793238462
gotmilk 0:c0e8fdd254a9 10
gotmilk 0:c0e8fdd254a9 11 Serial pc(USBTX, USBRX);
gotmilk 0:c0e8fdd254a9 12 GPS gps(p13, p14);
gotmilk 2:95d21fb43f6f 13 DigitalOut led2(LED1);
gotmilk 0:c0e8fdd254a9 14 uLCD_4DGL uLCD(p28,p27,p30);
gotmilk 0:c0e8fdd254a9 15 Timer t;
gotmilk 2:95d21fb43f6f 16 SDFileSystem sd(p5, p6, p7, p8, "sd");
gotmilk 2:95d21fb43f6f 17 AnalogOut DACout(p18);
gotmilk 2:95d21fb43f6f 18 wave_player waver(&DACout);
gotmilk 0:c0e8fdd254a9 19
gotmilk 0:c0e8fdd254a9 20
gotmilk 0:c0e8fdd254a9 21 float oldlat, oldlong, oldtime;
gotmilk 0:c0e8fdd254a9 22
gotmilk 0:c0e8fdd254a9 23 float distance(float oldlat, float oldlong, float newlat, float newlong)
gotmilk 0:c0e8fdd254a9 24 {
gotmilk 0:c0e8fdd254a9 25 oldlat = oldlat * M_PI / 180.0;
gotmilk 0:c0e8fdd254a9 26 oldlong = oldlong * M_PI / 180.0;
gotmilk 0:c0e8fdd254a9 27 newlat = newlat * M_PI / 180.0;
gotmilk 0:c0e8fdd254a9 28 newlong = newlong * M_PI / 180.0;
gotmilk 0:c0e8fdd254a9 29 const float r = 6378100;
gotmilk 0:c0e8fdd254a9 30 float rho1 = r * cos(oldlat);
gotmilk 0:c0e8fdd254a9 31 float z1 = r * sin(oldlat);
gotmilk 0:c0e8fdd254a9 32 float x1 = rho1 * cos(oldlong);
gotmilk 0:c0e8fdd254a9 33 float y1 = rho1 * sin(oldlong);
gotmilk 0:c0e8fdd254a9 34
gotmilk 0:c0e8fdd254a9 35 float rho2 = r * cos(newlat);
gotmilk 0:c0e8fdd254a9 36 float z2 = r * sin(newlat);
gotmilk 0:c0e8fdd254a9 37 float x2 = rho2 * cos(newlong);
gotmilk 0:c0e8fdd254a9 38 float y2 = rho2 * sin(newlong);
gotmilk 0:c0e8fdd254a9 39
gotmilk 0:c0e8fdd254a9 40 float dot = (x1 * x2 + y1 * y2 + z1 * z2);
gotmilk 0:c0e8fdd254a9 41 float cos_theta = dot / (r * r);
gotmilk 0:c0e8fdd254a9 42 float theta = acos(cos_theta);
gotmilk 0:c0e8fdd254a9 43 return r * theta;
gotmilk 0:c0e8fdd254a9 44 }
gotmilk 0:c0e8fdd254a9 45 //Algorithm reference: http://www.ridgesolutions.ie/index.php/2013/11/14/algorithm-to-calculate-speed-from-two-gps-latitude-and-longitude-points-and-time-difference/
gotmilk 0:c0e8fdd254a9 46
gotmilk 0:c0e8fdd254a9 47 void checkgps()
gotmilk 0:c0e8fdd254a9 48 {
gotmilk 0:c0e8fdd254a9 49 uLCD.locate(0,1);
gotmilk 2:95d21fb43f6f 50 if(gps.sample()) {
gotmilk 2:95d21fb43f6f 51 time_t seconds = time(NULL);
gotmilk 0:c0e8fdd254a9 52 float newtime = t.read();
gotmilk 0:c0e8fdd254a9 53 float newlat = gps.latitude;
gotmilk 0:c0e8fdd254a9 54 float newlong = gps.longitude;
gotmilk 0:c0e8fdd254a9 55 float dist = distance(oldlat, oldlong, newlat, newlong);
gotmilk 0:c0e8fdd254a9 56 float timediff = newtime - oldtime;
gotmilk 0:c0e8fdd254a9 57 float speed = dist/timediff; //meters per second
gotmilk 0:c0e8fdd254a9 58 float speed_mph = (speed * 2.23);
gotmilk 0:c0e8fdd254a9 59 uLCD.printf("Longitude: %f degrees\n\rLatitude: %f degrees\n\r", newlong, newlat);
gotmilk 0:c0e8fdd254a9 60 uLCD.printf("Distance: %f\n\r", dist);
gotmilk 0:c0e8fdd254a9 61 uLCD.printf("D-time: %f\n\r", timediff);
gotmilk 0:c0e8fdd254a9 62 uLCD.printf("Speed in M/ph: %f\n\r", speed_mph);
gotmilk 0:c0e8fdd254a9 63 if (speed_mph > 50 && speed_mph < 100) {
gotmilk 2:95d21fb43f6f 64 FILE *wave_file;
gotmilk 2:95d21fb43f6f 65 wave_file=fopen("/sd/fast.wav","r");
gotmilk 2:95d21fb43f6f 66 waver.play(wave_file);
gotmilk 2:95d21fb43f6f 67 fclose(wave_file);
gotmilk 2:95d21fb43f6f 68 FILE *fp = fopen("/sd/speedlog.txt", "w");
gotmilk 2:95d21fb43f6f 69 if(fp == NULL) {
gotmilk 2:95d21fb43f6f 70 uLCD.printf("Can't open file");
gotmilk 2:95d21fb43f6f 71 }
gotmilk 2:95d21fb43f6f 72 fprintf(fp, "Driver drove above limit on %s\n", ctime(&seconds));
gotmilk 2:95d21fb43f6f 73 fclose(fp);
gotmilk 0:c0e8fdd254a9 74 }
gotmilk 0:c0e8fdd254a9 75 oldlat = newlat;
gotmilk 0:c0e8fdd254a9 76 oldlong = newlong;
gotmilk 0:c0e8fdd254a9 77 oldtime = newtime;
gotmilk 0:c0e8fdd254a9 78 } else {
gotmilk 0:c0e8fdd254a9 79 uLCD.printf("Oh Dear! No lock :(\n\r");
gotmilk 0:c0e8fdd254a9 80 }
gotmilk 0:c0e8fdd254a9 81 }
gotmilk 0:c0e8fdd254a9 82
gotmilk 0:c0e8fdd254a9 83 int main()
gotmilk 0:c0e8fdd254a9 84 {
gotmilk 2:95d21fb43f6f 85 time_t seconds = time(NULL);
gotmilk 2:95d21fb43f6f 86 uLCD.cls();
gotmilk 0:c0e8fdd254a9 87 uLCD.printf("start main\r\n");
gotmilk 0:c0e8fdd254a9 88 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
gotmilk 0:c0e8fdd254a9 89 IMU.begin();
gotmilk 0:c0e8fdd254a9 90 if (!IMU.begin()) {
gotmilk 0:c0e8fdd254a9 91 uLCD.printf("Failed to communicate with LSM9DS1.\n");
gotmilk 0:c0e8fdd254a9 92 }
gotmilk 0:c0e8fdd254a9 93 IMU.calibrate(1);
gotmilk 0:c0e8fdd254a9 94 int counter, display = 0;
gotmilk 0:c0e8fdd254a9 95 t.start();
gotmilk 0:c0e8fdd254a9 96 t.reset();
gotmilk 0:c0e8fdd254a9 97 float newtime, accel = 0;
gotmilk 0:c0e8fdd254a9 98 oldtime = t.read();
gotmilk 0:c0e8fdd254a9 99 oldlat = gps.latitude;
gotmilk 0:c0e8fdd254a9 100 oldlong = gps.longitude;
gotmilk 2:95d21fb43f6f 101 //checkgps();
gotmilk 0:c0e8fdd254a9 102 while(1) {
gotmilk 0:c0e8fdd254a9 103 newtime = t.read();
gotmilk 0:c0e8fdd254a9 104 if ((newtime-oldtime)>300) {
gotmilk 0:c0e8fdd254a9 105 checkgps();
gotmilk 0:c0e8fdd254a9 106 t.reset();
gotmilk 0:c0e8fdd254a9 107 oldtime = t.read();
gotmilk 0:c0e8fdd254a9 108 } else {
gotmilk 0:c0e8fdd254a9 109 while(!IMU.accelAvailable());
gotmilk 0:c0e8fdd254a9 110 IMU.readAccel();
gotmilk 0:c0e8fdd254a9 111 display++;
gotmilk 0:c0e8fdd254a9 112 if (display%100==0) {
gotmilk 2:95d21fb43f6f 113 uLCD.locate(0,10);
gotmilk 0:c0e8fdd254a9 114 accel = IMU.calcAccel(IMU.ay);
gotmilk 0:c0e8fdd254a9 115 uLCD.printf("Y-acceleration in g's: %2f\n", accel);
gotmilk 0:c0e8fdd254a9 116 if (abs(accel) > .6 && abs(accel) < 1) {
gotmilk 0:c0e8fdd254a9 117 counter++;
gotmilk 2:95d21fb43f6f 118 } else {
gotmilk 2:95d21fb43f6f 119 counter = 0;
gotmilk 0:c0e8fdd254a9 120 }
gotmilk 0:c0e8fdd254a9 121 if (counter >= 2) {
gotmilk 2:95d21fb43f6f 122 FILE *wave_file;
gotmilk 2:95d21fb43f6f 123 wave_file=fopen("/sd/turn.wav","r");
gotmilk 2:95d21fb43f6f 124 waver.play(wave_file);
gotmilk 2:95d21fb43f6f 125 fclose(wave_file);
gotmilk 2:95d21fb43f6f 126 FILE *fp = fopen("/sd/speedlog.txt", "w");
gotmilk 2:95d21fb43f6f 127 if(fp == NULL) {
gotmilk 2:95d21fb43f6f 128 uLCD.printf("Can't open file");
gotmilk 2:95d21fb43f6f 129 }
gotmilk 2:95d21fb43f6f 130 fprintf(fp, "Driver turned above g-limit on on %s\n", ctime(&seconds));
gotmilk 2:95d21fb43f6f 131 fclose(fp);
gotmilk 0:c0e8fdd254a9 132 counter = 0;
gotmilk 0:c0e8fdd254a9 133 }
gotmilk 0:c0e8fdd254a9 134 }
gotmilk 0:c0e8fdd254a9 135 }
gotmilk 0:c0e8fdd254a9 136 }
gotmilk 0:c0e8fdd254a9 137 }
gotmilk 0:c0e8fdd254a9 138
gotmilk 0:c0e8fdd254a9 139
gotmilk 0:c0e8fdd254a9 140 /*
gotmilk 0:c0e8fdd254a9 141 $GPRMC,000115.039,V,,,,,,,291006,,*2C
gotmilk 0:c0e8fdd254a9 142 $GPGGA,000116.031,,,,,0,00,,,M,0.0,M,,0000*52
gotmilk 0:c0e8fdd254a9 143 $GPGSA,A,1,,,,,,,,,,,,,,,*1E
gotmilk 0:c0e8fdd254a9 144 $GPGSV,3,1,12,20,00,000,,10,00,000,,31,00,000,,27,00,000,*7C
gotmilk 0:c0e8fdd254a9 145 $GPGSV,3,2,12,19,00,000,,07,00,000,,04,00,000,,24,00,000,*76
gotmilk 0:c0e8fdd254a9 146 $GPGSV,3,3,12,16,00,000,,28,00,000,,26,00,000,,29,00,000,*78
gotmilk 0:c0e8fdd254a9 147 $GPRMC,000116.031,V,,,,,,,291006,,*27
gotmilk 0:c0e8fdd254a9 148 $GPGGA,000117.035,,,,,0,00,,,M,0.0,M,,0000*57
gotmilk 0:c0e8fdd254a9 149 $GPGSA,A,1,,,,,,,,,,,,,,,*1E
gotmilk 0:c0e8fdd254a9 150 $GPRMC,000117.035,V,,,,,,,291006,,*22
gotmilk 0:c0e8fdd254a9 151 $GPGGA,000118.039,,,,,0,00,,,M,0.0,M,,0000*54
gotmilk 0:c0e8fdd254a9 152 $GPGSA,A,1,,,,,,,,,,,,,,,*1E
gotmilk 0:c0e8fdd254a9 153 $GPRMC,000118.039,V,,,,,,,291006,,*21
gotmilk 0:c0e8fdd254a9 154 $GPGGA,000119.035,,,,,0,00,,,M,0.0,M,,0000*59
gotmilk 0:c0e8fdd254a9 155 $GPGSA,A,1,,,,,,,,,,,,,,,*1E
gotmilk 0:c0e8fdd254a9 156 $GPRMC,000119.035,V,,,,,,,291006,,*2C
gotmilk 0:c0e8fdd254a9 157 $GPGGA,000120.037,,,,,0,00,,,M,0.0,M,,0000*51
gotmilk 0:c0e8fdd254a9 158 $GPGSA,A,1,,,,,,,,,,,,,,,*1E
gotmilk 0:c0e8fdd254a9 159 $GPRMC,000120.037,V,,,,,,,291006,,*24
gotmilk 0:c0e8fdd254a9 160 */