Subversion Repositories shark

#include <kernel/kern.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "nmea.h"

extern struct OUTDATA gNMEAdata;
static void do_lat_lon(char *sentence, int begin);
static void do_grid(void);
static char *field(char *sentence, short n);
static void update_field_i(char *sentence, int fld, int *dest, int mask);

void processGPRMC(char *sentence)
{
    char s[20], d[10];
    int tmp;

    sscanf(field(sentence, 9), "%s", d);        /* Date: ddmmyy */

    strncpy(s, d + 2, 2);       /* copy month */

    strncpy(s + 3, d, 2);       /* copy date */

    sscanf((d+4), "%2d", &tmp);

    /* Tf.: Window the year from 1970 to 2069. This buys us some time. */
    if (tmp < 70)
      strncpy(s + 6, "20", 2);  /* 21th century */
    else
      strncpy(s + 6, "19", 2);  /* 20th century */

    strncpy(s + 8, d + 4, 2);   /* copy year */

    sscanf(field(sentence, 1), "%s", d);        /* Time: hhmmss */

    strncpy(s + 11, d, 2);      /* copy hours */

    strncpy(s + 14, d + 2, 2);  /* copy minutes */

    strncpy(s + 17, d + 4, 2);  /* copy seconds */

    s[2] = s[5] = '/';          /* add the '/'s, ':'s, ' ' and string terminator */

    s[10] = ' ';
    s[13] = s[16] = ':';
    s[19] = '\0';

    strcpy(gNMEAdata.utc, s);

    /* A = valid, V = invalid */
    if (strcmp(field(sentence, 2), "V") == 0)
        gNMEAdata.status = 0;
#if 0    /* Let the GGA sentence do the update so we catch diff fixes */
    else
        gNMEAdata.status = 0;
#endif

    sscanf(field(sentence, 7), "%lf", &gNMEAdata.speed);
    sscanf(field(sentence, 8), "%lf", &gNMEAdata.track);

    do_lat_lon(sentence, 3);
    do_grid();

}

/* ----------------------------------------------------------------------- */

void processGPVTG(char *sentence)
{
    sscanf(field(sentence, 3), "%lf", &gNMEAdata.speed);
    sscanf(field(sentence, 1), "%lf", &gNMEAdata.track);
}

/* ----------------------------------------------------------------------- */

void processGPGGA(char *sentence)
{
    do_lat_lon(sentence, 2);
    do_grid();
    /* 0 = none, 1 = normal, 2 = diff */
    sscanf(field(sentence, 6), "%d", &gNMEAdata.status);
    if ((gNMEAdata.status > 0) && (gNMEAdata.mode < 2))
        gNMEAdata.mode = 2;
    if ((gNMEAdata.status < 1) && (gNMEAdata.mode > 1))
        gNMEAdata.mode = 1;
    sscanf(field(sentence, 7), "%d", &gNMEAdata.satellites);
    sscanf(field(sentence, 9), "%lf", &gNMEAdata.altitude);
}

/* ----------------------------------------------------------------------- */

void processGPGSA(char *sentence)
{

  /* 1 = none, 2 = 2d, 3 = 3d */
    sscanf(field(sentence, 2), "%d", &gNMEAdata.mode);
    if ((gNMEAdata.mode > 1) && (gNMEAdata.status < 1))
        gNMEAdata.status = 1;
    if ((gNMEAdata.mode < 2) && (gNMEAdata.status > 0))
        gNMEAdata.status = 0;
    sscanf(field(sentence, 15), "%lf", &gNMEAdata.pdop);
    sscanf(field(sentence, 16), "%lf", &gNMEAdata.hdop);
    sscanf(field(sentence, 17), "%lf", &gNMEAdata.vdop);
}

/* ----------------------------------------------------------------------- */

void processGPGSV(char *sentence)
{
    int n, m, f = 4;


    if (sscanf(field(sentence, 2), "%d", &n) < 1)
        return;
    update_field_i(sentence, 3, &gNMEAdata.in_view, C_SAT);

    n = (n - 1) * 4;
    m = n + 4;

    while (n < gNMEAdata.in_view && n < m) {
        update_field_i(sentence, f++, &gNMEAdata.PRN[n], C_SAT);
        update_field_i(sentence, f++, &gNMEAdata.elevation[n], C_SAT);
        update_field_i(sentence, f++, &gNMEAdata.azimuth[n], C_SAT);
        if (*(field(sentence, f)))
            update_field_i(sentence, f, &gNMEAdata.ss[n], C_SAT);
        f++;
        n++;
    }
}

/* ----------------------------------------------------------------------- */

