Diff: main.cpp

Revision:

5:244867f8fc75

Parent:

3:5353387b52f7

Child:

6:c8efb299b0f0

--- a/main.cpp	Mon Jul 13 12:46:38 2015 +0000
+++ b/main.cpp	Mon Jul 13 14:35:32 2015 +0000
@@ -16,6 +16,7 @@
 static DigitalOut b_led(LED_BLUE);
 static TSIAnalogSlider slider(ELEC0, ELEC1, 40);
 static TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2);
+static Serial pc(USBTX, USBRX);
 
 const double PI = 3.141592654; // Pi greco
 const double R  = 6371000.0;   // Raggio della Terra in metri
@@ -49,6 +50,18 @@
     return ceil(R * c);
 }
 
+static double distThomas(double lat1, double lon1, double lat2, double lon2)
+{
+    double x = toRadians(lon2 - lon1) * cos(toRadians(lat1 + lat2)/2);
+    double y = toRadians(lat2 - lat1);
+    return sqrt(x*x + y*y) * R;
+}
+
+static double distThomasFromInit(double lat, double lon)
+{
+    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)
 {
@@ -96,22 +109,17 @@
             stdevLat = stdev(coords[LAT], N);
             stdevLon = stdev(coords[LON], N);
             
-            lcd.cls();
-            lcd.printf("%f\n%f %d", stdevLat, stdevLon, i);
-            
-            if (stdevLat <= 0.00001)
-                set = true;
-            /*for (int j=0; j<N; j++) {
+            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 (c>=N*.5) {
                 initPos[LAT] = coords[LAT][i%N];
                 initPos[LON] = coords[LON][i%N];
                 set = true;
-            }*/
+            }
         }
         
         i++;
@@ -120,17 +128,12 @@
 
 static void setRGBColor(double r, double g, double b)
 {
-    r_led = 1 - r;
-    g_led = 1 - g;
-    b_led = 1 - b;
+    r_led = 1 - r; g_led = 1 - g; b_led = 1 - b;
 }
 
 int main(void)
 {
-    wait(2);
-    printf("Ciao");
-
-    double d;
+    double d1, d2;
 
     gps.start(&dataAvailable, MTK3339::NmeaGga);
 
@@ -138,7 +141,7 @@
 
         while (waitData == 0);
 
-        //lcd.cls();
+        lcd.cls();
 
         if ((waitData & MTK3339::NmeaGga) != 0) {
             waitData &= ~(MTK3339::NmeaGga);
@@ -157,10 +160,13 @@
                 }
 
                 // Calcola la distanza tra punto iniziale e attuale
-                d = distFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
+                d1 = distFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
+                d2 = distThomasFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
+                
+                lcd.printf("D1 = %f m\nD2 = %f m", d1, d2);
 
                 // Verifica che la soglia sia superata
-                if (d >= thrs) {
+                if (d2 >= thrs) {
                     //lcd.printf("THRS=%2.0fm SATS=%d\nDIST=%2.0fm ALARM!", thrs, gps.gga.satellites, d);
                     setRGBColor(1, 0, 0);
                 } else {