Diff: CsrLocation.cpp

Revision:

2:d4fe184925f2

Parent:

1:bbaf9b8d646a

Child:

3:71690f7bb480

--- a/CsrLocation.cpp	Tue Mar 25 05:26:26 2014 +0000
+++ b/CsrLocation.cpp	Wed Mar 26 09:16:55 2014 +0000
@@ -8,15 +8,15 @@
 #include "CsrLocation.h"
 
 
-static CsrUint8 sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3};
-static CsrUint8 sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3};
-static CsrUint8 sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E,
+static uint8_t sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3};
+static uint8_t sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3};
+static uint8_t sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E,
                                 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
                                 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x63, 0xB0, 0xB3};
-static CsrUint8 sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3};
-static CsrUint8 sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00, 
+static uint8_t sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3};
+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 CsrCharString sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n";
+static char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n";
 
 CsrLocation::CsrLocation(tCsrLocConfig *pLocConfig)
 {
@@ -74,8 +74,8 @@
         _CsrLocHwOnoff();
 
         csrLocInst.locState = CSR_LOC_STATE_RUN;
-    	csrLocInst.bPwrModeRsp = FALSE;
-    	csrLocInst.bVerRsp = FALSE;
+    	csrLocInst.bPwrModeRsp = false;
+    	csrLocInst.bVerRsp = false;
     	
     }
     else
@@ -118,7 +118,18 @@
     {
         CSR_LOG_INFO("LpmGetPos ");
         _CsrLocHwOnoff();
-    }    
+    }
+    else
+    {
+    	if(csrLocInst.locState != CSR_LOC_STATE_RUN)
+    	{
+        	CSR_LOG_INFO("Not in run state.\r\n");
+    	}
+    	else if(csrLocInst.pwrMode != PWR_PTF)
+    	{
+        	CSR_LOG_INFO("Not in low power PTF mode.\r\n");
+    	}
+	}    
 }
 
 void CsrLocation::CsrLocDebugSwitch2Nmea(void)
@@ -153,7 +164,7 @@
 void CsrLocation::_CsrLocProcessRawStream(void)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
-    CsrUint8    data;
+    uint8_t    data;
 
     if(pLocInst->in != pLocInst->out)
     {
@@ -162,14 +173,12 @@
         switch(pLocInst->protoState)
         {
         case PROTO_STATE_DET_OSP:
+        case PROTO_STATE_DET_OSP_FROM_NMEA:
             _CsrLocDetProtoOsp(data);
             break;
         case PROTO_STATE_DET_NMEA:
             _CsrLocDetProtoNmea(data);
             break;
-        case PROTO_STATE_DET_OSP_FROM_NMEA:
-            _CsrLocDetProtoOsp(data);
-            break;
         case PROTO_STATE_DET_OK:
             _CsrLocProcessRawOspStream(data);
             break;
@@ -181,7 +190,7 @@
     }
 }
 
-void CsrLocation::_CsrLocDetProtoOsp(CsrUint8 data)
+void CsrLocation::_CsrLocDetProtoOsp(uint8_t data)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
 
@@ -190,7 +199,7 @@
         /* Failed to detect OSP and try to detect NMEA */
         pLocInst->pSerialLoc->attach(NULL);
         pLocInst->pTimeoutChk->detach();
-        pLocInst->bTimeoutFlag = FALSE;
+        pLocInst->bTimeoutFlag = false;
         if(pLocInst->protoState == PROTO_STATE_DET_OSP)
         {
             pLocInst->protoState = PROTO_STATE_DET_NMEA;
@@ -210,7 +219,7 @@
     _CsrLocProcessRawOspStream(data);
 }
 
-void CsrLocation::_CsrLocDetProtoNmea(CsrUint8 data)
+void CsrLocation::_CsrLocDetProtoNmea(uint8_t data)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
 
@@ -219,7 +228,7 @@
         /* Failed to detect OSP and try to detect NMEA */
         pLocInst->pSerialLoc->attach(NULL);
         pLocInst->pTimeoutChk->detach();
-        pLocInst->bTimeoutFlag = FALSE;
+        pLocInst->bTimeoutFlag = false;
         pLocInst->protoState = PROTO_STATE_DET_INVALID;
         pLocInst->baudRate = BAUDRATE_OSP;
         CSR_LOG_INFO("Checking NMEA failed.\r\n");
