Diff: CsrLocation.cpp

Revision:

3:71690f7bb480

Parent:

2:d4fe184925f2

Child:

4:0d9b711fb646

--- a/CsrLocation.cpp	Wed Mar 26 09:16:55 2014 +0000
+++ b/CsrLocation.cpp	Wed May 21 08:55:55 2014 +0000
@@ -17,6 +17,8 @@
 static uint8_t sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00, 
                                 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x12, 0xC0, 0x01, 0x64, 0xB0, 0xB3};
 static char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n";
+static char sNmeaStopReq[] = "$PSRF117,16*0B\r\n";
+
 
 CsrLocation::CsrLocation(tCsrLocConfig *pLocConfig)
 {
@@ -56,7 +58,7 @@
 	_CsrLocHwReset();
 }
 
-void CsrLocation::CsrLocStart(ePowerMode pwrMode)
+void CsrLocation::CsrLocStart(ePowerMode pwrMode, eProto proto)
 {
     csrLocInst.pwrMode = pwrMode;
 
@@ -64,10 +66,22 @@
     {
         /* Csr Location SDK version */
         CSR_LOG_INFO("==== CSR LOC SDK version: %s Date: %s ====\r\n", CSR_LOC_SDK_VER, __DATE__ "  " __TIME__);
+
+        csrLocInst.proto = proto;
+        
         /* open UART */
-        CSR_LOG_INFO("Checking OSP protocol...\r\n");
-        csrLocInst.protoState  = PROTO_STATE_DET_OSP;
-        csrLocInst.baudRate    = BAUDRATE_OSP;
+        if(proto == PROTO_NMEA)
+        {
+            CSR_LOG_INFO("Checking NMEA protocol...\r\n");
+            csrLocInst.protoState  = PROTO_STATE_DET_NMEA;
+            csrLocInst.baudRate    = BAUDRATE_NMEA;
+        }
+        else
+        {
+            CSR_LOG_INFO("Checking OSP protocol...\r\n");
+            csrLocInst.protoState  = PROTO_STATE_DET_OSP;
+            csrLocInst.baudRate    = BAUDRATE_OSP;
+        }
         _CsrLocUartInit();
 
         /* trigger on_off */
@@ -103,8 +117,15 @@
         /* in sleep mode, trigger on_off firstly */
         _CsrLocHwOnoff();
         wait_ms(500);
-    }    
-    _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ);
+    }
+    if(csrLocInst.proto == PROTO_NMEA)
+    {
+        _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ);
+    }
+    else
+    {
+        _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ);
+    }
     wait_ms(10);
     _CsrLocHwReset();
     csrLocInst.locState = CSR_LOC_STATE_IDLE;
@@ -166,6 +187,25 @@
     tCsrLocInst *pLocInst = &csrLocInst;
     uint8_t    data;
 
+    if(pLocInst->bTimeoutFlag)
+    {
+        pLocInst->pTimeoutChk->detach();
+        pLocInst->bTimeoutFlag = false;
+        if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA)
+        {
+            _CsrLocDetProtoOspTimeout();
+        }
+        else if(pLocInst->protoState == PROTO_STATE_DET_NMEA || pLocInst->protoState == PROTO_STATE_DET_NMEA_FROM_OSP)
+        {
+            _CsrLocDetProtoNmeaTimeout();
+        }
+        else
+        {
+            CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", pLocInst->protoState);
+        }
+        return;
+    }
+
     if(pLocInst->in != pLocInst->out)
     {
         data = pLocInst->serialBuf[pLocInst->out++];
@@ -174,13 +214,21 @@
         {
         case PROTO_STATE_DET_OSP:
         case PROTO_STATE_DET_OSP_FROM_NMEA:
-            _CsrLocDetProtoOsp(data);
+            _CsrLocProcessRawOspStream(data);
             break;
         case PROTO_STATE_DET_NMEA:
-            _CsrLocDetProtoNmea(data);
+        case PROTO_STATE_DET_NMEA_FROM_OSP:
+            _CsrLocProcessRawNmeaStream(data);
             break;
         case PROTO_STATE_DET_OK:
-            _CsrLocProcessRawOspStream(data);
+            if(pLocInst->proto == PROTO_NMEA)
+            {
+                _CsrLocProcessRawNmeaStream(data);
+            }
+            else
+            {
+                _CsrLocProcessRawOspStream(data);
+            }
             break;
         default:
             /* Discard received data */
@@ -190,16 +238,23 @@
     }
 }
 
-void CsrLocation::_CsrLocDetProtoOsp(uint8_t data)
+void CsrLocation::_CsrLocDetProtoOspTimeout(void)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
 
-    if(pLocInst->bTimeoutFlag)
+    if(pLocInst->proto == PROTO_NMEA)
+    {
+        /* Failed to detect OSP */
+        pLocInst->pSerialLoc->attach(NULL);
+        pLocInst->protoState = PROTO_STATE_DET_INVALID;
+        pLocInst->baudRate = BAUDRATE_NMEA;
+        CSR_LOG_INFO("Checking OSP failed.\r\n");
+        pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1);
+    }
+    else
     {
         /* Failed to detect OSP and try to detect NMEA */
         pLocInst->pSerialLoc->attach(NULL);
-        pLocInst->pTimeoutChk->detach();
-        pLocInst->bTimeoutFlag = false;
         if(pLocInst->protoState == PROTO_STATE_DET_OSP)
         {
             pLocInst->protoState = PROTO_STATE_DET_NMEA;
@@ -213,29 +268,48 @@
             CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n");
             pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1);
         }
