schneider françois / gps_event_vitesse

Class pour trame Nmea.

Dependents:   Test_gps_event_vitesse_bluetooth gpsmpu60500 Test_gps_event_vitesse_bluetooth_ok

Fork of gps_event_vitesse by schneider françois

gps_event_vitesse.cpp@0:ba9bedfe1044, 2016-05-16 (annotated)

Committer:
schnf30
Date:
Mon May 16 21:40:59 2016 +0000
Revision:
0:ba9bedfe1044
Child:
1:dfc738d8aba1
Les libraires sont event et se base sur la reception de trame NMEA. ; 2 m?thodes sont utilis?es pour mesur?es la vitesse.; - M?thode 1 calcule a partir des angles et de l mesure de temps.; - M?thode 2 acquisition ? partir de la trame GPRMC.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
schnf30 0:ba9bedfe1044 1 #include "gps_event_vitesse.h"
schnf30 0:ba9bedfe1044 2 //#define debug_gps // permet d'avoir des informations de fonctionnement sur l'interface usb e, mode com.
schnf30 0:ba9bedfe1044 3
schnf30 0:ba9bedfe1044 4 #ifdef debig_gps
schnf30 0:ba9bedfe1044 5 extern Serial pc;
schnf30 0:ba9bedfe1044 6 #endif
schnf30 0:ba9bedfe1044 7
schnf30 0:ba9bedfe1044 8 Gps::Gps(PinName Txd, PinName Rxd) : Serial(Txd,Rxd)
schnf30 0:ba9bedfe1044 9 {
schnf30 0:ba9bedfe1044 10 _num = 0;
schnf30 0:ba9bedfe1044 11 _msg[0] = 0;
schnf30 0:ba9bedfe1044 12 _time = 0; // date
schnf30 0:ba9bedfe1044 13 _latitude = 0;
schnf30 0:ba9bedfe1044 14 _ns = 0; // nombre satellite.
schnf30 0:ba9bedfe1044 15 _longitude = 0;
schnf30 0:ba9bedfe1044 16 _ew = 0;
schnf30 0:ba9bedfe1044 17 _lock = 0;
schnf30 0:ba9bedfe1044 18 _old_lock = 0;
schnf30 0:ba9bedfe1044 19 _vitesse_gps_ok = 0;
schnf30 0:ba9bedfe1044 20 _gps_data_ok = false;
schnf30 0:ba9bedfe1044 21 attach(this,&Gps::receive, Serial::RxIrq);
schnf30 0:ba9bedfe1044 22 }
schnf30 0:ba9bedfe1044 23
schnf30 0:ba9bedfe1044 24 void Gps::receive()
schnf30 0:ba9bedfe1044 25 {
schnf30 0:ba9bedfe1044 26 char data;
schnf30 0:ba9bedfe1044 27 if(readable()) {
schnf30 0:ba9bedfe1044 28 data = getc();
schnf30 0:ba9bedfe1044 29 switch (data) {
schnf30 0:ba9bedfe1044 30 case '$' : // debut de phrase Gps
schnf30 0:ba9bedfe1044 31 _num = 1;
schnf30 0:ba9bedfe1044 32 _msg [0] = '$';
schnf30 0:ba9bedfe1044 33 _msg [1] = 0;
schnf30 0:ba9bedfe1044 34 break;
schnf30 0:ba9bedfe1044 35 case '\f' :
schnf30 0:ba9bedfe1044 36 break;
schnf30 0:ba9bedfe1044 37 case '\r' : // fin de phrase Gps
schnf30 0:ba9bedfe1044 38 double latitude, longitude, time;
schnf30 0:ba9bedfe1044 39 char status;
schnf30 0:ba9bedfe1044 40 double speed;
schnf30 0:ba9bedfe1044 41 int lock;
schnf30 0:ba9bedfe1044 42 if(sscanf(_msg, "$GPGGA,%lf,%lf,%c,%lf,%c,%d,%d", &time, &latitude, &_ns, &longitude, &_ew, &lock,&_nbsattelite) >= 1) {
schnf30 0:ba9bedfe1044 43 #ifdef debug_gps
schnf30 0:ba9bedfe1044 44 pc.printf("Message %s\r\n",_msg);
schnf30 0:ba9bedfe1044 45 #endif
schnf30 0:ba9bedfe1044 46 _old_lock = _lock;
schnf30 0:ba9bedfe1044 47 _lock = lock;
schnf30 0:ba9bedfe1044 48 _old_time = _time;
schnf30 0:ba9bedfe1044 49 _time = time;
schnf30 0:ba9bedfe1044 50 _old_latitude = _latitude;
schnf30 0:ba9bedfe1044 51 _latitude = latitude;
schnf30 0:ba9bedfe1044 52 _old_longitude = _longitude;
schnf30 0:ba9bedfe1044 53 _longitude = longitude;
schnf30 0:ba9bedfe1044 54 if(_ns == 'S') _latitude *= -1.0;
schnf30 0:ba9bedfe1044 55 if(_ew == 'W') _longitude *= -1.0;
schnf30 0:ba9bedfe1044 56 double degrees = _trunc(_latitude / 100.0f);
schnf30 0:ba9bedfe1044 57 double minutes = _latitude - (degrees * 100.0f);
schnf30 0:ba9bedfe1044 58 _latitude = degrees + minutes / 60.0f;
schnf30 0:ba9bedfe1044 59 degrees = _trunc(_longitude / 100.0f);
schnf30 0:ba9bedfe1044 60 minutes = _longitude - (degrees * 100.0f);
schnf30 0:ba9bedfe1044 61 _longitude = degrees + minutes / 60.0f;
schnf30 0:ba9bedfe1044 62 _gps_data_ok = true;
schnf30 0:ba9bedfe1044 63 _num = 0;
schnf30 0:ba9bedfe1044 64 _msg[0] = 0;
schnf30 0:ba9bedfe1044 65 } else if(sscanf(_msg, "$GPRMC,%lf,%c,%lf,%c,%lf,%c,%lf", &time,&status, &latitude, &_ns, &longitude, &_ew, &speed) >= 1) {
schnf30 0:ba9bedfe1044 66 #ifdef debug_gps
schnf30 0:ba9bedfe1044 67 pc.printf("Message %s\r\n",_msg);
schnf30 0:ba9bedfe1044 68 #endif
schnf30 0:ba9bedfe1044 69 _vitesse_gps_ok = (status == 'A');
schnf30 0:ba9bedfe1044 70 if (_vitesse_gps_ok) {
schnf30 0:ba9bedfe1044 71 speed = speed * 4.98f / 2.69f;
schnf30 0:ba9bedfe1044 72 _vitesse_gps = (float) speed;
schnf30 0:ba9bedfe1044 73 } else _vitesse_gps = 1000;
schnf30 0:ba9bedfe1044 74 }
schnf30 0:ba9bedfe1044 75 break;
schnf30 0:ba9bedfe1044 76 default:
schnf30 0:ba9bedfe1044 77 if ((_num < trame_nmea_max)&&(_msg [0] == '$')) {
schnf30 0:ba9bedfe1044 78 _msg [_num++] = data;
schnf30 0:ba9bedfe1044 79 _msg [_num] = 0;
schnf30 0:ba9bedfe1044 80 }
schnf30 0:ba9bedfe1044 81 }
schnf30 0:ba9bedfe1044 82 }
schnf30 0:ba9bedfe1044 83 }
schnf30 0:ba9bedfe1044 84
schnf30 0:ba9bedfe1044 85 double Gps::_trunc(double v)
schnf30 0:ba9bedfe1044 86 {
schnf30 0:ba9bedfe1044 87 if(v < 0.0) {
schnf30 0:ba9bedfe1044 88 v*= -1.0;
schnf30 0:ba9bedfe1044 89 v = floor(v);
schnf30 0:ba9bedfe1044 90 v*=-1.0;
schnf30 0:ba9bedfe1044 91 } else {
schnf30 0:ba9bedfe1044 92 v = floor(v);
schnf30 0:ba9bedfe1044 93 }
schnf30 0:ba9bedfe1044 94 return v;
schnf30 0:ba9bedfe1044 95 }
schnf30 0:ba9bedfe1044 96 double Gps::time()
schnf30 0:ba9bedfe1044 97 {
schnf30 0:ba9bedfe1044 98 return _time;
schnf30 0:ba9bedfe1044 99 }
schnf30 0:ba9bedfe1044 100
schnf30 0:ba9bedfe1044 101 double Gps::latitude()
schnf30 0:ba9bedfe1044 102 {
schnf30 0:ba9bedfe1044 103 return _latitude;
schnf30 0:ba9bedfe1044 104 }
schnf30 0:ba9bedfe1044 105 char Gps::ns()
schnf30 0:ba9bedfe1044 106 {
schnf30 0:ba9bedfe1044 107 return _ns;
schnf30 0:ba9bedfe1044 108 }
schnf30 0:ba9bedfe1044 109
schnf30 0:ba9bedfe1044 110 double Gps::longitude()
schnf30 0:ba9bedfe1044 111 {
schnf30 0:ba9bedfe1044 112 return _longitude;
schnf30 0:ba9bedfe1044 113 }
schnf30 0:ba9bedfe1044 114 char Gps::ew()
schnf30 0:ba9bedfe1044 115 {
schnf30 0:ba9bedfe1044 116 return _ew;
schnf30 0:ba9bedfe1044 117 }
schnf30 0:ba9bedfe1044 118 int Gps::lock()
schnf30 0:ba9bedfe1044 119 {
schnf30 0:ba9bedfe1044 120 return _lock;
schnf30 0:ba9bedfe1044 121 }
schnf30 0:ba9bedfe1044 122 int Gps::nbsattelite()
schnf30 0:ba9bedfe1044 123 {
schnf30 0:ba9bedfe1044 124 return _nbsattelite;
schnf30 0:ba9bedfe1044 125 }
schnf30 0:ba9bedfe1044 126
schnf30 0:ba9bedfe1044 127 char Gps::sample()
schnf30 0:ba9bedfe1044 128 {
schnf30 0:ba9bedfe1044 129 if (_gps_data_ok) {
schnf30 0:ba9bedfe1044 130 _gps_data_ok = false;
schnf30 0:ba9bedfe1044 131 return true;
schnf30 0:ba9bedfe1044 132 } else return false;
schnf30 0:ba9bedfe1044 133 }
schnf30 0:ba9bedfe1044 134 double Gps::_time_sec(double time)
schnf30 0:ba9bedfe1044 135 {
schnf30 0:ba9bedfe1044 136 double heure, minute, seconde;
schnf30 0:ba9bedfe1044 137 heure = _trunc(time/10000);
schnf30 0:ba9bedfe1044 138 minute = _trunc((time-heure*10000)/100);
schnf30 0:ba9bedfe1044 139 seconde = time - heure*10000 - minute *100;
schnf30 0:ba9bedfe1044 140 return (heure * 3600.0 + minute * 60 + seconde);
schnf30 0:ba9bedfe1044 141 }
schnf30 0:ba9bedfe1044 142 float Gps::vitesse()
schnf30 0:ba9bedfe1044 143 {
schnf30 0:ba9bedfe1044 144 double vitesse, duree;
schnf30 0:ba9bedfe1044 145 double latitude_rad;
schnf30 0:ba9bedfe1044 146 double old_latitude_rad;
schnf30 0:ba9bedfe1044 147 double longitude_rad;
schnf30 0:ba9bedfe1044 148 double old_longitude_rad;
schnf30 0:ba9bedfe1044 149 if (vitesse_ok()) {
schnf30 0:ba9bedfe1044 150 latitude_rad = pi * _latitude /180.0f;
schnf30 0:ba9bedfe1044 151 old_latitude_rad = pi * _old_latitude / 180.0f;
schnf30 0:ba9bedfe1044 152 longitude_rad = pi * _longitude / 180.0f;
schnf30 0:ba9bedfe1044 153 old_longitude_rad = pi * _old_longitude /180.0f;
schnf30 0:ba9bedfe1044 154 vitesse = acos(sin(latitude_rad)*sin(old_latitude_rad)+cos(latitude_rad)*cos(old_latitude_rad)*cos(longitude_rad-old_longitude_rad));
schnf30 0:ba9bedfe1044 155 vitesse = vitesse * rayon;
schnf30 0:ba9bedfe1044 156 duree = _time_sec(_time) - _time_sec(_old_time);
schnf30 0:ba9bedfe1044 157 if (duree!=0) vitesse = (vitesse / duree) *3.6f;
schnf30 0:ba9bedfe1044 158 else vitesse = 2000.0;
schnf30 0:ba9bedfe1044 159 } else vitesse = 1000.0;
schnf30 0:ba9bedfe1044 160 return (float) vitesse;
schnf30 0:ba9bedfe1044 161 }
schnf30 0:ba9bedfe1044 162 char Gps::vitesse_ok()
schnf30 0:ba9bedfe1044 163 {
schnf30 0:ba9bedfe1044 164 char vitesse_ok;
schnf30 0:ba9bedfe1044 165 vitesse_ok = (_lock && _old_lock);
schnf30 0:ba9bedfe1044 166 return vitesse_ok;
schnf30 0:ba9bedfe1044 167 }
schnf30 0:ba9bedfe1044 168 float Gps::vitesse_gps()
schnf30 0:ba9bedfe1044 169 {
schnf30 0:ba9bedfe1044 170 return _vitesse_gps;
schnf30 0:ba9bedfe1044 171 }
schnf30 0:ba9bedfe1044 172 char Gps::vitesse_gps_ok()
schnf30 0:ba9bedfe1044 173 {
schnf30 0:ba9bedfe1044 174 char vitesse_ok;
schnf30 0:ba9bedfe1044 175 vitesse_ok = (_lock && _vitesse_gps_ok);
schnf30 0:ba9bedfe1044 176 return vitesse_ok;
schnf30 0:ba9bedfe1044 177 }