NMEA HAL层 与 java层 的接口

NMEA--- National Marine Electronics Association, GPS导航专用名词～

HAL 层代码如下：

#define NMEA_MAX_SIZE 83

typedef struct {
    int pos;
    int overflow;
    int utc_year;
    int utc_mon;
    int utc_day;
    int utc_diff;
    GpsLocation fix;
    GpsStatus    gps_status;
#ifdef Svpnd_Version    
    GpsCallbacks callback;
    GpsSvStatus sv_status;
    int sv_status_changed;
#else
    gps_location_callback callback;
#endif
    char in[ NMEA_MAX_SIZE+1 ];
} NmeaReader;

#define MAX_NMEA_TOKENS 83

typedef struct {
    int count;
    Token tokens[ MAX_NMEA_TOKENS ];
} NmeaTokenizer;

static int
nmea_tokenizer_init( NmeaTokenizer* t, const char* p, const char* end )
{
    int count = 0;
    char* q;

    /* the initial '$' is optional */
    if (p < end && p[0] == '$')
        p += 1;

    /* remove trailing newline */
    if (end > p && end[-1] == '\n') {
        end -= 1;
        if (end > p && end[-1] == '\r')
            end -= 1;
    }

    /* get rid of checksum at the end of the sentecne */
    if (end >= p+3 && end[-3] == '*') {
        end -= 3;
    }

    while (p < end) {
        const char* q = p;

        q = memchr(p, ',', end-p);
        if (q == NULL)
            q = end;

        /*if (q > p) { */
            if (count < MAX_NMEA_TOKENS) {
                t->tokens[count].p = p;
                t->tokens[count].end = q;
                count += 1;
            }
        /*}*/
        if (q < end)
            q += 1;

        p = q;
    }

    t->count = count;
    return count;
}

static Token
nmea_tokenizer_get( NmeaTokenizer* t, int index )
{
    Token tok;
    static const char* dummy = "";

    if (index < 0 || index >= t->count) {
        tok.p = tok.end = dummy;
    } else
        tok = t->tokens[index];

    return tok;
}

/* this is the state of our connection to the qemu_gpsd daemon */
typedef struct {
    int init;
    int fd; // UART FD

    #if GPS_GPIO_INCLUDE
    int fdGps; // GPS_GPIO FD

    #endif
    GpsCallbacks callbacks;
    pthread_t thread;
    int control[2];
    char device[32];
    sem_t fix_sem;
    
    /* QemuChannel channel; // We are not use QEMU */
} GpsState;

static void
nmea_reader_update_utc_diff( NmeaReader* r )
{
    time_t now = time(NULL);
    struct tm tm_local;
    struct tm tm_utc;
    long time_local, time_utc;

    gmtime_r( &now, &tm_utc );
    localtime_r( &now, &tm_local );

    time_local = tm_local.tm_sec +
                 60*(tm_local.tm_min +
                 60*(tm_local.tm_hour +
                 24*(tm_local.tm_yday +
                 365*tm_local.tm_year)));

    time_utc = tm_utc.tm_sec +
               60*(tm_utc.tm_min +
               60*(tm_utc.tm_hour +
               24*(tm_utc.tm_yday +
               365*tm_utc.tm_year)));

    r->utc_diff = time_utc - time_local;
}


static void
nmea_reader_init( NmeaReader* r )
{
    GpsCallbacks empty;

    memset( r, 0, sizeof(*r) );

    r->pos = 0;
    r->overflow = 0;
    r->utc_year = -1;
    r->utc_mon = -1;
    r->utc_day = -1;
    
    #ifdef Svpnd_Version
     r->callback = empty;
    #else
     r->callback = NULL;
     #endif
     r->fix.size = sizeof(r->fix);
     r->gps_status.size =sizeof(r->gps_status);

    
    nmea_reader_update_utc_diff( r );
}



#ifdef Svpnd_Version
static void
nmea_reader_set_callback( NmeaReader* r, GpsCallbacks cb )
{
    r->callback.location_cb = cb.location_cb;
    r->callback.status_cb = cb.status_cb;
    r->callback.sv_status_cb = cb.sv_status_cb;
    
    if (r->fix.flags != 0) {
        D("%s: sending latest fix to new callback", __FUNCTION__);
        r->callback.location_cb( &r->fix );
        r->fix.flags = 0;
    }
}

#else
static void
nmea_reader_set_callback( NmeaReader* r, gps_location_callback cb )
{
    r->callback = cb;
    if (cb != NULL && r->fix.flags != 0) {
        D("%s: sending latest fix to new callback", __FUNCTION__);
        r->callback( &r->fix );
        r->fix.flags = 0;
    }
}

#endif