-        return;
     }
-
-    _CsrLocProcessRawOspStream(data);
 }
 
-void CsrLocation::_CsrLocDetProtoNmea(uint8_t data)
+void CsrLocation::_CsrLocDetProtoNmeaTimeout(void)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
 
-    if(pLocInst->bTimeoutFlag)
+    CSR_LOG_INFO("Checking NMEA protocol failed\r\n");
+
+    if(pLocInst->proto == PROTO_NMEA)
     {
-        /* Failed to detect OSP and try to detect NMEA */
+        /* Failed to detect NMEA and try to detect OSP */
         pLocInst->pSerialLoc->attach(NULL);
-        pLocInst->pTimeoutChk->detach();
-        pLocInst->bTimeoutFlag = false;
+        if(pLocInst->protoState == PROTO_STATE_DET_NMEA)
+        {
+            pLocInst->protoState = PROTO_STATE_DET_OSP;
+            pLocInst->baudRate = BAUDRATE_OSP;
+            CSR_LOG_INFO("Checking NMEA protocol failed, now check OSP protocol...\r\n");
+            _CsrLocUartInit();
+        }
+        else
+        {
+            pLocInst->protoState = PROTO_STATE_DET_INVALID;
+            CSR_LOG_INFO("Checking switched NMEA protocol failed.\r\n");
+            pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1);
+        }
+    }
+    else
+    {
+        /* Failed to detect NEMA */
+        pLocInst->pSerialLoc->attach(NULL);
         pLocInst->protoState = PROTO_STATE_DET_INVALID;
         pLocInst->baudRate = BAUDRATE_OSP;
         CSR_LOG_INFO("Checking NMEA failed.\r\n");
         pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1);
-        return;
     }
+}
 
+void CsrLocation::_CsrLocProcessRawNmeaStream(uint8_t data)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+    
     switch (pLocInst->protoDetState)
     {
     case STATE_START1:
@@ -249,6 +323,8 @@
         if (NMEA_MSG_HEAD1 == data)
         {
             pLocInst->protoDetState = STATE_END1;
+            pLocInst->decodeIndex = 0;
+            pLocInst->serialPkt[pLocInst->decodeIndex++] = data;
         }
         else if (NMEA_MSG_HEAD0 == data)
         {
@@ -262,24 +338,49 @@
     case STATE_END1:
         if (NMEA_MSG_TAIL0 == data)
         {
-            pLocInst->pSerialLoc->attach(NULL);
             pLocInst->pTimeoutChk->detach();
             pLocInst->bTimeoutFlag = false;
-            pLocInst->protoState = PROTO_STATE_SWI_OSP_FROM_NMEA;
-            pLocInst->protoDetState = STATE_START1;
-            CSR_LOG_INFO("Checking OSP protocol OK, switching to OSP...\r\n");
-            _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ);
-            wait_ms(100);
+            if(pLocInst->proto == PROTO_NMEA)
+            {
+                pLocInst->protoDetState = STATE_START1;
+                if(pLocInst->protoState == PROTO_STATE_DET_NMEA || pLocInst->protoState == PROTO_STATE_DET_NMEA_FROM_OSP)
+                {
+                    CSR_LOG_INFO("Checking NMEA protocol OK.\r\n");
+                    pLocInst->protoState = PROTO_STATE_DET_OK;
+                    pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0);
+                }
 
-            pLocInst->protoState = PROTO_STATE_DET_OSP_FROM_NMEA;
-            pLocInst->baudRate = BAUDRATE_OSP;
-            CSR_LOG_INFO("Checking switched OSP protocol...\r\n");
-            _CsrLocUartInit();
+                pLocInst->serialPkt[pLocInst->decodeIndex++] = '\0';
+                _CsrLocProcessRawNmeaPkt();
+            }
+            else
+            {
+                pLocInst->pSerialLoc->attach(NULL);
+                
+                CSR_LOG_INFO("Checking NMEA protocol OK, switching to OSP...\r\n");
+                _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ);
+                wait_ms(100);
+                pLocInst->protoState = PROTO_STATE_DET_OSP_FROM_NMEA;
+                pLocInst->baudRate = BAUDRATE_OSP;
+                CSR_LOG_INFO("Checking switched OSP protocol...\r\n");
+                _CsrLocUartInit();
+            }
         }
         else if (NMEA_MSG_HEAD0 == data)
         {               
             pLocInst->protoDetState = STATE_START2;
         }
+        else
+        {
+            if(pLocInst->decodeIndex < (MAX_SERIAL_PKT_LEN - 2))
+            {
+                pLocInst->serialPkt[pLocInst->decodeIndex++] = data;
+            }
+            else
+            {
+                pLocInst->protoDetState = STATE_START1;
+            }
+        }
         break;
     default:
         break;
