File content as of revision 12:45cdb2b65f79:

#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(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);
static Serial pc(USBTX, USBRX);

const double PI = 3.141592654; // Pi greco
const double R  = 6371000.0;   // Raggio della Terra in metri
const int    N  = 100;         // Campioni da acquisire per determinare la posizione
const int SATS  = 5;           // Satelliti da fixare prima di acquisire la posizione

double initPos[2] = {.0, .0};  // Latitudine, Longitudine
double thrs       = 5.0;       // Soglia di allarme di default in metri
bool   set        = false;     // Posizione iniziale individuata

static void dataAvailable()
{
    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)
{
    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 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);
}

// Calcola la distanza tra 2 punti in metri (approssimazione equirettangolare)
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);
    double d = sqrt(x*x + y*y) * R;
    return isnan(d) ? 0.0 : d;
}

// Calcola la distanza tra punto iniziale e attuale in metri (approssimazione equirettangolare)
static double distThomasFromInit(double lat, double lon)
{
    return distThomas(initPos[LAT], initPos[LON], lat, lon);
}

// Calcola la media di un vettore
static double mean(double* x, int n)
{
    double sum = 0;
    double   c = 0;
    for (int i=0; i<n; i++) {
        if (!isnan(x[i])) {
            sum += x[i];
            c++;
        }
    }
    return sum / c;
}

// Calcola la varianza di un vettore
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));
}

// Calcola la deviazione standard di un vettore
static double stdev(double* x, int n)
{
    return sqrt(var(x,n));
}

// Determina la posizione iniziale
static void setInitPos()
{
    double coords[2][N], lat, lon, meanLat, meanLon, stdevLat, stdevLon;
    int i = 0;

    while (!set && gps.gga.satellites>=SATS) {

        lat = gps.getLatitudeAsDegrees();
        lon = gps.getLongitudeAsDegrees();

        wait(1);

        if (!isnan(lat+lon)) {
            coords[LAT][i%N] = lat;
            coords[LON][i%N] = lon;

            lcd.cls();

            if (i >= 2*N) {
                meanLat  = mean(coords[LAT], N);
                meanLon  = mean(coords[LON], N);
                stdevLat = stdev(coords[LAT], N);
                stdevLon = stdev(coords[LON], N);

                lcd.printf("ACCURACY=%0.2fm\nTIME=%ds GPS=%d", distThomas(0, 0, 3*stdevLat, 3*stdevLon), i-2*N, gps.gga.satellites);
                if (distThomas(0, 0, 3*stdevLat, 3*stdevLon) < 2.0) {
                    initPos[LAT] = meanLat;
                    initPos[LON] = meanLon;
                    set = true;
                }
            } else {
                lcd.printf("Acquiring\nposition (%ds)", 2*N-i);
            }

            i++;
        }

    }
}

int main(void)
{
    double d;
    int c = 0;

    gps.start(&dataAvailable, MTK3339::NmeaGga);

    while (1) {

        while (waitData == 0);

        lcd.cls();

        if ((waitData & MTK3339::NmeaGga) != 0) {
            waitData &= ~(MTK3339::NmeaGga);

            if (gps.gga.satellites < SATS) {
                lcd.printf("Fixing\nsats (%d/%d)", gps.gga.satellites, SATS);
                setRGBColor(1, 1, 0);
                c = 0;
            } else if (!set) {
                // La prima volta prendi la posizione iniziale
                lcd.printf("Acquiring\nposition...");
                setInitPos();
            } else {
                // Prendi la soglia impostata dall'utente
                if (slider.readPercentage() != 0.0) {
                    thrs = floor(slider.readPercentage() * 13.0 + 3.0);
                }

                // Calcola la distanza tra punto iniziale e attuale
                d = distThomasFromInit(gps.getLatitudeAsDegrees(), gps.getLongitudeAsDegrees());

                // Verifica che la soglia sia superata
                if (d > thrs) {
                    if (++c >= 15) {
                        lcd.printf("THR=%0.0fm SATS=%d\nDIST=%2.1fm ALARM", thrs, gps.gga.satellites, d);
                        setRGBColor(1, 0, 0);
                    } else {
                        lcd.printf("THR=%0.0fm SATS=%d\nDIST=%2.1fm    OK", thrs, gps.gga.satellites, thrs);
                        setRGBColor(1, 0, 0);
                    }
                } else {
                    lcd.printf("THR=%0.0fm SATS=%d\nDIST=%2.1fm    OK", thrs, gps.gga.satellites, d);
                    setRGBColor(0, 1, 0);
                    c = 0;
                }
            }

        }
        waitData &= MTK3339::NmeaGga;
    }
}