Diff: gps_event_vitesse.cpp

Revision:

0:ba9bedfe1044

Child:

1:dfc738d8aba1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gps_event_vitesse.cpp	Mon May 16 21:40:59 2016 +0000
@@ -0,0 +1,177 @@
+#include "gps_event_vitesse.h"
+//#define debug_gps  // permet d'avoir des informations de fonctionnement sur l'interface usb e, mode com.
+
+#ifdef debig_gps
+extern Serial pc;
+#endif
+
+Gps::Gps(PinName Txd, PinName Rxd) : Serial(Txd,Rxd)
+{
+    _num = 0;
+    _msg[0] = 0;
+    _time = 0; // date
+    _latitude = 0;
+    _ns = 0; // nombre satellite.
+    _longitude = 0;
+    _ew = 0;
+    _lock = 0;
+    _old_lock = 0;
+    _vitesse_gps_ok = 0;
+    _gps_data_ok = false;
+    attach(this,&Gps::receive, Serial::RxIrq);
+}
+
+void Gps::receive()
+{
+    char data;
+    if(readable()) {
+        data = getc();
+        switch (data) {
+            case '$' :  // debut de phrase Gps
+                _num = 1;
+                _msg [0] = '$';
+                _msg [1] = 0;
+                break;
+            case '\f' :
+                break;
+            case '\r' : // fin de phrase Gps
+                double latitude, longitude, time;
+                char status;
+                double speed;
+                int lock;
+                if(sscanf(_msg, "$GPGGA,%lf,%lf,%c,%lf,%c,%d,%d", &time, &latitude, &_ns, &longitude, &_ew, &lock,&_nbsattelite) >= 1) {
+#ifdef debug_gps
+                    pc.printf("Message %s\r\n",_msg);
+#endif
+                    _old_lock = _lock;
+                    _lock = lock;
+                    _old_time = _time;
+                    _time = time;
+                    _old_latitude = _latitude;
+                    _latitude = latitude;
+                    _old_longitude = _longitude;
+                    _longitude = longitude;
+                    if(_ns == 'S')  _latitude  *= -1.0;
+                    if(_ew == 'W') _longitude *= -1.0;
+                    double degrees = _trunc(_latitude / 100.0f);
+                    double minutes = _latitude - (degrees * 100.0f);
+                    _latitude = degrees + minutes / 60.0f;
+                    degrees = _trunc(_longitude / 100.0f);
+                    minutes = _longitude - (degrees * 100.0f);
+                    _longitude = degrees + minutes / 60.0f;
+                    _gps_data_ok = true;
+                    _num = 0;
+                    _msg[0] = 0;
+                } else if(sscanf(_msg, "$GPRMC,%lf,%c,%lf,%c,%lf,%c,%lf", &time,&status, &latitude, &_ns, &longitude, &_ew, &speed) >= 1) {
+#ifdef debug_gps
+                    pc.printf("Message %s\r\n",_msg);
+#endif
+                    _vitesse_gps_ok = (status == 'A');
+                    if (_vitesse_gps_ok) {
+                        speed = speed * 4.98f / 2.69f;
+                        _vitesse_gps = (float) speed;
+                    } else _vitesse_gps = 1000;
+                }
+                break;
+            default:
+                if ((_num < trame_nmea_max)&&(_msg [0] == '$')) {
+                    _msg [_num++] = data;
+                    _msg [_num] = 0;
+                }
+        }
+    }
+}
+
+double Gps::_trunc(double v)
+{
+    if(v < 0.0) {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}
+double Gps::time()
+{
+    return _time;
+}
+
+double Gps::latitude()
+{
+    return _latitude;
+}
+char Gps::ns()
+{
+    return _ns;
+}
+
+double Gps::longitude()
+{
+    return _longitude;
+}
+char Gps::ew()
+{
+    return _ew;
+}
+int Gps::lock()
+{
+    return _lock;
+}
+int Gps::nbsattelite()
+{
+    return _nbsattelite;
+}
+
+char Gps::sample()
+{
+    if (_gps_data_ok) {
+        _gps_data_ok = false;
+        return true;
+    } else return false;
+}
+double Gps::_time_sec(double time)
+{
+    double heure, minute, seconde;
+    heure = _trunc(time/10000);
+    minute = _trunc((time-heure*10000)/100);
+    seconde = time - heure*10000 - minute *100;
+    return (heure * 3600.0 + minute * 60 + seconde);
+}
+float Gps::vitesse()
+{
+    double vitesse, duree;
+    double latitude_rad;
+    double old_latitude_rad;
+    double longitude_rad;
+    double old_longitude_rad;
+    if (vitesse_ok()) {
+        latitude_rad = pi * _latitude /180.0f;
+        old_latitude_rad = pi * _old_latitude / 180.0f;
+        longitude_rad = pi * _longitude / 180.0f;
+        old_longitude_rad = pi * _old_longitude /180.0f;
+        vitesse = acos(sin(latitude_rad)*sin(old_latitude_rad)+cos(latitude_rad)*cos(old_latitude_rad)*cos(longitude_rad-old_longitude_rad));
+        vitesse = vitesse * rayon;
+        duree = _time_sec(_time) - _time_sec(_old_time);
+        if (duree!=0) vitesse = (vitesse / duree) *3.6f;
+        else vitesse = 2000.0;
+    } else vitesse = 1000.0;
+    return (float) vitesse;
+}
+char Gps::vitesse_ok()
+{
+    char vitesse_ok;
+    vitesse_ok = (_lock && _old_lock);
+    return vitesse_ok;
+}
+float Gps::vitesse_gps()
+{
+    return _vitesse_gps;
+}
+char Gps::vitesse_gps_ok()
+{
+    char vitesse_ok;
+    vitesse_ok = (_lock && _vitesse_gps_ok);
+    return vitesse_ok;
+}
\ No newline at end of file