Stefano Dudine / app_gps

modifiche posizione iniziale e sistema d'allarme.

Dependencies:   MTK3339 TextLCD mbed tsi_sensor

Fork of app_gps by Dennis Morello

main.cpp@5:244867f8fc75, 2015-07-13 (annotated)

Committer:
den90
Date:
Mon Jul 13 14:35:32 2015 +0000
Revision:
5:244867f8fc75
Parent:
3:5353387b52f7
Child:
6:c8efb299b0f0
distThomas

Who changed what in which revision?

UserRevisionLine numberNew contents of line
embeddedartists 0:828a0c36c3e2 1 #include "mbed.h"
embeddedartists 0:828a0c36c3e2 2 #include "MTK3339.h"
den90 1:80d31bcee0e5 3 #include "TextLCD.h"
den90 1:80d31bcee0e5 4 #include "tsi_sensor.h"
den90 1:80d31bcee0e5 5
den90 1:80d31bcee0e5 6 #define ELEC0 9
den90 1:80d31bcee0e5 7 #define ELEC1 10
den90 1:80d31bcee0e5 8 #define LAT 0
den90 1:80d31bcee0e5 9 #define LON 1
embeddedartists 0:828a0c36c3e2 10
embeddedartists 0:828a0c36c3e2 11 static int waitData = 0;
den90 1:80d31bcee0e5 12
den90 1:80d31bcee0e5 13 static MTK3339 gps(PTE22, PTE23);
den90 1:80d31bcee0e5 14 static DigitalOut r_led(LED_RED);
den90 1:80d31bcee0e5 15 static DigitalOut g_led(LED_GREEN);
den90 1:80d31bcee0e5 16 static DigitalOut b_led(LED_BLUE);
den90 1:80d31bcee0e5 17 static TSIAnalogSlider slider(ELEC0, ELEC1, 40);
den90 1:80d31bcee0e5 18 static TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2);
den90 5:244867f8fc75 19 static Serial pc(USBTX, USBRX);
embeddedartists 0:828a0c36c3e2 20
den90 1:80d31bcee0e5 21 const double PI = 3.141592654; // Pi greco
den90 1:80d31bcee0e5 22 const double R = 6371000.0; // Raggio della Terra in metri
den90 1:80d31bcee0e5 23 const int N = 60;
embeddedartists 0:828a0c36c3e2 24
den90 1:80d31bcee0e5 25 double initPos[2] = {.0, .0};
den90 1:80d31bcee0e5 26 double thrs = 3.0; // Soglia di allarme in metri
den90 1:80d31bcee0e5 27 bool set = false;
den90 1:80d31bcee0e5 28
den90 1:80d31bcee0e5 29 static void dataAvailable()
den90 1:80d31bcee0e5 30 {
embeddedartists 0:828a0c36c3e2 31 waitData |= gps.getAvailableDataType();
embeddedartists 0:828a0c36c3e2 32 }
embeddedartists 0:828a0c36c3e2 33
den90 1:80d31bcee0e5 34 // Trasforma un angolo da gradi in radianti
den90 1:80d31bcee0e5 35 static double toRadians(double deg)
den90 1:80d31bcee0e5 36 {
den90 1:80d31bcee0e5 37 return deg * PI / 180.0;
den90 1:80d31bcee0e5 38 }
den90 1:80d31bcee0e5 39
den90 1:80d31bcee0e5 40 // Calcola la distanza tra 2 punti in metri
den90 1:80d31bcee0e5 41 static double dist(double lat1, double lon1, double lat2, double lon2)
den90 1:80d31bcee0e5 42 {
den90 1:80d31bcee0e5 43 double dPhi, dLam, a, c;
den90 1:80d31bcee0e5 44
den90 1:80d31bcee0e5 45 dPhi = toRadians(lat2 - lat1);
den90 1:80d31bcee0e5 46 dLam = toRadians(lon2 - lon1);
den90 1:80d31bcee0e5 47 a = sin(dPhi/2) * sin(dPhi/2) +
den90 1:80d31bcee0e5 48 cos(toRadians(lat1)) * cos(toRadians(lat2)) * sin(dLam/2) * sin(dLam/2);
den90 1:80d31bcee0e5 49 c = 2 * atan2(sqrt(a), sqrt(1-a));
den90 1:80d31bcee0e5 50 return ceil(R * c);
den90 1:80d31bcee0e5 51 }
den90 1:80d31bcee0e5 52
den90 5:244867f8fc75 53 static double distThomas(double lat1, double lon1, double lat2, double lon2)
den90 5:244867f8fc75 54 {
den90 5:244867f8fc75 55 double x = toRadians(lon2 - lon1) * cos(toRadians(lat1 + lat2)/2);
den90 5:244867f8fc75 56 double y = toRadians(lat2 - lat1);
den90 5:244867f8fc75 57 return sqrt(x*x + y*y) * R;
den90 5:244867f8fc75 58 }
den90 5:244867f8fc75 59
den90 5:244867f8fc75 60 static double distThomasFromInit(double lat, double lon)
den90 5:244867f8fc75 61 {
den90 5:244867f8fc75 62 return distThomas(initPos[LAT], initPos[LON], lat, lon);
den90 5:244867f8fc75 63 }
den90 5:244867f8fc75 64
den90 1:80d31bcee0e5 65 // Calcola la distanza tra punto iniziale e attuale in metri
den90 1:80d31bcee0e5 66 static double distFromInit(double lat, double lon)
den90 1:80d31bcee0e5 67 {
den90 1:80d31bcee0e5 68 return dist(initPos[LAT], initPos[LON], lat, lon);
den90 1:80d31bcee0e5 69 }
den90 1:80d31bcee0e5 70
den90 1:80d31bcee0e5 71 static double mean(double* x, int n)
den90 1:80d31bcee0e5 72 {
den90 1:80d31bcee0e5 73 double sum = 0;
den90 1:80d31bcee0e5 74 for (int i=0; i<n; i++) {
den90 1:80d31bcee0e5 75 sum += x[i];
den90 1:80d31bcee0e5 76 }
den90 1:80d31bcee0e5 77 return sum / n;
den90 1:80d31bcee0e5 78 }
den90 1:80d31bcee0e5 79
den90 1:80d31bcee0e5 80 static double var(double* x, int n)
den90 1:80d31bcee0e5 81 {
den90 1:80d31bcee0e5 82 double x1[n];
den90 1:80d31bcee0e5 83 for (int i=0; i<n; i++) {
den90 1:80d31bcee0e5 84 x1[i] = x[i]*x[i];
den90 1:80d31bcee0e5 85 }
den90 1:80d31bcee0e5 86 return mean(x1,n) - (mean(x,n)*mean(x,n));
den90 1:80d31bcee0e5 87 }
den90 1:80d31bcee0e5 88
den90 2:3a9cacc54417 89 static double stdev(double* x, int n)
den90 1:80d31bcee0e5 90 {
den90 2:3a9cacc54417 91 return sqrt(var(x,n));
den90 1:80d31bcee0e5 92 }
embeddedartists 0:828a0c36c3e2 93
den90 1:80d31bcee0e5 94 static void setInitPos()
den90 1:80d31bcee0e5 95 {
den90 1:80d31bcee0e5 96 double coords[2][N];
den90 1:80d31bcee0e5 97 int i = 0;
den90 1:80d31bcee0e5 98 int c = 0;
den90 1:80d31bcee0e5 99 double meanLat, meanLon, stdevLat, stdevLon;
den90 2:3a9cacc54417 100
den90 1:80d31bcee0e5 101 while (!set && gps.gga.satellites>=4) {
den90 1:80d31bcee0e5 102 coords[LAT][i%N] = gps.getLatitudeAsDegrees();
den90 1:80d31bcee0e5 103 coords[LON][i%N] = gps.getLongitudeAsDegrees();
den90 1:80d31bcee0e5 104 wait(1);
embeddedartists 0:828a0c36c3e2 105
den90 3:5353387b52f7 106 if (i>=2*N) {
den90 1:80d31bcee0e5 107 meanLat = mean(coords[LAT], N);
den90 1:80d31bcee0e5 108 meanLon = mean(coords[LON], N);
den90 1:80d31bcee0e5 109 stdevLat = stdev(coords[LAT], N);
den90 1:80d31bcee0e5 110 stdevLon = stdev(coords[LON], N);
den90 1:80d31bcee0e5 111
den90 5:244867f8fc75 112 for (int j=0; j<N; j++) {
den90 1:80d31bcee0e5 113 if (coords[LAT][j] > meanLat-stdevLat && coords[LAT][j] < meanLat+stdevLat &&
den90 1:80d31bcee0e5 114 coords[LON][j] > meanLon-stdevLon && coords[LON][j] < meanLon+stdevLon)
den90 1:80d31bcee0e5 115 c++;
den90 5:244867f8fc75 116 }
den90 1:80d31bcee0e5 117
den90 5:244867f8fc75 118 if (c>=N*.5) {
den90 1:80d31bcee0e5 119 initPos[LAT] = coords[LAT][i%N];
den90 1:80d31bcee0e5 120 initPos[LON] = coords[LON][i%N];
den90 1:80d31bcee0e5 121 set = true;
den90 5:244867f8fc75 122 }
den90 1:80d31bcee0e5 123 }
den90 2:3a9cacc54417 124
den90 2:3a9cacc54417 125 i++;
den90 1:80d31bcee0e5 126 }
den90 1:80d31bcee0e5 127 }
den90 1:80d31bcee0e5 128
den90 1:80d31bcee0e5 129 static void setRGBColor(double r, double g, double b)
den90 1:80d31bcee0e5 130 {
den90 5:244867f8fc75 131 r_led = 1 - r; g_led = 1 - g; b_led = 1 - b;
den90 1:80d31bcee0e5 132 }
den90 1:80d31bcee0e5 133
den90 1:80d31bcee0e5 134 int main(void)
den90 1:80d31bcee0e5 135 {
den90 5:244867f8fc75 136 double d1, d2;
den90 1:80d31bcee0e5 137
den90 1:80d31bcee0e5 138 gps.start(&dataAvailable, MTK3339::NmeaGga);
den90 1:80d31bcee0e5 139
den90 1:80d31bcee0e5 140 while (1) {
den90 1:80d31bcee0e5 141
den90 1:80d31bcee0e5 142 while (waitData == 0);
den90 1:80d31bcee0e5 143
den90 5:244867f8fc75 144 lcd.cls();
den90 1:80d31bcee0e5 145
embeddedartists 0:828a0c36c3e2 146 if ((waitData & MTK3339::NmeaGga) != 0) {
embeddedartists 0:828a0c36c3e2 147 waitData &= ~(MTK3339::NmeaGga);
den90 1:80d31bcee0e5 148
den90 1:80d31bcee0e5 149 if (gps.gga.satellites < 4) {
den90 1:80d31bcee0e5 150 lcd.printf("Fixing\nsatellites (%d)", gps.gga.satellites);
den90 1:80d31bcee0e5 151 setRGBColor(1, 1, 0);
den90 1:80d31bcee0e5 152 } else if (!set) {
den90 1:80d31bcee0e5 153 // La prima volta prendi la posizione iniziale
den90 1:80d31bcee0e5 154 lcd.printf("Acquiring\nposition...", gps.gga.satellites);
den90 1:80d31bcee0e5 155 setInitPos();
den90 1:80d31bcee0e5 156 } else {
den90 1:80d31bcee0e5 157 // Prendi la soglia impostata dall'utente
den90 1:80d31bcee0e5 158 if (slider.readPercentage() != 0.0) {
den90 1:80d31bcee0e5 159 thrs = floor(slider.readPercentage() * 12.0 + 3.0);
den90 1:80d31bcee0e5 160 }
den90 1:80d31bcee0e5 161
den90 1:80d31bcee0e5 162 // Calcola la distanza tra punto iniziale e attuale
den90 5:244867f8fc75 163 d1 = distFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
den90 5:244867f8fc75 164 d2 = distThomasFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
den90 5:244867f8fc75 165
den90 5:244867f8fc75 166 lcd.printf("D1 = %f m\nD2 = %f m", d1, d2);
den90 1:80d31bcee0e5 167
den90 1:80d31bcee0e5 168 // Verifica che la soglia sia superata
den90 5:244867f8fc75 169 if (d2 >= thrs) {
den90 2:3a9cacc54417 170 //lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm ALARM!", thrs, gps.gga.satellites, d);
den90 1:80d31bcee0e5 171 setRGBColor(1, 0, 0);
den90 1:80d31bcee0e5 172 } else {
den90 2:3a9cacc54417 173 //lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm OK!", thrs, gps.gga.satellites, d);
den90 1:80d31bcee0e5 174 setRGBColor(0, 1, 0);
den90 1:80d31bcee0e5 175 }
den90 1:80d31bcee0e5 176 }
den90 1:80d31bcee0e5 177
embeddedartists 0:828a0c36c3e2 178 }
den90 1:80d31bcee0e5 179
den90 1:80d31bcee0e5 180 waitData &= MTK3339::NmeaGga;
embeddedartists 0:828a0c36c3e2 181 }
embeddedartists 0:828a0c36c3e2 182
embeddedartists 0:828a0c36c3e2 183
embeddedartists 0:828a0c36c3e2 184
den90 1:80d31bcee0e5 185 }