File content as of revision 1:d83f8332ebfe:

#include "mbed.h"
#include "drone.h"

RawSerial gps(p28, p27);

static char buf_gps[80];
static int count_gps = 0;

extern struct Status stat_gnd;

float gpsminsec2degree (float ms) { // dddmm.mmmm -> deg
    float deg;

    deg = (int)ms / 100;
    deg += (ms - (deg * 100)) / 60.0;
    return deg;
}

int dmy2ymd (int d) {
    if (! d) return 0;
    return ((2000 + (d % 100)) * 10000) + (((d / 100) % 100) * 100) + ((d / 10000) % 100);
}

void parseGpsNmea () {
    int i = 0;
    float f;
    char *buf = buf_gps;

    if (strncmp(buf, "$GPRMC", 6) == 0) {
        for (;;) {
            buf = strstr(buf, ",");
            if (buf == NULL) break;
            buf ++;
            f = atof(buf);
            i ++;
            switch (i) {
            case 1:
                stat_gnd.gps_time = f;
                break;
            case 9:
                stat_gnd.gps_date = dmy2ymd(f);
                break;
            }
        }
    } else
    if (strncmp(buf, "$GPGGA", 6) == 0) {
        for (;;) {
            buf = strstr(buf, ",");
            if (buf == NULL) break;
            buf ++;
            f = atof(buf);
            i ++;
            switch (i) {
            case 2:
                stat_gnd.gps_lat = gpsminsec2degree(f) * 10000000;
                break;
            case 3:
                if (buf[0] == 'S') stat_gnd.gps_lat = - stat_gnd.gps_lat;
                break;
            case 4:
                stat_gnd.gps_lng = gpsminsec2degree(f) * 10000000;
                break;
            case 5:
                if (buf[0] == 'W') stat_gnd.gps_lng = - stat_gnd.gps_lng;
                break;
            case 6:
                if (buf[0] != '0') {
                    stat_gnd.gps_flg = 1;
                } else {
                    stat_gnd.gps_lat = 0;
                    stat_gnd.gps_lng = 0;
                }
                break;
            case 7:
//                gps_cnt = f;
                break;
            case 9:
                stat_gnd.gps_h = f;
                break;
            }
        }
    } else
    if (strncmp(buf, "$GPGSV", 6) == 0) {
        for (;;) {
            buf = strstr(buf, ",");
            if (buf == NULL) break;
            buf ++;
            f = atof(buf);
            i ++;
            switch (i) {
            case 3:
                stat_gnd.gps_sat = f;
                break;
            }
        }
    } else
    if (strncmp(buf, "$GPGLL", 6) == 0) {
    }
}

void isrGps () {
    char c;
    c = gps.getc();
    if (c == '\r' || c == '\n') {
        buf_gps[count_gps] = 0;
        if (count_gps) {
            parseGpsNmea();
            count_gps = 0;
        }
    } else
    if (count_gps < sizeof(buf_gps) - 1) {
        buf_gps[count_gps] = c;
        count_gps ++;
    }
}

void initGps () {
    gps.baud(4800);
    gps.attach(isrGps);
}