@@ -255,7 +264,7 @@
         {
             pLocInst->pSerialLoc->attach(NULL);
             pLocInst->pTimeoutChk->detach();
-            pLocInst->bTimeoutFlag = FALSE;
+            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");
@@ -277,7 +286,7 @@
     }        
 }
 
-void CsrLocation::_CsrLocProcessRawOspStream(CsrUint8 data)
+void CsrLocation::_CsrLocProcessRawOspStream(uint8_t data)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
 
@@ -307,7 +316,7 @@
 
     case STATE_SIZE1:
         pLocInst->msgSize = data ;
-        pLocInst->msgSize <<= 8;  /* high CsrUint8 */
+        pLocInst->msgSize <<= 8;  /* high uint8_t */
         pLocInst->protoDetState = STATE_SIZE2;
         break;
 
@@ -408,7 +417,7 @@
         if (OSP_MSG_TAIL1 == data)
         {
             pLocInst->pTimeoutChk->detach();
-            pLocInst->bTimeoutFlag = FALSE;
+            pLocInst->bTimeoutFlag = false;
 
             if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA)
             {
@@ -417,7 +426,6 @@
                 pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0);
             }
             
-//            pLocInst->bRawOspPktReady = TRUE;
             _CsrLocProcessRawOspPkt();
             pLocInst->protoDetState = STATE_START1;
         }
@@ -441,15 +449,9 @@
 {
     tCsrLocInst *pLocInst = &csrLocInst;
     tOspMsg     *pOspMsg;
-    CsrUint32    msgSize;
+    uint32_t    msgSize;
     CsrResult    result;
     
-//    if(!pLocInst->bRawOspPktReady)
-//    {
-//    	return;	
-//    }
-    
-//    pLocInst->bRawOspPktReady = FALSE;
     msgSize = _CsrLocCalcMsgSize();
     if(msgSize > 0)
     {
@@ -480,13 +482,13 @@
 	free(pOspMsg);
 }
 
-CsrUint32 CsrLocation::_CsrLocCalcMsgSize(void)
+uint32_t CsrLocation::_CsrLocCalcMsgSize(void)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
-    CsrUint8 	*ptr = pLocInst->serialPkt;
-    CsrUint32 	 msgSize = 0;
-    CsrUint32 	 msgId;
-    CsrUint8 	 mid, sid = 0;
+    uint8_t 	*ptr = pLocInst->serialPkt;
+    uint32_t 	 msgSize = 0;
+    uint32_t 	 msgId;
+    uint8_t 	 mid, sid = 0;
 
     mid   = BINARY_IMPORT_UINT8(ptr);
     msgId = OSP_MAKE_MSG_ID(mid,sid);
@@ -504,7 +506,7 @@
     case OSP_MSG_PWR_MODE_FPM_RSP:
     case OSP_MSG_PWR_MODE_LPM_RSP:
     case OSP_MSG_HW_CONFIG_REQ :
-        msgSize = sizeof(CsrUint8);
+        msgSize = sizeof(uint8_t);
         break;
     case OSP_MSG_SW_VERSION :
         msgSize = MAX_VERSION_LENGTH;
@@ -513,10 +515,10 @@
         msgSize = sizeof(tLocPosResp);
         break;
     case OSP_MSG_GNSS_SAT_DATA:
-        msgSize = sizeof(CsrUint8);
+        msgSize = sizeof(uint8_t);
         break;
     case OSP_MSG_GNSS_NAV_DATA:
-        msgSize = sizeof(CsrUint8);
+        msgSize = sizeof(uint8_t);
         break;
 
     default :
@@ -527,12 +529,12 @@
     return msgSize;
 }
 
