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