modifiche posizione iniziale e sistema d'allarme.
Dependencies: MTK3339 TextLCD mbed tsi_sensor
Fork of app_gps by
main.cpp
- Committer:
- den90
- Date:
- 2015-07-13
- Revision:
- 6:c8efb299b0f0
- Parent:
- 5:244867f8fc75
- Child:
- 7:ffc4ef2442fa
File content as of revision 6:c8efb299b0f0:
#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 = 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); } 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) { 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>=2*N) { meanLat = mean(coords[LAT], N); meanLon = mean(coords[LON], N); stdevLat = stdev(coords[LAT], N); stdevLon = stdev(coords[LON], N); if (3*stdevLat <= 0.00001 && 3*stdevLon <= 0.00001) { initPos[LAT] = meanLat; initPos[LON] = meanLon; set = true; } lcd.cls(); lcd.printf("%f\n%f %d", stdevLat, stdevLon, i); /* 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 d1, d2; 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 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 (d2 >= 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; } }