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@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?
User | Revision | Line number | New 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 | } |