@@ -418,16 +519,32 @@
         {
             pLocInst->pTimeoutChk->detach();
             pLocInst->bTimeoutFlag = false;
+            pLocInst->protoDetState = STATE_START1;
 
-            if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA)
+            if(pLocInst->proto == PROTO_NMEA)
             {
-                CSR_LOG_INFO("Checking OSP protocol OK.\r\n");
-                pLocInst->protoState = PROTO_STATE_DET_OK;
-                pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0);
+                pLocInst->pSerialLoc->attach(NULL);
+                
+                CSR_LOG_INFO("Checking OSP protocol OK, switching to NMEA...\r\n");
+                _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ);
+                wait_ms(100);
+                pLocInst->protoState = PROTO_STATE_DET_NMEA_FROM_OSP;
+                pLocInst->baudRate = BAUDRATE_NMEA;
+                CSR_LOG_INFO("Checking switched NMEA protocol...\r\n");
+                _CsrLocUartInit();
+
             }
-            
-            _CsrLocProcessRawOspPkt();
-            pLocInst->protoDetState = STATE_START1;
+            else
+            {
+                if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA)
+                {
+                    CSR_LOG_INFO("Checking OSP protocol OK.\r\n");
+                    pLocInst->protoState = PROTO_STATE_DET_OK;
+                    pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0);
+                }
+                
+                _CsrLocProcessRawOspPkt();
+            }
         }
         else
         {
@@ -445,6 +562,61 @@
     
 }
 
+void CsrLocation::_CsrLocProcessRawNmeaPkt(void)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+    tLocPosResp pos;
+    const char *pNmeaGga = "GPGGA";
+    float       deg, min;
+	char        ns, ew;
+	int         svUsed;
+	float       horDop;
+    int         valid;
+    int         i, cnt;
+
+    if(strncmp((char *)pLocInst->serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0)
+    {
+        cnt = 0;
+        for(i = 0; i < (int)strlen((char *)pLocInst->serialPkt); i++)
+        {
+            if(pLocInst->serialPkt[i] == ',')
+            {
+                cnt++;
+                if(cnt == 6)
+                {
+                    break;
+                }
+            }
+        }
+        if(cnt != 6)
+        {
+            return;
+        }
+        i++;
+        sscanf((char *)(&pLocInst->serialPkt[i]), "%d,", &valid);
+        if(valid == 0)
+        {
+            return;
+        }
+        
+        /* Parse GPGGA and output position information */
+        memset(&pos, 0, sizeof(tLocPosResp));
+        if(sscanf((char *)pLocInst->serialPkt, "GPGGA,%f,%lf,%c,%lf,%c,%d,%d,%f,%lf", &pos.u.utcTime, &pos.lat, &ns, &pos.lon, &ew, &valid, &svUsed, &horDop, &pos.alt) >= 1)
+        {
+			if(ns == 'S') {	pos.lat  *= -1.0; }
+			if(ew == 'W') {	pos.lon *= -1.0; }
+			deg = (float)(static_cast<int>(pos.lat * 0.01f));
+			min = pos.lat - (deg * 100.0f);
+			pos.lat = deg + min / 60.0f;	
+			deg = (float)(static_cast<int>(pos.lon * 0.01f));
+			min = pos.lon - (deg * 100.0f);
+			pos.lon = deg + min / 60.0f;
+
+            csrLocInst.appOutCb(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp));
+        }
+    }
+}
+
 void CsrLocation::_CsrLocProcessRawOspPkt(void)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
@@ -582,8 +754,8 @@
             *pMsgLen = sizeof(*pPos);
 
             ptr += 2;
-            pPos->gps_week  = BINARY_IMPORT_UINT16(ptr);
-            pPos->tow       = BINARY_IMPORT_UINT32(ptr);
+            pPos->u.gpsTime.gps_week = BINARY_IMPORT_UINT16(ptr);
+            pPos->u.gpsTime.tow      = BINARY_IMPORT_UINT32(ptr);
             ptr += 12;
             pPos->lat       = (double)BINARY_IMPORT_SINT32(ptr);
             pPos->lat      *= 1e-7;
@@ -785,7 +957,7 @@
     pLocInst->in &= (MAX_SERIAL_BUF_LEN-1);
     if(pLocInst->in == pLocInst->out)
     {
-        CSR_LOG_INFO("rx overwritten %d %d.\r\n", pLocInst->in, pLocInst->out);
+        CSR_LOG_INFO("rx overwritten %lu %lu.\r\n", pLocInst->in, pLocInst->out);
     }
 }
 
@@ -821,7 +993,11 @@
         pData = (const uint8_t *)sNmeaSwitch2OspReq;
         size  = strlen(sNmeaSwitch2OspReq);
         break;
-        
+    case SEND_DATA_TYPE_NMEA_STOP_REQ:
+        pData = (const uint8_t *)sNmeaStopReq;
+        size  = strlen(sNmeaStopReq);
+        break;
+    
     default:
         pData = NULL;
     }