static int
nmea_reader_update_accuracy( NmeaReader* r,
                             Token accuracy )
{
    double acc;
    Token tok = accuracy;

    if (tok.p >= tok.end)
        return -1;

    r->fix.accuracy = str2float(tok.p, tok.end);

    if (r->fix.accuracy == 99.99){
      return 0;
    }

    r->fix.flags |= GPS_LOCATION_HAS_ACCURACY;
    return 0;
}


static int
nmea_reader_update_time( NmeaReader* r, Token tok )
{
    int hour, minute;
    double seconds;
    struct tm tm;
    time_t fix_time;

    if (tok.p + 6 > tok.end)
        return -1;

    if (r->utc_year < 0) {
        /* no date yet, get current one */
        time_t now = time(NULL);
        gmtime_r( &now, &tm );
        r->utc_year = tm.tm_year + 1900;
        r->utc_mon = tm.tm_mon + 1;
        r->utc_day = tm.tm_mday;
    }

    hour = str2int(tok.p, tok.p+2);
    minute = str2int(tok.p+2, tok.p+4);
    seconds = str2float(tok.p+4, tok.end);

    tm.tm_hour = hour;
    tm.tm_min = minute;
    tm.tm_sec = (int) seconds;
    tm.tm_year = r->utc_year - 1900;
    tm.tm_mon = r->utc_mon - 1;
    tm.tm_mday = r->utc_day;
    tm.tm_isdst = -1;

    fix_time = mktime( &tm ) + r->utc_diff;
    r->fix.timestamp = (long long)fix_time * 1000;
    return 0;
}

static int nmea_reader_update_cdate( NmeaReader* r, Token tok_d, Token tok_m, Token tok_y )
{
  if ( (tok_d.p + 2 > tok_d.end) || (tok_m.p + 2 > tok_m.end) || (tok_y.p + 4 > tok_y.end) )
    return -1;
  r->utc_day = str2int(tok_d.p, tok_d.p+2);
  r->utc_mon = str2int(tok_m.p, tok_m.p+2);
  r->utc_year = str2int(tok_y.p, tok_y.end+4);
  return 0;
}

static int
nmea_reader_update_date( NmeaReader* r, Token date, Token time )
{
    Token tok = date;
    int day, mon, year;

    if (tok.p + 6 != tok.end) {
        D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }
    day = str2int(tok.p, tok.p+2);
    mon = str2int(tok.p+2, tok.p+4);
    year = str2int(tok.p+4, tok.p+6) + 2000;

    if ((day|mon|year) < 0) {
        D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }

    r->utc_year = year;
    r->utc_mon = mon;
    r->utc_day = day;

    return nmea_reader_update_time( r, time );
}


static double
convert_from_hhmm( Token tok )
{
    double val = str2float(tok.p, tok.end);
    int degrees = (int)(floor(val) / 100);
    double minutes = val - degrees*100.;
    double dcoord = degrees + minutes / 60.0;
    return dcoord;
}


static int
nmea_reader_update_latlong( NmeaReader* r,
                            Token latitude,
                            char latitudeHemi,
                            Token longitude,
                            char longitudeHemi )
{
    double lat, lon;
    Token tok;

    tok = latitude;
    if (tok.p + 6 > tok.end) {
        D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }
    lat = convert_from_hhmm(tok);
    if (latitudeHemi == 'S')
        lat = -lat;

    tok = longitude;
    if (tok.p + 6 > tok.end) {
        D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }
    lon = convert_from_hhmm(tok);
    if (longitudeHemi == 'W')
        lon = -lon;

    r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG;
    r->fix.latitude = lat;
    r->fix.longitude = lon;
    return 0;
}


static int
nmea_reader_update_altitude( NmeaReader* r,
                             Token altitude,
                             Token units )
{
    double alt;
    Token tok = altitude;

    if (tok.p >= tok.end)
        return -1;

    r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
    r->fix.altitude = str2float(tok.p, tok.end);
    return 0;
}


static int
nmea_reader_update_bearing( NmeaReader* r,
                            Token bearing )
{
    double alt;
    Token tok = bearing;

    if (tok.p >= tok.end)
        return -1;

    r->fix.flags |= GPS_LOCATION_HAS_BEARING;
    r->fix.bearing = str2float(tok.p, tok.end);
    return 0;
}


static int
nmea_reader_update_speed( NmeaReader* r,
                          Token speed )
{
    double alt;
    Token tok = speed;

    if (tok.p >= tok.end)
        return -1;

    r->fix.flags |= GPS_LOCATION_HAS_SPEED;
    r->fix.speed = str2float(tok.p, tok.end);
    return 0;
}