void processPRWIZCH(char *sentence)
{
    int i;

    for (i = 0; i < 12; i++) {
        update_field_i(sentence, 2 * i + 1, &gNMEAdata.Zs[i], C_ZCH);
        update_field_i(sentence, 2 * i + 2, &gNMEAdata.Zv[i], C_ZCH);
    }
    gNMEAdata.ZCHseen = 1;
}

/* ----------------------------------------------------------------------- */

static void do_lat_lon(char *sentence, int begin)
{
    double lat, lon, d, m;
    char str[20], *p;
    int updated = 0;


    if (*(p = field(sentence, begin + 0)) != '\0') {
        strncpy(str, p, 20);
        sscanf(p, "%lf", &lat);
        m = 100.0 * modf(lat / 100.0, &d);
        lat = d + m / 60.0;
        p = field(sentence, begin + 1);
        if (*p == 'S')
            lat = -lat;
        if (gNMEAdata.latitude != lat) {
            gNMEAdata.latitude = lat;
            gNMEAdata.cmask |= C_LATLON;
        }
        updated++;
    }
    if (*(p = field(sentence, begin + 2)) != '\0') {
        strncpy(str, p, 20);
        sscanf(p, "%lf", &lon);
        m = 100.0 * modf(lon / 100.0, &d);
        lon = d + m / 60.0;

        p = field(sentence, begin + 3);
        if (*p == 'W')
            lon = -lon;
        if (gNMEAdata.longitude != lon) {
            gNMEAdata.longitude = lon;
            gNMEAdata.cmask |= C_LATLON;
        }
        updated++;
    }
    if (updated == 2)
        gNMEAdata.last_update = sys_gettime(NULL);
}

/* ----------------------------------------------------------------------- */

static void do_grid() {
  double lat, lon;

  lat = gNMEAdata.latitude;
  lon = gNMEAdata.longitude;

  gNMEAdata.grid[0] = 65 + (int)((180.0 + lon) / 20.0);
  gNMEAdata.grid[1] = 65 + (int)((90.0 + lat) / 10.0);
  gNMEAdata.grid[2] = 48 + (int)((180.0 + lon) / 2) % 10;
  gNMEAdata.grid[3] = 48 + (int)(90.0 + lat) % 10;
  gNMEAdata.grid[4] = 97 + ((int)(180.0 + lon) % 2) * 12 + (int)(((180.0 + lon) - (int)(180.0 + lon)) * 12);
  gNMEAdata.grid[5] = 97 + (int)(((90.0 + lat) - (int)(90.0 + lat)) * 24);
  gNMEAdata.grid[6] = '\0';
}

/* ----------------------------------------------------------------------- */

static void update_field_i(char *sentence, int fld, int *dest, int mask)
{
    int tmp;

    sscanf(field(sentence, fld), "%d", &tmp);

    if (tmp != *dest) {
        *dest = tmp;
        gNMEAdata.cmask |= mask;
    }
}

void process_message(char *sentence)
{
    if (checksum(sentence)) {
        if (strncmp(GPRMC, sentence, 5) == 0) {
            processGPRMC(sentence);
        } else if (strncmp(GPGGA, sentence, 5) == 0) {
            processGPGGA(sentence);
        } else if (strncmp(GPVTG, sentence, 5) == 0) {
            processGPVTG(sentence);
        } else if (strncmp(GPGSA, sentence, 5) == 0) {
            processGPGSA(sentence);
        } else if (strncmp(GPGSV, sentence, 5) == 0) {
            processGPGSV(sentence);
        } else if (strncmp(PRWIZCH, sentence, 7) == 0) {
            processPRWIZCH(sentence);
        } else {
            cprintf("Unknown sentence: \"%s\"\n",sentence);
        }
    } else {
        cprintf("Bad Checksum !\n");
    }
}

/* ----------------------------------------------------------------------- */

short checksum(char *sentence)
{
    unsigned char sum = '\0';
    char c, *p = sentence, csum[3];

    while ((c = *p++) != '*' && c != '\0')
        sum ^= c;

    sprintf(csum, "%02x", sum);
    return (strncmp(csum, p, 2) == 0);
}

void add_checksum(char *sentence)
{
    unsigned char sum = '\0';
    char c, *p = sentence;

    while ((c = *p++) != '*')
        sum ^= c;

    sprintf(p, "%02x\r\n", sum);
}

static char *field(char *sentence, short n)
{
    static char result[100];
    char *p = sentence;

    while (n-- > 0)
        while (*p++ != ',');
    strcpy(result, p);
    p = result;
    while (*p && *p != ',' && *p != '*' && *p != '\r')
        p++;

    *p = '\0';
    return result;
}