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@1:dfc738d8aba1, 2016-05-20 (annotated)

Committer:
schnf30
Date:
Fri May 20 16:24:50 2016 +0000
Revision:
1:dfc738d8aba1
Parent:
0:ba9bedfe1044 
Driver adapt? au EM-40- et 506.

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 1:dfc738d8aba1 4 #ifdef debug_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 1:dfc738d8aba1 46 if (!(lock==1) && !(lock==2)) lock = 0;
schnf30 0:ba9bedfe1044 47 _old_lock = _lock;
schnf30 0:ba9bedfe1044 48 _lock = lock;
schnf30 0:ba9bedfe1044 49 _old_time = _time;
schnf30 0:ba9bedfe1044 50 _time = time;
schnf30 0:ba9bedfe1044 51 _old_latitude = _latitude;
schnf30 0:ba9bedfe1044 52 _latitude = latitude;
schnf30 0:ba9bedfe1044 53 _old_longitude = _longitude;
schnf30 0:ba9bedfe1044 54 _longitude = longitude;
schnf30 0:ba9bedfe1044 55 if(_ns == 'S') _latitude *= -1.0;
schnf30 0:ba9bedfe1044 56 if(_ew == 'W') _longitude *= -1.0;
schnf30 0:ba9bedfe1044 57 double degrees = _trunc(_latitude / 100.0f);
schnf30 0:ba9bedfe1044 58 double minutes = _latitude - (degrees * 100.0f);
schnf30 0:ba9bedfe1044 59 _latitude = degrees + minutes / 60.0f;
schnf30 0:ba9bedfe1044 60 degrees = _trunc(_longitude / 100.0f);
schnf30 0:ba9bedfe1044 61 minutes = _longitude - (degrees * 100.0f);
schnf30 0:ba9bedfe1044 62 _longitude = degrees + minutes / 60.0f;
schnf30 0:ba9bedfe1044 63 _gps_data_ok = true;
schnf30 0:ba9bedfe1044 64 _num = 0;
schnf30 0:ba9bedfe1044 65 _msg[0] = 0;
schnf30 0:ba9bedfe1044 66 } else if(sscanf(_msg, "$GPRMC,%lf,%c,%lf,%c,%lf,%c,%lf", &time,&status, &latitude, &_ns, &longitude, &_ew, &speed) >= 1) {
schnf30 0:ba9bedfe1044 67 #ifdef debug_gps
schnf30 0:ba9bedfe1044 68 pc.printf("Message %s\r\n",_msg);
schnf30 0:ba9bedfe1044 69 #endif
schnf30 0:ba9bedfe1044 70 _vitesse_gps_ok = (status == 'A');
schnf30 0:ba9bedfe1044 71 if (_vitesse_gps_ok) {
schnf30 0:ba9bedfe1044 72 speed = speed * 4.98f / 2.69f;
schnf30 0:ba9bedfe1044 73 _vitesse_gps = (float) speed;
schnf30 0:ba9bedfe1044 74 } else _vitesse_gps = 1000;
schnf30 0:ba9bedfe1044 75 }
schnf30 0:ba9bedfe1044 76 break;
schnf30 0:ba9bedfe1044 77 default:
schnf30 0:ba9bedfe1044 78 if ((_num < trame_nmea_max)&&(_msg [0] == '$')) {
schnf30 0:ba9bedfe1044 79 _msg [_num++] = data;
schnf30 0:ba9bedfe1044 80 _msg [_num] = 0;
schnf30 0:ba9bedfe1044 81 }
schnf30 0:ba9bedfe1044 82 }
schnf30 0:ba9bedfe1044 83 }
schnf30 0:ba9bedfe1044 84 }
schnf30 0:ba9bedfe1044 85
schnf30 0:ba9bedfe1044 86 double Gps::_trunc(double v)
schnf30 0:ba9bedfe1044 87 {
schnf30 0:ba9bedfe1044 88 if(v < 0.0) {
schnf30 0:ba9bedfe1044 89 v*= -1.0;
schnf30 0:ba9bedfe1044 90 v = floor(v);
schnf30 0:ba9bedfe1044 91 v*=-1.0;
schnf30 0:ba9bedfe1044 92 } else {
schnf30 0:ba9bedfe1044 93 v = floor(v);
schnf30 0:ba9bedfe1044 94 }
schnf30 0:ba9bedfe1044 95 return v;
schnf30 0:ba9bedfe1044 96 }
schnf30 0:ba9bedfe1044 97 double Gps::time()
schnf30 0:ba9bedfe1044 98 {
schnf30 0:ba9bedfe1044 99 return _time;
schnf30 0:ba9bedfe1044 100 }
schnf30 0:ba9bedfe1044 101
schnf30 0:ba9bedfe1044 102 double Gps::latitude()
schnf30 0:ba9bedfe1044 103 {
schnf30 0:ba9bedfe1044 104 return _latitude;
schnf30 0:ba9bedfe1044 105 }
schnf30 0:ba9bedfe1044 106 char Gps::ns()
schnf30 0:ba9bedfe1044 107 {
schnf30 0:ba9bedfe1044 108 return _ns;
schnf30 0:ba9bedfe1044 109 }
schnf30 0:ba9bedfe1044 110
schnf30 0:ba9bedfe1044 111 double Gps::longitude()
schnf30 0:ba9bedfe1044 112 {
schnf30 0:ba9bedfe1044 113 return _longitude;
schnf30 0:ba9bedfe1044 114 }
schnf30 0:ba9bedfe1044 115 char Gps::ew()
schnf30 0:ba9bedfe1044 116 {
schnf30 0:ba9bedfe1044 117 return _ew;
schnf30 0:ba9bedfe1044 118 }
schnf30 0:ba9bedfe1044 119 int Gps::lock()
schnf30 0:ba9bedfe1044 120 {
schnf30 0:ba9bedfe1044 121 return _lock;
schnf30 0:ba9bedfe1044 122 }
schnf30 0:ba9bedfe1044 123 int Gps::nbsattelite()
schnf30 0:ba9bedfe1044 124 {
schnf30 0:ba9bedfe1044 125 return _nbsattelite;
schnf30 0:ba9bedfe1044 126 }
schnf30 0:ba9bedfe1044 127
schnf30 0:ba9bedfe1044 128 char Gps::sample()
schnf30 0:ba9bedfe1044 129 {
schnf30 0:ba9bedfe1044 130 if (_gps_data_ok) {
schnf30 0:ba9bedfe1044 131 _gps_data_ok = false;
schnf30 0:ba9bedfe1044 132 return true;
schnf30 0:ba9bedfe1044 133 } else return false;
schnf30 0:ba9bedfe1044 134 }
schnf30 0:ba9bedfe1044 135 double Gps::_time_sec(double time)
schnf30 0:ba9bedfe1044 136 {
schnf30 0:ba9bedfe1044 137 double heure, minute, seconde;
schnf30 0:ba9bedfe1044 138 heure = _trunc(time/10000);
schnf30 0:ba9bedfe1044 139 minute = _trunc((time-heure*10000)/100);
schnf30 0:ba9bedfe1044 140 seconde = time - heure*10000 - minute *100;
schnf30 0:ba9bedfe1044 141 return (heure * 3600.0 + minute * 60 + seconde);
schnf30 0:ba9bedfe1044 142 }
schnf30 0:ba9bedfe1044 143 float Gps::vitesse()
schnf30 0:ba9bedfe1044 144 {
schnf30 0:ba9bedfe1044 145 double vitesse, duree;
schnf30 0:ba9bedfe1044 146 double latitude_rad;
schnf30 0:ba9bedfe1044 147 double old_latitude_rad;
schnf30 0:ba9bedfe1044 148 double longitude_rad;
schnf30 0:ba9bedfe1044 149 double old_longitude_rad;
schnf30 0:ba9bedfe1044 150 if (vitesse_ok()) {
schnf30 0:ba9bedfe1044 151 latitude_rad = pi * _latitude /180.0f;
schnf30 0:ba9bedfe1044 152 old_latitude_rad = pi * _old_latitude / 180.0f;
schnf30 0:ba9bedfe1044 153 longitude_rad = pi * _longitude / 180.0f;
schnf30 0:ba9bedfe1044 154 old_longitude_rad = pi * _old_longitude /180.0f;
schnf30 0:ba9bedfe1044 155 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 156 vitesse = vitesse * rayon;
schnf30 0:ba9bedfe1044 157 duree = _time_sec(_time) - _time_sec(_old_time);
schnf30 0:ba9bedfe1044 158 if (duree!=0) vitesse = (vitesse / duree) *3.6f;
schnf30 0:ba9bedfe1044 159 else vitesse = 2000.0;
schnf30 0:ba9bedfe1044 160 } else vitesse = 1000.0;
schnf30 0:ba9bedfe1044 161 return (float) vitesse;
schnf30 0:ba9bedfe1044 162 }
schnf30 0:ba9bedfe1044 163 char Gps::vitesse_ok()
schnf30 0:ba9bedfe1044 164 {
schnf30 0:ba9bedfe1044 165 char vitesse_ok;
schnf30 0:ba9bedfe1044 166 vitesse_ok = (_lock && _old_lock);
schnf30 0:ba9bedfe1044 167 return vitesse_ok;
schnf30 0:ba9bedfe1044 168 }
schnf30 0:ba9bedfe1044 169 float Gps::vitesse_gps()
schnf30 0:ba9bedfe1044 170 {
schnf30 0:ba9bedfe1044 171 return _vitesse_gps;
schnf30 0:ba9bedfe1044 172 }
schnf30 0:ba9bedfe1044 173 char Gps::vitesse_gps_ok()
schnf30 0:ba9bedfe1044 174 {
schnf30 0:ba9bedfe1044 175 char vitesse_ok;
schnf30 0:ba9bedfe1044 176 vitesse_ok = (_lock && _vitesse_gps_ok);
schnf30 0:ba9bedfe1044 177 return vitesse_ok;
schnf30 0:ba9bedfe1044 178 }