static void
nmea_reader_parse( NmeaReader* r )
{
   /* we received a complete sentence, now parse it to generate
    * a new GPS fix...
    */
    NmeaTokenizer tzer[1];
    Token tok;

   D("Received: '%.*s'", r->pos, r->in);
    if (r->pos < 9) {
        D("Too short. discarded.");
        return;
    }

    nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#if GPS_DEBUG
    {
        int n;
        for (n = 0; n < tzer->count; n++) {
            Token tok = nmea_tokenizer_get(tzer,n);
        }
    }
#endif

    tok = nmea_tokenizer_get(tzer, 0);
    if (tok.p + 5 > tok.end) {
        return;
    }

    /* ignore first two characters.*/
    tok.p += 2;
    if ( !memcmp(tok.p, "GGA", 3) ) {
        /* GPS fix */
        Token tok_time = nmea_tokenizer_get(tzer,1);
        Token tok_latitude = nmea_tokenizer_get(tzer,2);
        Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
        Token tok_longitude = nmea_tokenizer_get(tzer,4);
        Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
        Token tok_altitude = nmea_tokenizer_get(tzer,9);
        Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);

        nmea_reader_update_time(r, tok_time);
        nmea_reader_update_latlong(r, tok_latitude,
                                      tok_latitudeHemi.p[0],
                                      tok_longitude,
                                      tok_longitudeHemi.p[0]);
        nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
    }else if ( !memcmp(tok.p, "GLL", 3) ) {
    Token tok_fixstaus = nmea_tokenizer_get(tzer,6);
    if (tok_fixstaus.p[0] == 'A') {
     Token tok_latitude = nmea_tokenizer_get(tzer,1);
     Token tok_latitudeHemi = nmea_tokenizer_get(tzer,2);
     Token tok_longitude = nmea_tokenizer_get(tzer,3);
     Token tok_longitudeHemi = nmea_tokenizer_get(tzer,4);
     Token tok_time = nmea_tokenizer_get(tzer,5);
     nmea_reader_update_time(r, tok_time);
     nmea_reader_update_latlong(r, tok_latitude, tok_latitudeHemi.p[0], tok_longitude, tok_longitudeHemi.p[0]);
    }
    }
    #ifdef Svpnd_Version
    else if ( !memcmp(tok.p, "GSV", 3) ) {
    Token tok_noSatellites = nmea_tokenizer_get(tzer, 3);
    int noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);
        
    if (noSatellites > 0) {
     Token tok_noSentences = nmea_tokenizer_get(tzer, 1);
     Token tok_sentence     = nmea_tokenizer_get(tzer, 2);
    
     int sentence = str2int(tok_sentence.p, tok_sentence.end);
     int totalSentences = str2int(tok_noSentences.p, tok_noSentences.end);
     int curr;
     int i;
            
     if (sentence == 1) {
     r->sv_status_changed = 0;
     r->sv_status.num_svs = 0;
     }
    
        curr = r->sv_status.num_svs;
    
        i = 0;
    
        while (i < 4 && r->sv_status.num_svs < noSatellites){
         Token    tok_prn = nmea_tokenizer_get(tzer, i * 4 + 4);
         Token    tok_elevation = nmea_tokenizer_get(tzer, i * 4 + 5);
         Token    tok_azimuth = nmea_tokenizer_get(tzer, i * 4 + 6);
         Token    tok_snr = nmea_tokenizer_get(tzer, i * 4 + 7);
    
         r->sv_status.sv_list[curr].prn = str2int(tok_prn.p, tok_prn.end);
         r->sv_status.sv_list[curr].elevation = str2float(tok_elevation.p, tok_elevation.end);
         r->sv_status.sv_list[curr].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
         r->sv_status.sv_list[curr].snr = str2float(tok_snr.p, tok_snr.end);
    
         r->sv_status.num_svs += 1;
    
         curr += 1;
    
         i += 1;
     }
    
     if (sentence == totalSentences) {
        r->sv_status_changed = 1;
     }
    
     D("%s: GSV message with total satellites %d", __FUNCTION__, noSatellites);
    
    }        
    
    }
    #endif
    
    else if ( !memcmp(tok.p, "GSA", 3) ) {
    #ifdef Svpnd_Version
        /* do something ? */
        {

        Token tok_fixStatus = nmea_tokenizer_get(tzer, 2);
        int i;

        if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1') {

            Token tok_accuracy = nmea_tokenizer_get(tzer, 15);

            nmea_reader_update_accuracy(r, tok_accuracy);

            r->sv_status.used_in_fix_mask = 0ul;

            for (i = 3; i <= 14; ++i){

                Token tok_prn = nmea_tokenizer_get(tzer, i);
                int prn = str2int(tok_prn.p, tok_prn.end);

                if (prn > 0){
                    r->sv_status.used_in_fix_mask |= (1ul << (32 - prn));
                    r->sv_status_changed = 1;
                    D("%s: fix mask is %d", __FUNCTION__, r->sv_status.used_in_fix_mask);
         D(" [log hit][%s:%d] fix.flags=0x%x ", __FUNCTION__, __LINE__, r->fix.flags);
               }

           }

       }

       D(" [log hit][%s:%d] fix.flags=0x%x ", __FUNCTION__, __LINE__, r->fix.flags);

    }
    #endif
        /* do something ? */
    } else if ( !memcmp(tok.p, "RMC", 3) ) {
        Token tok_time = nmea_tokenizer_get(tzer,1);
        Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
        Token tok_latitude = nmea_tokenizer_get(tzer,3);
        Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
        Token tok_longitude = nmea_tokenizer_get(tzer,5);
        Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
        Token tok_speed = nmea_tokenizer_get(tzer,7);
        Token tok_bearing = nmea_tokenizer_get(tzer,8);
        Token tok_date = nmea_tokenizer_get(tzer,9);

        D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
        if (tok_fixStatus.p[0] == 'A')
        {
            nmea_reader_update_date( r, tok_date, tok_time );

            nmea_reader_update_latlong( r, tok_latitude,
                                           tok_latitudeHemi.p[0],
                                           tok_longitude,
                                           tok_longitudeHemi.p[0] );

            nmea_reader_update_bearing( r, tok_bearing );
            nmea_reader_update_speed ( r, tok_speed );
        }
    }
    else if ( !memcmp(tok.p, "VTG", 3) ) {
     Token tok_fixStatus = nmea_tokenizer_get(tzer,9);
     if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != 'N') {
     Token tok_bearing = nmea_tokenizer_get(tzer,1);
     Token tok_speed = nmea_tokenizer_get(tzer,5);
     nmea_reader_update_bearing( r, tok_bearing );
     nmea_reader_update_speed ( r, tok_speed );
     }
    }
    else if ( !memcmp(tok.p, "ZDA", 3) ) {
     Token tok_time;
     Token tok_year = nmea_tokenizer_get(tzer,4);
     if (tok_year.p[0] != '\0') {
     Token tok_day = nmea_tokenizer_get(tzer,2);
     Token tok_mon = nmea_tokenizer_get(tzer,3);
     nmea_reader_update_cdate( r, tok_day, tok_mon, tok_year );
     }
     tok_time = nmea_tokenizer_get(tzer,1);
     if (tok_time.p[0] != '\0')
     nmea_reader_update_time(r, tok_time);
    }
    else {
     tok.p -= 2;
     D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
    }
    if (r->fix.flags != 0) {
#if GPS_DEBUG
        char temp[256];
        char* p = temp;
        char* end = p + sizeof(temp);
        struct tm utc;

        p += snprintf( p, end-p, "sending fix" );
        if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
            p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
            p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
            p += snprintf(p, end-p, " speed=%g", r->fix.speed);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
            p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
            p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
        }
        gmtime_r( (time_t*) &r->fix.timestamp, &utc );
        p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
        D(temp);
