Diff: gps.cpp

Revision:

0:0bd73064b7f6

Child:

1:2f5fbb33ae8b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gps.cpp	Wed Mar 18 00:55:27 2015 +0000
@@ -0,0 +1,626 @@
+#include "mbed.h"
+#include "gps.h"
+
+const char NmeaDataTypeGPGGA[] = "GPGGA";
+//const char NmeaDataTypeGPGSA[] = "GPGSA";
+//const char NmeaDataTypeGPGSV[] = "GPGSV";
+const char NmeaDataTypeGPRMC[] = "GPRMC";
+
+/* Value used for the conversion of the position from DMS to decimal */
+const int32_t MaxNorthPosition = 8388607;       // 2^23 - 1
+const int32_t MaxSouthPosition = 8388608;       // -2^23
+const int32_t MaxEastPosition = 8388607;        // 2^23 - 1    
+const int32_t MaxWestPosition = 8388608;        // -2^23
+
+Serial gps_uart(PB_6, PB_7);
+DigitalOut gps_en(PB_11);
+InterruptIn pps_pin(PC_5);
+
+GPS::GPS()
+{
+ 
+}
+    
+GPS::~GPS()
+{
+}
+
+void GPS::pps()
+{
+    pps_occurred = true;
+}
+
+void GPS::on_uart_rx()
+{
+    //uint8_t *len = &rx_buf_lens[rx_bufs_in_idx];
+    uint8_t len = rx_buf_lens[rx_bufs_in_idx];
+    char ch;
+
+    do {
+        ch = gps_uart.getc();
+
+        if (ch != '$') {
+            if (len < RX_BUF_SIZE) {
+                rx_bufs[rx_bufs_in_idx][len] = ch;
+                rx_buf_lens[rx_bufs_in_idx]++;
+            } else
+                printf("S");
+        } else if (len != 0) {
+            printf("$ at %d\r\n", len);
+        }
+        
+        if (ch == '\n') {
+            rx_bufs[rx_bufs_in_idx][len] = 0;  // null terminate
+            if (++rx_bufs_in_idx == NUM_RX_BUFS)
+                rx_bufs_in_idx = 0;
+            /* just let it overflow -- if (rx_bufs_in_idx == rx_bufs_out_idx)
+                printf("gps overflow\r\n");*/
+            rx_buf_lens[rx_bufs_in_idx] = 0;
+        }
+    } while (gps_uart.readable());
+}
+
+void GPS::init()
+{
+    rx_bufs_in_idx = 0;
+    rx_buf_lens[rx_bufs_in_idx] = 0;
+    rx_bufs_out_idx = 0;
+
+    //gps_uart.baud(57600);
+    gps_uart.attach(this, &GPS::on_uart_rx);
+
+    pps_pin.rise(this, &GPS::pps);
+}
+
+#define DELAY_MAX   20
+void GPS::enable(bool en)
+{
+    if (en) {
+        /* probably better instead to remove 22uF from GPS VCC */
+        volatile int d;
+        volatile int i;
+        //volatile int d_on = 0;
+        for (d = DELAY_MAX; d > 0; d--) {
+            gps_en = 0;
+            /*for (i = 0; i < d_on; i++)
+                asm("nop");
+            if (d & 1)
+                d_on++;*/
+            gps_en = 1;
+            for (i = 0; i < d; i++) {
+                asm("nop");
+            }
+        }
+
+        for (d = 0; d < 10; d++) {
+            volatile int n;
+            for (n = 0; n < 3; n++) {
+                gps_en = 0;
+                for (i = 0; i < d; i++) {
+                    asm("nop");
+                }
+                gps_en = 1;
+                asm("nop");
+                asm("nop");
+                asm("nop");
+                asm("nop");
+            }
+        }
+        gps_en = 0;
+    } else {
+        gps_en = 1;
+    }
+    //gps_en = !en;
+    //printf("gps_en:%d\n", en);
+}
+
+bool GPS::enabled()
+{
+    return !gps_en.read();
+}
+
+/*char GPS::Nibble2HexChar( char a );
+{
+    if( a < 10 )
+    {
+        return '0' + a;
+    }
+    else if( a < 16 )
+    {
+        return 'A' + ( a - 10 );
+    }
+    else
+    {
+        return '?';
+    }
+}*/
+
+int GPS::NmeaChecksum( char *nmeaStr, uint8_t nmeaStrSize, char * checksum )
+{
+    int i = 0;
+    uint8_t checkNum = 0;
+
+    // Check input parameters
+    if( ( nmeaStr == NULL ) || ( checksum == NULL ) || ( nmeaStrSize <= 1 ) )
+    {
+        return -1;
+    }
+
+    // Skip the first '$' if necessary
+    if( nmeaStr[i] == '$' )
+    {
+        i += 1;
+    }
+
+    // XOR until '*' or max length is reached
+    while( nmeaStr[i] != '*' )
+    {
+        checkNum ^= nmeaStr[i];
+        i += 1;
+        if( i >= nmeaStrSize )
+        {
+            return -1;
+        }
+    }
+
+    // Convert checksum value to 2 hexadecimal characters
+    /*checksum[0] = Nibble2HexChar( checkNum / 16 ); // upper nibble
+    checksum[1] = Nibble2HexChar( checkNum % 16 ); // lower nibble
+    */
+    sprintf(checksum, "%02X", checkNum);
+
+    return i + 1;
+}
+
+bool GPS::NmeaValidateChecksum(int idx)
+{
+    int checksumIndex;
+    char checksum[4];
+    char *NmeaString = rx_bufs[idx];
+
+    checksumIndex = NmeaChecksum( NmeaString, rx_buf_lens[idx], checksum );
+
+    // could we calculate a verification checksum ?
+    if( checksumIndex < 0 )
+    {
+        printf("gps:checksumIndex:%d\r\n", checksumIndex);
+        return false;
+    }
+
+    // check if there are enough char in the serial buffer to read checksum
+    if( checksumIndex >= ( rx_buf_lens[idx] - 2 ) )
+    {
+        printf("gps:checksumIndex:%d\r\n", checksumIndex);
+        return false;
+    }
+
+    // check the checksum
+    if( ( NmeaString[checksumIndex] == checksum[0] ) && ( NmeaString[checksumIndex + 1] == checksum[1] ) )
+    {
+        return true;
+    }
+    else
+    {
+        if (verbose)
+            printf("gps:checksum fail idx:%d %c%c %c%c\r\n", checksumIndex, checksum[0], checksum[1], NmeaString[checksumIndex], NmeaString[checksumIndex+1]);
+        return false;
+    }
+}
+
+void GPS::ConvertPositionFromStringToNumerical( )
+{
+    int i;
+
+    double valueTmp1;
+    double valueTmp2;
+    double valueTmp3;
+    double valueTmp4;
+
+    // Convert the latitude from ASCII to uint8_t values
+    for( i = 0 ; i < 10 ; i++ )
+    {
+        NmeaGpsData.NmeaLatitude[i] = NmeaGpsData.NmeaLatitude[i] & 0xF;  
+    }
+    // Convert latitude from degree/minute/second (DMS) format into decimal
+    valueTmp1 = ( double )NmeaGpsData.NmeaLatitude[0] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[1];
+    valueTmp2 = ( double )NmeaGpsData.NmeaLatitude[2] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[3];
+    valueTmp3 = ( double )NmeaGpsData.NmeaLatitude[5] * 1000.0 + ( double )NmeaGpsData.NmeaLatitude[6] * 100.0 + 
+                ( double )NmeaGpsData.NmeaLatitude[7] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[8];
+                
+    Latitude = valueTmp1 + ( ( valueTmp2 + ( valueTmp3 * 0.0001 ) ) / 60.0 ); 
+    
+    if( NmeaGpsData.NmeaLatitudePole[0] == 'S' )
+    {
+        Latitude *= -1;
+    }
+ 
+    // Convert the longitude from ASCII to uint8_t values
+    for( i = 0 ; i < 10 ; i++ )
+    {
+        NmeaGpsData.NmeaLongitude[i] = NmeaGpsData.NmeaLongitude[i] & 0xF;  
+    }
+    // Convert longitude from degree/minute/second (DMS) format into decimal
+    valueTmp1 = ( double )NmeaGpsData.NmeaLongitude[0] * 100.0 + ( double )NmeaGpsData.NmeaLongitude[1] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[2];
+    valueTmp2 = ( double )NmeaGpsData.NmeaLongitude[3] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[4];
+    valueTmp3 = ( double )NmeaGpsData.NmeaLongitude[6] * 1000.0 + ( double )NmeaGpsData.NmeaLongitude[7] * 100;
+    valueTmp4 = ( double )NmeaGpsData.NmeaLongitude[8] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[9];
+    
+    Longitude = valueTmp1 + ( valueTmp2 / 60.0 ) + ( ( ( valueTmp3 + valueTmp4 ) * 0.0001 ) / 60.0 );
+    
+    if( NmeaGpsData.NmeaLongitudePole[0] == 'W' )
+    {
+        Longitude *= -1;
+    }
+
+    //printf("DD %f, %f\r\n", Latitude, Longitude);
+}
+
+void GPS::ConvertPositionIntoBinary( )
+{
+    long double temp;
+
+    if( Latitude >= 0 ) // North
+    {    
+        temp = Latitude * MaxNorthPosition;
+        LatitudeBinary = temp / 90;
+    }
+    else                // South
+    {    
+        temp = Latitude * MaxSouthPosition;
+        LatitudeBinary = temp / 90;
+    }
+
+    if( Longitude >= 0 ) // East
+    {    
+        temp = Longitude * MaxEastPosition;
+        LongitudeBinary = temp / 180;
+    }
+    else                // West
+    {    
+        temp = Longitude * MaxWestPosition;
+        LongitudeBinary = temp / 180;
+    }
+}
+
+int GPS::ParseGPSData(int idx)
+{
+    uint8_t i, j, fieldSize;
+    char *NmeaString;
+
+    if (NmeaValidateChecksum(idx) == false) {
+        if (verbose)
+            printf("gps:bad nmea checksum:%s\r\n", rx_bufs[rx_bufs_out_idx]);
+        return FAIL;
+    }
+
+    NmeaString = rx_bufs[idx];
+    fieldSize = 0;
+    i = 0;
+    while( NmeaString[i + fieldSize++] != ',' )
+    {
+        if( fieldSize > 6 )
+        {
+            printf("gps:fieldSize:%d\r\n", fieldSize);
+            return FAIL;
+        }
+    }
+
+    for( j = 0; j < fieldSize; j++, i++ )
+    {
+        NmeaGpsData.NmeaDataType[j] = NmeaString[i];
+    }
+    // Parse the GPGGA data 
+    if( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPGGA, 5 ) == 0 )
+    {  
+        // NmeaUtcTime
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 11 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaUtcTime[j] = NmeaString[i];
+        }
+        // NmeaLatitude
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 10 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLatitude[j] = NmeaString[i];
+        }
+        // NmeaLatitudePole
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLatitudePole[j] = NmeaString[i];
+        }
+        // NmeaLongitude
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 11 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLongitude[j] = NmeaString[i];
+        }
+        // NmeaLongitudePole
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLongitudePole[j] = NmeaString[i];
+        }
+        // NmeaFixQuality
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaFixQuality[j] = NmeaString[i];
+        }
+        // NmeaSatelliteTracked
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 3 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaSatelliteTracked[j] = NmeaString[i];
+        }
+        // NmeaHorizontalDilution
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 6 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaHorizontalDilution[j] = NmeaString[i];
+        }
+        // NmeaAltitude
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 8 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaAltitude[j] = NmeaString[i];
+        }
+        // NmeaAltitudeUnit
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaAltitudeUnit[j] = NmeaString[i];
+        }
+        // NmeaHeightGeoid
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 8 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaHeightGeoid[j] = NmeaString[i];
+        }
+        // NmeaHeightGeoidUnit
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaHeightGeoidUnit[j] = NmeaString[i];
+        }
+
+        //FormatGpsData( );
+        ConvertPositionFromStringToNumerical( );
+        ConvertPositionIntoBinary( );
+        return SUCCESS;
+    }
+    else if ( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPRMC, 5 ) == 0 )
+    {    
+        // NmeaUtcTime
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 11 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaUtcTime[j] = NmeaString[i];
+        }
+        // NmeaDataStatus
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaDataStatus[j] = NmeaString[i];
+        }
+        // NmeaLatitude
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 10 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLatitude[j] = NmeaString[i];
+        }
+        // NmeaLatitudePole
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLatitudePole[j] = NmeaString[i];
+        }
+        // NmeaLongitude
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 11 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLongitude[j] = NmeaString[i];
+        }
+        // NmeaLongitudePole
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 2 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaLongitudePole[j] = NmeaString[i];
+        }
+        // NmeaSpeed
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 8 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaSpeed[j] = NmeaString[i];
+        }
+        // NmeaDetectionAngle
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 8 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaDetectionAngle[j] = NmeaString[i];
+        }
+        // NmeaDate
+        fieldSize = 0;
+        while( NmeaString[i + fieldSize++] != ',' )
+        {
+            if( fieldSize > 8 )
+            {
+                return FAIL;
+            }
+        }
+        for( j = 0; j < fieldSize; j++, i++ )
+        {
+            NmeaGpsData.NmeaDate[j] = NmeaString[i];
+        }
+
+        //FormatGpsData( );
+        ConvertPositionFromStringToNumerical( );
+        ConvertPositionIntoBinary( );
+        return SUCCESS;
+    }
+    else
+    {
+        return FAIL;
+    }
+}
+
+void GPS::service()
+{
+    while (rx_bufs_in_idx != rx_bufs_out_idx) {
+        //printf("%02d:%s\r\n", rx_buf_lens[rx_bufs_out_idx], rx_bufs[rx_bufs_out_idx]);
+        if (verbose)
+            printf("gps:%s\r\n", rx_bufs[rx_bufs_out_idx]);
+        ParseGPSData(rx_bufs_out_idx);
+        if (++rx_bufs_out_idx == NUM_RX_BUFS)
+            rx_bufs_out_idx = 0;
+    }
+
+    if (pps_occurred) {
+        pps_occurred = false;
+        if (verbose)
+            printf("gps:pps\r\n");
+    }
+}
+
+