Class pour trame Nmea.
Dependents: Test_gps_event_vitesse_bluetooth gpsmpu60500 Test_gps_event_vitesse_bluetooth_ok
Fork of gps_event_vitesse by
gps_event_vitesse.cpp
- Committer:
- schnf30
- Date:
- 2016-05-16
- Revision:
- 0:ba9bedfe1044
- Child:
- 1:dfc738d8aba1
File content as of revision 0:ba9bedfe1044:
#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; }