#endif
    #ifdef Svpnd_Version
     if (r->callback.location_cb) {
            r->callback.location_cb( &r->fix );
            r->fix.flags = 0;
        }
    
    #else
        if (r->callback) {
             r->callback( &r->fix );
             r->fix.flags = 0;
         }
        #endif
        
        else {
             /* D("no callback, keeping data until needed !"); */
        }
    }
    /* sv status !!! */
    #ifdef Svpnd_Version
     if (r->sv_status_changed != 0) {
           if (r->callback.sv_status_cb) {
                 r->callback.sv_status_cb( &(r->sv_status) );
               r->sv_status_changed = 0;
             }else {
               /* D("no callback, keeping status data until needed !");*/
           }

        }
     #endif
}


static void
nmea_reader_add( NmeaReader* r, char *buff,int count)
{
    char i,c;
for( i=0;i<count;i++)
{
   c=buff[i];
    if (r->overflow) {
        r->overflow = (c != '\n');
        return;
    }

    if (r->pos >= (int) sizeof(r->in)-1 ) {
        r->overflow = 1;
        r->pos = 0;
    return;
    }

    r->in[r->pos] = (char)c;
    r->pos += 1;

    if (c == '\n') {
         GPS_STATE_LOCK_FIX(gps_state);
        nmea_reader_parse( r );
        r->pos = 0;
     GPS_STATE_UNLOCK_FIX(gps_state);
    }
 }
}


static void
nmea_reader_addc( NmeaReader* r, int c )
{
    if (r->overflow)
   {
        r->overflow = (c != '\n');
        return;
    }

    if (r->pos >= (int) sizeof(r->in)-1 )
   {
        r->overflow = 1;
        r->pos = 0;
        return;
    }

    r->in[r->pos] = (char)c;
    r->pos += 1;

    if (c == '\n')
   {
         GPS_STATE_LOCK_FIX(gps_state);
        nmea_reader_parse( r );
        r->pos = 0;
     GPS_STATE_UNLOCK_FIX(gps_state);
    }
}