File content as of revision 8:c285a6043b98:

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

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

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

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

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;
    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 >= 2*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("ACCURACY (%d)\n%f m", i, distThomas(0, 0, 3*stdevLat, 3*stdevLon));
            
            if (distThomas(0, 0, 3*stdevLat, 3*stdevLon) < 3) {
                initPos[LAT] = coords[LAT][i%N];
                initPos[LON] = coords[LON][i%N];
                set = true;
            }
        }
        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 < 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() * 7.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 >= 5 ){
                        lcd.printf("TH=%2.0fm SATS=%d\nDIST=%2.1fm ALARM", thrs, gps.gga.satellites, d);
                        setRGBColor(1, 0, 0);
                    } else {
                        c++;
                    }
                } else {
                    lcd.printf("TH=%2.0fm SATS=%d\nDIST=%2.1fm OK", thrs, gps.gga.satellites, d);
                    setRGBColor(0, 1, 0);
                    c=0;
                }
            }

        }
        waitData &= MTK3339::NmeaGga;
    }
}