File content as of revision 1:dfc738d8aba1:

#include "gps_event_vitesse.h"
//#define debug_gps  // permet d'avoir des informations de fonctionnement sur l'interface usb e, mode com.

#ifdef debug_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
                    if (!(lock==1) && !(lock==2)) lock = 0;
                    _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;
}