-CsrResult CsrLocation::_CsrLocDecodeOspPkt( CsrUint8 *pPayload, CsrUint32 payloadLen, CsrUint32 *pMsgId, void *pMsgData, CsrUint32 *pMsgLen)
+CsrResult CsrLocation::_CsrLocDecodeOspPkt( uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen)
 {
     CsrResult tRet = CSR_RESULT_SUCCESS;
-    CsrUint8 *ptr = pPayload;
-    CsrUint32 i;
-    CsrUint8 mid, sid = 0;
+    uint8_t *ptr = pPayload;
+    uint32_t i;
+    uint8_t mid, sid = 0;
 
     mid         = BINARY_IMPORT_UINT8(ptr);
     *pMsgId     = OSP_MAKE_MSG_ID(mid,sid);
@@ -561,14 +563,14 @@
         break;
 
     case OSP_MSG_OK_TO_SEND: /* 0x12 */
-        *((CsrUint8 *)pMsgData) = BINARY_IMPORT_UINT8(ptr);
-        *pMsgLen = sizeof(CsrUint8);
+        *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr);
+        *pMsgLen = sizeof(uint8_t);
         break;
 
     case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */
     {
         tLocPosResp *pPos = (tLocPosResp*) pMsgData;
-        CsrUint16   valid;
+        uint16_t   valid;
 
         valid = BINARY_IMPORT_UINT16(ptr);
         if(valid != 0)
@@ -583,12 +585,12 @@
             pPos->gps_week  = BINARY_IMPORT_UINT16(ptr);
             pPos->tow       = BINARY_IMPORT_UINT32(ptr);
             ptr += 12;
-            pPos->lat       = (CsrDouble)BINARY_IMPORT_SINT32(ptr);
+            pPos->lat       = (double)BINARY_IMPORT_SINT32(ptr);
             pPos->lat      *= 1e-7;
-            pPos->lon       = (CsrDouble)BINARY_IMPORT_SINT32(ptr);
+            pPos->lon       = (double)BINARY_IMPORT_SINT32(ptr);
             pPos->lon      *= 1e-7;
             ptr += 4;
-            pPos->alt       = (CsrDouble)BINARY_IMPORT_SINT32(ptr);
+            pPos->alt       = (double)BINARY_IMPORT_SINT32(ptr);
             pPos->alt      *= 1e-2;
         }
         break;
