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