/* * Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. * SPDX-License-Identifier: MIT-0 * * Permission is hereby granted, free of charge, to any person obtaining a copy of this * software and associated documentation files (the "Software"), to deal in the Software * without restriction, including without limitation the rights to use, copy, modify, * merge, publish, distribute, sublicense, and/or sell copies of the Software, and to * permit persons to whom the Software is furnished to do so. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /** * @file gps_library.c * @brief Functions to interact with OBD dongle gps. */ #include #include #include #include "FreeRTOS.h" #include "task.h" #include "FreeRTOS_IO.h" #include "obd_library.h" #include "gps_library.h" /*-----------------------------------------------------------*/ #define GET_UPTIME_MS() ( (uint32_t) ( xTaskGetTickCount() * portTICK_PERIOD_MS ) ) #define GPS_DEVICE_READY_TIME_MS ( 1000 ) #define GPS_COMMAND_TIMEOUT_MS ( 100 ) #define GPS_NMEA_COMMAND_TIMEOUT_MS ( 200 ) #define abs( x ) ( x > 0.0 ? ( x ) : ( x * -1.0 ) ) /*-----------------------------------------------------------*/ int GPSLib_GetNMEA( Peripheral_Descriptor_t obdDevice, char * buffer, int bufsize ) { return OBDLib_SendCommand( obdDevice, "ATGRR\r", buffer, bufsize, GPS_NMEA_COMMAND_TIMEOUT_MS ); } /*-----------------------------------------------------------*/ bool GPSLib_Begin( Peripheral_Descriptor_t obdDevice ) { char buf[ 64 ] = { 0 }; uint32_t startTime = GET_UPTIME_MS(); bool retGPSBegin = false; OBDLib_SendCommand( obdDevice, "ATGPSON\r", buf, sizeof( buf ), GPS_COMMAND_TIMEOUT_MS ); do { /* Wait until get NMEA is okay. */ if( ( GPSLib_GetNMEA( obdDevice, buf, sizeof( buf ) ) > 0 ) && ( strstr( buf, ( "$G" ) ) != NULL ) ) { retGPSBegin = true; break; } } while ( ( GET_UPTIME_MS() - startTime ) < GPS_DEVICE_READY_TIME_MS ); return retGPSBegin; } /*-----------------------------------------------------------*/ /* Define for strtoi overflow checking. */ #define CONST_INT32_QUOTIENT_10 ( 214748364 ) #define CONST_INT32_REMAINDER_10 ( 7 ) static int prvGpsStrtoi( const char * pStr, int32_t * pResult ) { bool retStrtoi = true; int32_t result = 0, i = 0, digit = 0, tmpResult = 0; int32_t signFlag = 0; if( pStr == NULL ) { retStrtoi = false; } else if( pStr[0] == '-' ) { signFlag = 1; i = 1; } else { /* Empty Else MISRA 15.7 */ } if( retStrtoi == true ) { for( ; pStr[i] != '\0'; i++ ) { tmpResult = result; if( ( pStr[i] >= '0' ) && ( pStr[i] <= '9' ) ) { digit = ( ( ( int32_t ) ( pStr[i] ) ) - ( ( int32_t ) '0' ) ); result = ( result * 10 ) + digit; } else { /* Non digits should break. */ retStrtoi = true; break; } /* Overflow detection. */ if( tmpResult > CONST_INT32_QUOTIENT_10 ) { retStrtoi = false; } else if( ( tmpResult == CONST_INT32_QUOTIENT_10 ) && ( digit > ( CONST_INT32_REMAINDER_10 + signFlag ) ) ) { retStrtoi = false; } else { /* Empty Else MISRA 15.7 */ } if( retStrtoi != true ) { break; } } } /* Handle the value sign part. */ if( retStrtoi == true ) { if( signFlag == 1 ) { *pResult = result * ( -1 ); } else { *pResult = result; } } return retStrtoi; } /*-----------------------------------------------------------*/ static int prvGpsAtoi( const char * pStr ) { int retValue = 0; if( prvGpsStrtoi( pStr, &retValue ) == false) { retValue = 0; } return retValue; } /*-----------------------------------------------------------*/ bool GPSLib_GetData( Peripheral_Descriptor_t obdDevice, ObdGpsData_t * gpsData ) { char buf[ 160 ]; if( OBDLib_SendCommand( obdDevice, "ATGPS\r", buf, sizeof( buf ), GPS_COMMAND_TIMEOUT_MS ) == 0 ) { return false; } char * s = strstr( buf, "$GNIFO," ); if( !s ) { return false; } s += 7; double lat = 0; double lng = 0; double alt = 0; bool good = false; do { uint32_t date = prvGpsAtoi( s ); if( !( s = strchr( s, ',' ) ) ) { break; } uint32_t time = prvGpsAtoi( ++s ); if( !( s = strchr( s, ',' ) ) ) { break; } if( !date ) { break; } gpsData->date = date; gpsData->time = time; lat = ( double ) prvGpsAtoi( ++s ) / 1000000; if( !( s = strchr( s, ',' ) ) ) { break; } lng = ( double ) prvGpsAtoi( ++s ) / 1000000; if( !( s = strchr( s, ',' ) ) ) { break; } alt = ( double ) prvGpsAtoi( ++s ) / 100; good = true; if( !( s = strchr( s, ',' ) ) ) { break; } gpsData->speed = ( double ) prvGpsAtoi( ++s ) / 100; if( !( s = strchr( s, ',' ) ) ) { break; } gpsData->heading = prvGpsAtoi( ++s ) / 100; if( !( s = strchr( s, ',' ) ) ) { break; } gpsData->sat = prvGpsAtoi( ++s ); if( !( s = strchr( s, ',' ) ) ) { break; } gpsData->hdop = prvGpsAtoi( ++s ); } while( 0 ); if( good && ( gpsData->lat || gpsData->lng || gpsData->alt ) ) { /* filter out invalid coordinates */ good = ( abs( lat * 1000000 - gpsData->lat * 1000000 ) < 100000 && abs( lng * 1000000 - gpsData->lng * 1000000 ) < 100000 ); } if( !good ) { return false; } gpsData->lat = lat; gpsData->lng = lng; gpsData->alt = alt; printf( "[GPSLib] %lf %lf SATS %d Course: %d\r\n", gpsData->lat, gpsData->lng, gpsData->sat, gpsData->heading ); return true; } /*-----------------------------------------------------------*/ bool GPSLib_End( Peripheral_Descriptor_t obdDevice ) { char buf[ 16 ] = { 0 }; bool retEnd = false; if( obdDevice == NULL ) { retEnd = false; } else { if( OBDLib_SendCommand( obdDevice, "ATGPSOFF", buf, sizeof( buf ), 0 ) > 0 ) { retEnd = true; } } return retEnd; } /*-----------------------------------------------------------*/