@@ -611,17 +613,17 @@
     case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */
     {
         tLocSvStatus *pSvStatus = &csrLocInst.svStatus;
-        CsrUint16 week;
-        CsrUint32 tow;
-        CsrUint32 towSubMs;
-        CsrUint8  info;
-        CsrInt32  nMsg = 0;
-        CsrUint16 satInfo;
-        CsrUint16 az;
-        CsrUint16 el;
-        CsrUint16 cno;
-        CsrUint8  gnssType;
-        CsrUint16 index = 0;
+        uint16_t week;
+        uint32_t tow;
+        uint32_t towSubMs;
+        uint8_t  info;
+        int32_t  nMsg = 0;
+        uint16_t satInfo;
+        uint16_t az;
+        uint16_t el;
+        uint16_t cno;
+        uint8_t  gnssType;
+        uint16_t index = 0;
 
         *pMsgLen = sizeof(*pSvStatus);
 
@@ -649,16 +651,16 @@
             cno     = BINARY_IMPORT_UINT16(ptr);
             ptr    += 4;
             
-            gnssType = (CsrUint8)((satInfo>>13)&0x0003);
+            gnssType = (uint8_t)((satInfo>>13)&0x0003);
             if(0 == gnssType || 1 == gnssType) // GPS, SBAS, QZSS
             {
                 index = pSvStatus->numOfSVs;
                 if(index < LOC_MAX_GNSS_SVS && cno >0)
                 {
-                    pSvStatus->svList[index].prn = (CsrUint8)(satInfo & 0xFF);
-                    pSvStatus->svList[index].cno = (CsrFloat)(cno/10.0); // Scale: 10
-                    pSvStatus->svList[index].elevation = (CsrFloat)(el/10.0); // Scale: 10
-                    pSvStatus->svList[index].azimuth = (CsrFloat)(az/10.0); // Scale: 10
+                    pSvStatus->svList[index].prn = (uint8_t)(satInfo & 0xFF);
+                    pSvStatus->svList[index].cno = (float)(cno/10.0); // Scale: 10
+                    pSvStatus->svList[index].elevation = (float)(el/10.0); // Scale: 10
+                    pSvStatus->svList[index].azimuth = (float)(az/10.0); // Scale: 10
                     pSvStatus->numOfSVs++;
                     pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn-1); // prn range: 1-32
                 }
@@ -668,19 +670,19 @@
                 index = pSvStatus->numOfGloSVs;
                 if(index < CODEC_GLO_MAX_CHANNELS && cno>0)
                 {
-                    CsrInt16 freqChan = (satInfo & 0X1F00)>>8;
-                    CsrInt16 slotNum  = (satInfo & 0X00FF);
+                    int16_t freqChan = (satInfo & 0X1F00)>>8;
+                    int16_t slotNum  = (satInfo & 0X00FF);
                     if(slotNum > 0)
                     {
                         if(freqChan & 0X0010)
                         {
                             freqChan |= 0xFFE0;
                         }
-                        pSvStatus->gloSvList[index].prn = (CsrUint8)(freqChan + LOC_GLO_FREQ_OFFSET);
-                        pSvStatus->gloSvList[index].sno = (CsrUint8)slotNum;
-                        pSvStatus->gloSvList[index].cno = (CsrFloat)(cno/10.0); // Scale: 10
-                        pSvStatus->gloSvList[index].elevation = (CsrFloat)(el/10.0); // Scale: 10
-                        pSvStatus->gloSvList[index].azimuth = (CsrFloat)(az/10.0); // Scale: 10
+                        pSvStatus->gloSvList[index].prn = (uint8_t)(freqChan + LOC_GLO_FREQ_OFFSET);
+                        pSvStatus->gloSvList[index].sno = (uint8_t)slotNum;
+                        pSvStatus->gloSvList[index].cno = (float)(cno/10.0); // Scale: 10
+                        pSvStatus->gloSvList[index].elevation = (float)(el/10.0); // Scale: 10
+                        pSvStatus->gloSvList[index].azimuth = (float)(az/10.0); // Scale: 10
                         pSvStatus->numOfGloSVs++;
                         pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn-LOC_GLO_FREQ_ID_START);
                     }
@@ -698,8 +700,8 @@
         break;
 
     case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */
-        *((CsrUint8 *)pMsgData) = *ptr;
-        *pMsgLen = sizeof(CsrUint8);
+        *((uint8_t *)pMsgData) = *ptr;
+        *pMsgLen = sizeof(uint8_t);
         break;
 
     default:
@@ -734,11 +736,12 @@
         CSR_LOG_INFO("Ok to send %u\r\n", csrLocInst.engStatus);
         break;
     case OSP_MSG_SW_VERSION:
+    	csrLocInst.bVerRsp = true;
         CSR_LOG_INFO("Ver: %s\r\n", pOspMsg->payload);
         break;
     case OSP_MSG_HW_CONFIG_REQ:
         CSR_LOG_INFO("hw config req.\r\n");
-    	if(!csrLocInst.bVerRsp);
+    	if(!csrLocInst.bVerRsp)
     	{
             _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
         }
@@ -756,11 +759,11 @@
         }
         break;
     case OSP_MSG_PWR_MODE_LPM_RSP:
-    	csrLocInst.bPwrModeRsp = TRUE;
+    	csrLocInst.bPwrModeRsp = true;
         CSR_LOG_INFO("lpm response.\r\n");
         break;
     case OSP_MSG_PWR_MODE_FPM_RSP:
-    	csrLocInst.bPwrModeRsp = TRUE;
+    	csrLocInst.bPwrModeRsp = true;
         CSR_LOG_INFO("fpm response.\r\n");
         break;
     default:
@@ -771,7 +774,7 @@
 
 void CsrLocation::_CsrLocTimeout(void)
 {
-    csrLocInst.bTimeoutFlag = TRUE;
+    csrLocInst.bTimeoutFlag = true;
 }
 
 void CsrLocation::_CsrLocRxHandler(void)
@@ -789,8 +792,8 @@
 void CsrLocation::_CsrLocSendData(eSendDataType type)
 {
     tCsrLocInst *pLocInst = &csrLocInst;
-    CsrUint32 i, size;
-    const CsrUint8 *pData;
+    uint32_t i, size;
+    const uint8_t *pData;
 
     switch(type)
     {
@@ -815,7 +818,7 @@
         size  = sizeof(sOspSwitch2NmeaReq);
         break;
     case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ:
-        pData = (const CsrUint8 *)sNmeaSwitch2OspReq;
+        pData = (const uint8_t *)sNmeaSwitch2OspReq;
         size  = strlen(sNmeaSwitch2OspReq);
         break;