File content as of revision 2:3a9cacc54417:

#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);

const double PI = 3.141592654; // Pi greco
const double R  = 6371000.0;   // Raggio della Terra in metri
const int    N  = 60;

double initPos[2] = {.0, .0};
double thrs       = 3.0;       // Soglia di allarme in metri
bool   set        = false;

static void dataAvailable()
{
    waitData |= gps.getAvailableDataType();
}

// 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* x, int n)
{
    return sqrt(var(x,n));
}

static void setInitPos()
{
    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);
        
        if (i>=N) {
            meanLat  = mean(coords[LAT], N);
            meanLon  = mean(coords[LON], N);
            stdevLat = stdev(coords[LAT], N);
            stdevLon = stdev(coords[LON], N);
            
            lcd.cls();
            lcd.printf("%f\n%f", stdevLat, stdevLon);
            
            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;
            }
        }
        
        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 d;

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

    while (1) {

        while (waitData == 0);

        //lcd.cls();

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

            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);
                }
            }

        }

        waitData &= MTK3339::NmeaGga;
    }



}