Diff: main.cpp

Revision:

1:80d31bcee0e5

Parent:

0:828a0c36c3e2

Child:

2:3a9cacc54417

--- a/main.cpp	Thu Nov 07 11:53:56 2013 +0000
+++ b/main.cpp	Mon Jul 13 10:04:05 2015 +0000
@@ -1,36 +1,171 @@
 #include "mbed.h"
 #include "MTK3339.h"
+#include "TextLCD.h"
+#include "tsi_sensor.h"
+
+#define ELEC0 9
+#define ELEC1 10
+#define LAT 0
+#define LON 1
 
 static int waitData = 0;
-static MTK3339 gps(P4_22, P4_23);
+
+static MTK3339 gps(PTE22, PTE23);
+static DigitalOut r_led(LED_RED);
+static DigitalOut g_led(LED_GREEN);
+static DigitalOut b_led(LED_BLUE);
+static TSIAnalogSlider slider(ELEC0, ELEC1, 40);
+static TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2);
 
+const double PI = 3.141592654; // Pi greco
+const double R  = 6371000.0;   // Raggio della Terra in metri
+const int    N  = 60;
 
-static void dataAvailable() {
+double initPos[2] = {.0, .0};
+double thrs       = 3.0;       // Soglia di allarme in metri
+bool   set        = false;
+
+static void dataAvailable()
+{
     waitData |= gps.getAvailableDataType();
 }
 
-int main(void) {
-   
-    gps.start(&dataAvailable, (MTK3339::NmeaGga|MTK3339::NmeaVtg));
+// Trasforma un angolo da gradi in radianti
+static double toRadians(double deg)
+{
+    return deg * PI / 180.0;
+}
+
+// Calcola la distanza tra 2 punti in metri
+static double dist(double lat1, double lon1, double lat2, double lon2)
+{
+    double dPhi, dLam, a, c;
+
+    dPhi = toRadians(lat2 - lat1);
+    dLam = toRadians(lon2 - lon1);
+    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);
+}
+
+// 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;
+    for (int i=0; i<n; i++) {
+        sum += x[i];
+    }
+    return sum / n;
+}
+
+static double var(double* x, int n)
+{
+    double x1[n];
+    for (int i=0; i<n; i++) {
+        x1[i] = x[i]*x[i];
+    }
+    return mean(x1,n) - (mean(x,n)*mean(x,n));
+}
+
+static double stdev(double* y, int n)
+{
+    return sqrt(var(y,n));
+}
 
-    while(1) {
-        while(waitData == 0);
+static void setInitPos()
+{
+    set = false;
+    double coords[2][N];
+    int i = 0;
+    int c = 0;
+    double meanLat, meanLon, stdevLat, stdevLon;
+    while (!set && gps.gga.satellites>=4) {
+        coords[LAT][i%N] = gps.getLatitudeAsDegrees();
+        coords[LON][i%N] = gps.getLongitudeAsDegrees();
+        wait(1);
+        i++;
         
+        if (i>=N) {
+            meanLat  = mean(coords[LAT], N);
+            meanLon  = mean(coords[LON], N);
+            stdevLat = stdev(coords[LAT], N);
+            stdevLon = stdev(coords[LON], N);
+            
+            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) {
+                initPos[LAT] = coords[LAT][i%N];
+                initPos[LON] = coords[LON][i%N];
+                set = true;
+            }
+        }
+    }
+}
+
+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 d;
+
+    gps.start(&dataAvailable, MTK3339::NmeaGga);
+
+    while (1) {
+
+        while (waitData == 0);
+
+        lcd.cls();
+
         if ((waitData & MTK3339::NmeaGga) != 0) {
             waitData &= ~(MTK3339::NmeaGga);
-            printf("gpa: fix=%d, sats=%d, alt=%f, lat=%f, lon=%f\n", 
-                gps.gga.fix, gps.gga.satellites, gps.gga.altitude, 
-                gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());            
+
+            if (gps.gga.satellites < 4) {
+                lcd.printf("Fixing\nsatellites (%d)", gps.gga.satellites);
+                setRGBColor(1, 1, 0);
+            } else if (!set) {
+                // La prima volta prendi la posizione iniziale
+                lcd.printf("Acquiring\nposition...", gps.gga.satellites);
+                setInitPos();
+            } else {
+                // Prendi la soglia impostata dall'utente
+                if (slider.readPercentage() != 0.0) {
+                    thrs = floor(slider.readPercentage() * 12.0 + 3.0);
+                }
+
+                // Calcola la distanza tra punto iniziale e attuale
+                d = distFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());
+
+                // Verifica che la soglia sia superata
+                if (d >= thrs) {
+                    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);
+                    setRGBColor(0, 1, 0);
+                }
+            }
+
         }
-        if ((waitData & MTK3339::NmeaVtg) != 0) {
-            waitData &= ~(MTK3339::NmeaVtg);
-            printf("vtg: course=%f, speed=%f km/h, mode=%c\n", 
-                gps.vtg.course, gps.vtg.speedKmHour, gps.vtg.mode);            
-        }   
-        
-        waitData &= (MTK3339::NmeaGga|MTK3339::NmeaVtg);
+
+        waitData &= MTK3339::NmeaGga;
     }
 
 
 
-}
\ No newline at end of file
+}