Dennis Morello / app_gps

Progetto Tuzzi

Dependencies:   MTK3339 TextLCD mbed tsi_sensor

Fork of app_gps by EmbeddedArtists AB

main.cpp@12:45cdb2b65f79, 2015-07-15 (annotated)

Committer:
den90
Date:
Wed Jul 15 15:06:36 2015 +0000
Revision:
12:45cdb2b65f79
Parent:
11:d9fb0fa0cbcd 
FINAL VERSION!!!

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 11:d9fb0fa0cbcd 23 const int N = 100; // Campioni da acquisire per determinare la posizione
den90 11:d9fb0fa0cbcd 24 const int SATS = 5; // Satelliti da fixare prima di acquisire la posizione
embeddedartists 0:828a0c36c3e2 25
den90 11:d9fb0fa0cbcd 26 double initPos[2] = {.0, .0}; // Latitudine, Longitudine
den90 12:45cdb2b65f79 27 double thrs = 5.0; // Soglia di allarme di default in metri
den90 11:d9fb0fa0cbcd 28 bool set = false; // Posizione iniziale individuata
den90 1:80d31bcee0e5 29
den90 1:80d31bcee0e5 30 static void dataAvailable()
den90 1:80d31bcee0e5 31 {
embeddedartists 0:828a0c36c3e2 32 waitData |= gps.getAvailableDataType();
embeddedartists 0:828a0c36c3e2 33 }
embeddedartists 0:828a0c36c3e2 34
stefanodudine 8:ffc4ef2442fa 35 static void setRGBColor(double r, double g, double b)
stefanodudine 8:ffc4ef2442fa 36 {
den90 11:d9fb0fa0cbcd 37 r_led = 1 - r;
den90 11:d9fb0fa0cbcd 38 g_led = 1 - g;
den90 11:d9fb0fa0cbcd 39 b_led = 1 - b;
stefanodudine 8:ffc4ef2442fa 40 }
stefanodudine 8:ffc4ef2442fa 41
den90 1:80d31bcee0e5 42 // Trasforma un angolo da gradi in radianti
den90 1:80d31bcee0e5 43 static double toRadians(double deg)
den90 1:80d31bcee0e5 44 {
den90 1:80d31bcee0e5 45 return deg * PI / 180.0;
den90 1:80d31bcee0e5 46 }
den90 1:80d31bcee0e5 47
den90 1:80d31bcee0e5 48 // Calcola la distanza tra 2 punti in metri
den90 1:80d31bcee0e5 49 static double dist(double lat1, double lon1, double lat2, double lon2)
den90 1:80d31bcee0e5 50 {
den90 1:80d31bcee0e5 51 double dPhi, dLam, a, c;
den90 1:80d31bcee0e5 52
den90 1:80d31bcee0e5 53 dPhi = toRadians(lat2 - lat1);
den90 1:80d31bcee0e5 54 dLam = toRadians(lon2 - lon1);
den90 1:80d31bcee0e5 55 a = sin(dPhi/2) * sin(dPhi/2) +
den90 1:80d31bcee0e5 56 cos(toRadians(lat1)) * cos(toRadians(lat2)) * sin(dLam/2) * sin(dLam/2);
den90 1:80d31bcee0e5 57 c = 2 * atan2(sqrt(a), sqrt(1-a));
stefanodudine 8:ffc4ef2442fa 58 return R * c;
stefanodudine 8:ffc4ef2442fa 59 }
stefanodudine 8:ffc4ef2442fa 60
stefanodudine 8:ffc4ef2442fa 61 // Calcola la distanza tra punto iniziale e attuale in metri
stefanodudine 8:ffc4ef2442fa 62 static double distFromInit(double lat, double lon)
stefanodudine 8:ffc4ef2442fa 63 {
stefanodudine 8:ffc4ef2442fa 64 return dist(initPos[LAT], initPos[LON], lat, lon);
den90 1:80d31bcee0e5 65 }
den90 1:80d31bcee0e5 66
den90 11:d9fb0fa0cbcd 67 // Calcola la distanza tra 2 punti in metri (approssimazione equirettangolare)
den90 5:244867f8fc75 68 static double distThomas(double lat1, double lon1, double lat2, double lon2)
den90 5:244867f8fc75 69 {
den90 5:244867f8fc75 70 double x = toRadians(lon2 - lon1) * cos(toRadians(lat1 + lat2)/2);
den90 5:244867f8fc75 71 double y = toRadians(lat2 - lat1);
den90 11:d9fb0fa0cbcd 72 double d = sqrt(x*x + y*y) * R;
den90 11:d9fb0fa0cbcd 73 return isnan(d) ? 0.0 : d;
den90 5:244867f8fc75 74 }
den90 5:244867f8fc75 75
den90 11:d9fb0fa0cbcd 76 // Calcola la distanza tra punto iniziale e attuale in metri (approssimazione equirettangolare)
den90 5:244867f8fc75 77 static double distThomasFromInit(double lat, double lon)
den90 5:244867f8fc75 78 {
den90 5:244867f8fc75 79 return distThomas(initPos[LAT], initPos[LON], lat, lon);
den90 5:244867f8fc75 80 }
den90 5:244867f8fc75 81
den90 11:d9fb0fa0cbcd 82 // Calcola la media di un vettore
den90 1:80d31bcee0e5 83 static double mean(double* x, int n)
den90 1:80d31bcee0e5 84 {
den90 1:80d31bcee0e5 85 double sum = 0;
den90 11:d9fb0fa0cbcd 86 double c = 0;
den90 1:80d31bcee0e5 87 for (int i=0; i<n; i++) {
den90 11:d9fb0fa0cbcd 88 if (!isnan(x[i])) {
den90 11:d9fb0fa0cbcd 89 sum += x[i];
den90 11:d9fb0fa0cbcd 90 c++;
den90 11:d9fb0fa0cbcd 91 }
den90 1:80d31bcee0e5 92 }
den90 11:d9fb0fa0cbcd 93 return sum / c;
den90 1:80d31bcee0e5 94 }
den90 1:80d31bcee0e5 95
den90 11:d9fb0fa0cbcd 96 // Calcola la varianza di un vettore
den90 1:80d31bcee0e5 97 static double var(double* x, int n)
den90 1:80d31bcee0e5 98 {
den90 1:80d31bcee0e5 99 double x1[n];
den90 1:80d31bcee0e5 100 for (int i=0; i<n; i++) {
den90 1:80d31bcee0e5 101 x1[i] = x[i]*x[i];
den90 1:80d31bcee0e5 102 }
den90 1:80d31bcee0e5 103 return mean(x1,n) - (mean(x,n)*mean(x,n));
den90 1:80d31bcee0e5 104 }
den90 1:80d31bcee0e5 105
den90 11:d9fb0fa0cbcd 106 // Calcola la deviazione standard di un vettore
den90 2:3a9cacc54417 107 static double stdev(double* x, int n)
den90 1:80d31bcee0e5 108 {
den90 2:3a9cacc54417 109 return sqrt(var(x,n));
den90 1:80d31bcee0e5 110 }
embeddedartists 0:828a0c36c3e2 111
den90 11:d9fb0fa0cbcd 112 // Determina la posizione iniziale
den90 1:80d31bcee0e5 113 static void setInitPos()
den90 1:80d31bcee0e5 114 {
den90 11:d9fb0fa0cbcd 115 double coords[2][N], lat, lon, meanLat, meanLon, stdevLat, stdevLon;
den90 1:80d31bcee0e5 116 int i = 0;
den90 11:d9fb0fa0cbcd 117
den90 11:d9fb0fa0cbcd 118 while (!set && gps.gga.satellites>=SATS) {
den90 11:d9fb0fa0cbcd 119
den90 11:d9fb0fa0cbcd 120 lat = gps.getLatitudeAsDegrees();
den90 11:d9fb0fa0cbcd 121 lon = gps.getLongitudeAsDegrees();
den90 11:d9fb0fa0cbcd 122
den90 1:80d31bcee0e5 123 wait(1);
den90 11:d9fb0fa0cbcd 124
den90 11:d9fb0fa0cbcd 125 if (!isnan(lat+lon)) {
den90 11:d9fb0fa0cbcd 126 coords[LAT][i%N] = lat;
den90 11:d9fb0fa0cbcd 127 coords[LON][i%N] = lon;
den90 11:d9fb0fa0cbcd 128
den90 6:c8efb299b0f0 129 lcd.cls();
den90 11:d9fb0fa0cbcd 130
den90 11:d9fb0fa0cbcd 131 if (i >= 2*N) {
den90 11:d9fb0fa0cbcd 132 meanLat = mean(coords[LAT], N);
den90 11:d9fb0fa0cbcd 133 meanLon = mean(coords[LON], N);
den90 11:d9fb0fa0cbcd 134 stdevLat = stdev(coords[LAT], N);
den90 11:d9fb0fa0cbcd 135 stdevLon = stdev(coords[LON], N);
den90 11:d9fb0fa0cbcd 136
den90 11:d9fb0fa0cbcd 137 lcd.printf("ACCURACY=%0.2fm\nTIME=%ds GPS=%d", distThomas(0, 0, 3*stdevLat, 3*stdevLon), i-2*N, gps.gga.satellites);
den90 12:45cdb2b65f79 138 if (distThomas(0, 0, 3*stdevLat, 3*stdevLon) < 2.0) {
den90 11:d9fb0fa0cbcd 139 initPos[LAT] = meanLat;
den90 11:d9fb0fa0cbcd 140 initPos[LON] = meanLon;
den90 11:d9fb0fa0cbcd 141 set = true;
den90 11:d9fb0fa0cbcd 142 }
den90 11:d9fb0fa0cbcd 143 } else {
den90 11:d9fb0fa0cbcd 144 lcd.printf("Acquiring\nposition (%ds)", 2*N-i);
den90 5:244867f8fc75 145 }
den90 11:d9fb0fa0cbcd 146
den90 11:d9fb0fa0cbcd 147 i++;
den90 1:80d31bcee0e5 148 }
den90 11:d9fb0fa0cbcd 149
den90 1:80d31bcee0e5 150 }
den90 1:80d31bcee0e5 151 }
den90 1:80d31bcee0e5 152
den90 1:80d31bcee0e5 153 int main(void)
den90 1:80d31bcee0e5 154 {
stefanodudine 8:ffc4ef2442fa 155 double d;
stefanodudine 8:ffc4ef2442fa 156 int c = 0;
den90 1:80d31bcee0e5 157
den90 1:80d31bcee0e5 158 gps.start(&dataAvailable, MTK3339::NmeaGga);
den90 1:80d31bcee0e5 159
den90 1:80d31bcee0e5 160 while (1) {
den90 1:80d31bcee0e5 161
den90 1:80d31bcee0e5 162 while (waitData == 0);
den90 1:80d31bcee0e5 163
den90 5:244867f8fc75 164 lcd.cls();
den90 1:80d31bcee0e5 165
embeddedartists 0:828a0c36c3e2 166 if ((waitData & MTK3339::NmeaGga) != 0) {
embeddedartists 0:828a0c36c3e2 167 waitData &= ~(MTK3339::NmeaGga);
den90 1:80d31bcee0e5 168
den90 11:d9fb0fa0cbcd 169 if (gps.gga.satellites < SATS) {
den90 11:d9fb0fa0cbcd 170 lcd.printf("Fixing\nsats (%d/%d)", gps.gga.satellites, SATS);
den90 1:80d31bcee0e5 171 setRGBColor(1, 1, 0);
den90 11:d9fb0fa0cbcd 172 c = 0;
den90 1:80d31bcee0e5 173 } else if (!set) {
den90 1:80d31bcee0e5 174 // La prima volta prendi la posizione iniziale
den90 11:d9fb0fa0cbcd 175 lcd.printf("Acquiring\nposition...");
den90 1:80d31bcee0e5 176 setInitPos();
den90 1:80d31bcee0e5 177 } else {
den90 1:80d31bcee0e5 178 // Prendi la soglia impostata dall'utente
den90 1:80d31bcee0e5 179 if (slider.readPercentage() != 0.0) {
den90 11:d9fb0fa0cbcd 180 thrs = floor(slider.readPercentage() * 13.0 + 3.0);
den90 1:80d31bcee0e5 181 }
den90 1:80d31bcee0e5 182
den90 1:80d31bcee0e5 183 // Calcola la distanza tra punto iniziale e attuale
stefanodudine 8:ffc4ef2442fa 184 d = distThomasFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
den90 1:80d31bcee0e5 185
den90 1:80d31bcee0e5 186 // Verifica che la soglia sia superata
stefanodudine 8:ffc4ef2442fa 187 if (d > thrs) {
den90 11:d9fb0fa0cbcd 188 if (++c >= 15) {
den90 11:d9fb0fa0cbcd 189 lcd.printf("THR=%0.0fm SATS=%d\nDIST=%2.1fm ALARM", thrs, gps.gga.satellites, d);
den90 11:d9fb0fa0cbcd 190 setRGBColor(1, 0, 0);
den90 11:d9fb0fa0cbcd 191 } else {
den90 11:d9fb0fa0cbcd 192 lcd.printf("THR=%0.0fm SATS=%d\nDIST=%2.1fm OK", thrs, gps.gga.satellites, thrs);
stefanodudine 8:ffc4ef2442fa 193 setRGBColor(1, 0, 0);
stefanodudine 8:ffc4ef2442fa 194 }
den90 1:80d31bcee0e5 195 } else {
den90 11:d9fb0fa0cbcd 196 lcd.printf("THR=%0.0fm SATS=%d\nDIST=%2.1fm OK", thrs, gps.gga.satellites, d);
den90 1:80d31bcee0e5 197 setRGBColor(0, 1, 0);
den90 10:ce334838ed72 198 c = 0;
den90 1:80d31bcee0e5 199 }
den90 1:80d31bcee0e5 200 }
den90 1:80d31bcee0e5 201
embeddedartists 0:828a0c36c3e2 202 }
den90 1:80d31bcee0e5 203 waitData &= MTK3339::NmeaGga;
embeddedartists 0:828a0c36c3e2 204 }
den90 1:80d31bcee0e5 205 }