Diff: main.cpp

Revision:

7:ffc4ef2442fa

Parent:

6:c8efb299b0f0

Child:

8:c285a6043b98

--- a/main.cpp	Mon Jul 13 14:53:57 2015 +0000
+++ b/main.cpp	Mon Jul 13 16:52:40 2015 +0000
@@ -20,7 +20,7 @@
 
 const double PI = 3.141592654; // Pi greco
 const double R  = 6371000.0;   // Raggio della Terra in metri
-const int    N  = 60;
+const int    N  = 100;
 
 double initPos[2] = {.0, .0};
 double thrs       = 3.0;       // Soglia di allarme in metri
@@ -31,6 +31,11 @@
     waitData |= gps.getAvailableDataType();
 }
 
+static void setRGBColor(double r, double g, double b)
+{
+    r_led = 1 - r; g_led = 1 - g; b_led = 1 - b;
+}
+
 // Trasforma un angolo da gradi in radianti
 static double toRadians(double deg)
 {
@@ -47,7 +52,13 @@
     a    = sin(dPhi/2) * sin(dPhi/2) +
            cos(toRadians(lat1)) * cos(toRadians(lat2)) * sin(dLam/2) * sin(dLam/2);
     c    = 2 * atan2(sqrt(a), sqrt(1-a));
-    return ceil(R * c);
+    return R * c;
+}
+
+// Calcola la distanza tra punto iniziale e attuale in metri
+static double distFromInit(double lat, double lon)
+{
+    return dist(initPos[LAT], initPos[LON], lat, lon);
 }
 
 static double distThomas(double lat1, double lon1, double lat2, double lon2)
@@ -62,12 +73,6 @@
     return distThomas(initPos[LAT], initPos[LON], lat, lon);
 }
 
-// Calcola la distanza tra punto iniziale e attuale in metri
-static double distFromInit(double lat, double lon)
-{
-    return dist(initPos[LAT], initPos[LON], lat, lon);
-}
-
 static double mean(double* x, int n)
 {
     double sum = 0;
@@ -95,7 +100,6 @@
 {
     double coords[2][N];
     int i = 0;
-    int c = 0;
     double meanLat, meanLon, stdevLat, stdevLon;
     
     while (!set && gps.gga.satellites>=4) {
@@ -103,48 +107,30 @@
         coords[LON][i%N] = gps.getLongitudeAsDegrees();
         wait(1);
         
-        if (i>=2*N) {
+        if (i>=N) {
             meanLat  = mean(coords[LAT], N);
             meanLon  = mean(coords[LON], N);
             stdevLat = stdev(coords[LAT], N);
             stdevLon = stdev(coords[LON], N);
             
-            if (3*stdevLat <= 0.00001 && 3*stdevLon <= 0.00001) {
-                initPos[LAT] = meanLat;
-                initPos[LON] = meanLon;
-                set = true;
-            }
-            
             lcd.cls();
-            lcd.printf("%f\n%f %d", stdevLat, stdevLon, i);
+            lcd.printf("ACCURACY\n%f m (loop %d)", 2*distThomas(0, 0, 3*stdevLat, 3*stdevLon), i);
             
-            /*
-            for (int j=0; j<N; j++) {
-                if (coords[LAT][j] > meanLat-stdevLat && coords[LAT][j] < meanLat+stdevLat &&
-                    coords[LON][j] > meanLon-stdevLon && coords[LON][j] < meanLon+stdevLon)
-                    c++;
-            }
-            
-            if (c>=N*.5) {
+            if (distThomas(0, 0, stdevLat, stdevLon) < 2.5) {
                 initPos[LAT] = coords[LAT][i%N];
                 initPos[LON] = coords[LON][i%N];
                 set = true;
             }
-            */
         }
         
         i++;
     }
 }
 
-static void setRGBColor(double r, double g, double b)
-{
-    r_led = 1 - r; g_led = 1 - g; b_led = 1 - b;
-}
-
 int main(void)
 {
-    double d1, d2;
+    double d;
+    int c = 0;
 
     gps.start(&dataAvailable, MTK3339::NmeaGga);
 
@@ -167,30 +153,26 @@
             } else {
                 // Prendi la soglia impostata dall'utente
                 if (slider.readPercentage() != 0.0) {
-                    thrs = floor(slider.readPercentage() * 12.0 + 3.0);
+                    thrs = floor(slider.readPercentage() * 7.5 + 2.5);
                 }
 
                 // Calcola la distanza tra punto iniziale e attuale
-                d1 = distFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
-                d2 = distThomasFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
-                
-                lcd.printf("D1 = %f m\nD2 = %f m", d1, d2);
+                d = distThomasFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
 
                 // Verifica che la soglia sia superata
-                if (d2 >= thrs) {
-                    //lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm ALARM!", thrs, gps.gga.satellites, d);
-                    setRGBColor(1, 0, 0);
+                if (d > thrs) {
+                    if(++c > 5){
+                        lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm ALARM!", thrs, gps.gga.satellites, d);
+                        setRGBColor(1, 0, 0);
+                    }
                 } else {
-                    //lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm    OK!", thrs, gps.gga.satellites, d);
+                    lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm    OK!", thrs, gps.gga.satellites, d);
                     setRGBColor(0, 1, 0);
+                    c=0;
                 }
             }
 
         }
-
         waitData &= MTK3339::NmeaGga;
     }
-
-
-
 }