modifiche posizione iniziale e sistema d'allarme.
Dependencies: MTK3339 TextLCD mbed tsi_sensor
Fork of app_gps by
main.cpp@6:c8efb299b0f0, 2015-07-13 (annotated)
- Committer:
- den90
- Date:
- Mon Jul 13 14:53:57 2015 +0000
- Revision:
- 6:c8efb299b0f0
- Parent:
- 5:244867f8fc75
- Child:
- 7:ffc4ef2442fa
Thomas 2
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:c8efb299b0f0 | 112 | if (3*stdevLat <= 0.00001 && 3*stdevLon <= 0.00001) { |
den90 | 6:c8efb299b0f0 | 113 | initPos[LAT] = meanLat; |
den90 | 6:c8efb299b0f0 | 114 | initPos[LON] = meanLon; |
den90 | 6:c8efb299b0f0 | 115 | set = true; |
den90 | 6:c8efb299b0f0 | 116 | } |
den90 | 6:c8efb299b0f0 | 117 | |
den90 | 6:c8efb299b0f0 | 118 | lcd.cls(); |
den90 | 6:c8efb299b0f0 | 119 | lcd.printf("%f\n%f %d", stdevLat, stdevLon, i); |
den90 | 6:c8efb299b0f0 | 120 | |
den90 | 6:c8efb299b0f0 | 121 | /* |
den90 | 5:244867f8fc75 | 122 | for (int j=0; j<N; j++) { |
den90 | 1:80d31bcee0e5 | 123 | if (coords[LAT][j] > meanLat-stdevLat && coords[LAT][j] < meanLat+stdevLat && |
den90 | 1:80d31bcee0e5 | 124 | coords[LON][j] > meanLon-stdevLon && coords[LON][j] < meanLon+stdevLon) |
den90 | 1:80d31bcee0e5 | 125 | c++; |
den90 | 5:244867f8fc75 | 126 | } |
den90 | 1:80d31bcee0e5 | 127 | |
den90 | 5:244867f8fc75 | 128 | if (c>=N*.5) { |
den90 | 1:80d31bcee0e5 | 129 | initPos[LAT] = coords[LAT][i%N]; |
den90 | 1:80d31bcee0e5 | 130 | initPos[LON] = coords[LON][i%N]; |
den90 | 1:80d31bcee0e5 | 131 | set = true; |
den90 | 5:244867f8fc75 | 132 | } |
den90 | 6:c8efb299b0f0 | 133 | */ |
den90 | 1:80d31bcee0e5 | 134 | } |
den90 | 2:3a9cacc54417 | 135 | |
den90 | 2:3a9cacc54417 | 136 | i++; |
den90 | 1:80d31bcee0e5 | 137 | } |
den90 | 1:80d31bcee0e5 | 138 | } |
den90 | 1:80d31bcee0e5 | 139 | |
den90 | 1:80d31bcee0e5 | 140 | static void setRGBColor(double r, double g, double b) |
den90 | 1:80d31bcee0e5 | 141 | { |
den90 | 5:244867f8fc75 | 142 | r_led = 1 - r; g_led = 1 - g; b_led = 1 - b; |
den90 | 1:80d31bcee0e5 | 143 | } |
den90 | 1:80d31bcee0e5 | 144 | |
den90 | 1:80d31bcee0e5 | 145 | int main(void) |
den90 | 1:80d31bcee0e5 | 146 | { |
den90 | 5:244867f8fc75 | 147 | double d1, d2; |
den90 | 1:80d31bcee0e5 | 148 | |
den90 | 1:80d31bcee0e5 | 149 | gps.start(&dataAvailable, MTK3339::NmeaGga); |
den90 | 1:80d31bcee0e5 | 150 | |
den90 | 1:80d31bcee0e5 | 151 | while (1) { |
den90 | 1:80d31bcee0e5 | 152 | |
den90 | 1:80d31bcee0e5 | 153 | while (waitData == 0); |
den90 | 1:80d31bcee0e5 | 154 | |
den90 | 5:244867f8fc75 | 155 | lcd.cls(); |
den90 | 1:80d31bcee0e5 | 156 | |
embeddedartists | 0:828a0c36c3e2 | 157 | if ((waitData & MTK3339::NmeaGga) != 0) { |
embeddedartists | 0:828a0c36c3e2 | 158 | waitData &= ~(MTK3339::NmeaGga); |
den90 | 1:80d31bcee0e5 | 159 | |
den90 | 1:80d31bcee0e5 | 160 | if (gps.gga.satellites < 4) { |
den90 | 1:80d31bcee0e5 | 161 | lcd.printf("Fixing\nsatellites (%d)", gps.gga.satellites); |
den90 | 1:80d31bcee0e5 | 162 | setRGBColor(1, 1, 0); |
den90 | 1:80d31bcee0e5 | 163 | } else if (!set) { |
den90 | 1:80d31bcee0e5 | 164 | // La prima volta prendi la posizione iniziale |
den90 | 1:80d31bcee0e5 | 165 | lcd.printf("Acquiring\nposition...", gps.gga.satellites); |
den90 | 1:80d31bcee0e5 | 166 | setInitPos(); |
den90 | 1:80d31bcee0e5 | 167 | } else { |
den90 | 1:80d31bcee0e5 | 168 | // Prendi la soglia impostata dall'utente |
den90 | 1:80d31bcee0e5 | 169 | if (slider.readPercentage() != 0.0) { |
den90 | 1:80d31bcee0e5 | 170 | thrs = floor(slider.readPercentage() * 12.0 + 3.0); |
den90 | 1:80d31bcee0e5 | 171 | } |
den90 | 1:80d31bcee0e5 | 172 | |
den90 | 1:80d31bcee0e5 | 173 | // Calcola la distanza tra punto iniziale e attuale |
den90 | 5:244867f8fc75 | 174 | d1 = distFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees()); |
den90 | 5:244867f8fc75 | 175 | d2 = distThomasFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees()); |
den90 | 5:244867f8fc75 | 176 | |
den90 | 5:244867f8fc75 | 177 | lcd.printf("D1 = %f m\nD2 = %f m", d1, d2); |
den90 | 1:80d31bcee0e5 | 178 | |
den90 | 1:80d31bcee0e5 | 179 | // Verifica che la soglia sia superata |
den90 | 5:244867f8fc75 | 180 | if (d2 >= thrs) { |
den90 | 2:3a9cacc54417 | 181 | //lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm ALARM!", thrs, gps.gga.satellites, d); |
den90 | 1:80d31bcee0e5 | 182 | setRGBColor(1, 0, 0); |
den90 | 1:80d31bcee0e5 | 183 | } else { |
den90 | 2:3a9cacc54417 | 184 | //lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm OK!", thrs, gps.gga.satellites, d); |
den90 | 1:80d31bcee0e5 | 185 | setRGBColor(0, 1, 0); |
den90 | 1:80d31bcee0e5 | 186 | } |
den90 | 1:80d31bcee0e5 | 187 | } |
den90 | 1:80d31bcee0e5 | 188 | |
embeddedartists | 0:828a0c36c3e2 | 189 | } |
den90 | 1:80d31bcee0e5 | 190 | |
den90 | 1:80d31bcee0e5 | 191 | waitData &= MTK3339::NmeaGga; |
embeddedartists | 0:828a0c36c3e2 | 192 | } |
embeddedartists | 0:828a0c36c3e2 | 193 | |
embeddedartists | 0:828a0c36c3e2 | 194 | |
embeddedartists | 0:828a0c36c3e2 | 195 | |
den90 | 1:80d31bcee0e5 | 196 | } |