Diff: CsrLocation.cpp

Revision:

0:aba381fc8158

Child:

1:bbaf9b8d646a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CsrLocation.cpp	Mon Mar 24 08:23:25 2014 +0000
@@ -0,0 +1,838 @@
+
+/* CsrLocation class for mbed Microcontroller
+ * Copyright 2014 CSR plc
+ */
+
+
+#include "mbed.h"
+#include "CsrLocation.h"
+
+
+const static CsrUint8 sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3};
+const static CsrUint8 sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3};
+const static CsrUint8 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};
+const static CsrUint8 sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3};
+const static CsrUint8 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};
+const static CsrCharString sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n";
+
+CsrLocation::CsrLocation(tCsrLocConfig *pLocConfig)
+{
+    memset(&csrLocInst, 0, sizeof(tCsrLocInst));
+    csrLocInst.pSerialDebug = pLocConfig->pSerialDebug;
+    csrLocInst.pSerialLoc   = pLocConfig->pSerialLoc;
+    csrLocInst.pPinOnoff    = pLocConfig->pPinOnoff;
+    csrLocInst.pPinReset    = pLocConfig->pPinReset;
+    csrLocInst.pTimeoutChk  = new Timeout();
+
+    csrLocInst.pPinReset->write(1);
+    csrLocInst.pPinOnoff->write(0);
+}
+
+CsrLocation::~CsrLocation(void)
+{
+    if(csrLocInst.pTimeoutChk != NULL)
+    {
+        delete csrLocInst.pTimeoutChk;
+        csrLocInst.pTimeoutChk = NULL;
+    }
+    csrLocInst.pSerialLoc->attach(NULL);
+    csrLocInst.pPinReset->write(0);
+    csrLocInst.pPinOnoff->write(0);
+    memset(&csrLocInst, 0, sizeof(tCsrLocInst));
+
+}
+
+void CsrLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb)
+{
+    csrLocInst.appOutCb = app_output_cb;
+    csrLocInst.appEventCb = app_event_cb;
+}
+
+void CsrLocation::CsrLocReset(void)
+{
+	_CsrLocHwReset();
+}
+
+void CsrLocation::CsrLocStart(ePowerMode pwrMode)
+{
+    csrLocInst.pwrMode = pwrMode;
+
+    if(csrLocInst.locState == CSR_LOC_STATE_IDLE)
+    {
+        /* Csr Location SDK version */
+        CSR_LOG_INFO("==== CSR LOC SDK version: %s Date: %s ====\r\n", CSR_LOC_SDK_VER, __DATE__ "  " __TIME__);
+        /* open UART */
+        CSR_LOG_INFO("Checking OSP protocol...\r\n");
+        csrLocInst.protoState  = PROTO_STATE_DET_OSP;
+        csrLocInst.baudRate    = BAUDRATE_OSP;
+        _CsrLocUartInit();
+
+        /* trigger on_off */
+        _CsrLocHwOnoff();
+
+        csrLocInst.locState = CSR_LOC_STATE_RUN;
+    }
+    else
+    {
+        CSR_LOG_INFO("Already started.\r\n");
+    }
+}
+
+void CsrLocation::CsrLocUpdate(void)
+{
+    if(csrLocInst.locState == CSR_LOC_STATE_RUN)
+    {
+        /* wait and process uart data */
+        _CsrLocProcessRawStream();
+    }
+}
+
+void CsrLocation::CsrLocStop(void)
+{
+    csrLocInst.pSerialLoc->attach(NULL);
+    CSR_LOG_INFO("Stop command processed.\r\n");
+
+    if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND)
+    {
+        /* in sleep mode, trigger on_off firstly */
+        _CsrLocHwOnoff();
+        wait_ms(500);
+    }    
+    _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ);
+    wait_ms(10);
+    _CsrLocHwReset();
+    csrLocInst.locState = CSR_LOC_STATE_IDLE;
+    csrLocInst.appEventCb(CSR_LOC_EVENT_STOP_RESULT, 0);
+
+}
+
+void CsrLocation::CsrLocLpmGetPos(void)
+{
+    if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND)
+    {
+        CSR_LOG_INFO("LpmGetPos ");
+        _CsrLocHwOnoff();
+    }    
+}
+
+void CsrLocation::CsrLocDebugSwitch2Nmea(void)
+{
+    if(csrLocInst.locState == CSR_LOC_STATE_RUN)
+    {
+        _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ);
+        wait_ms(200);
+        _CsrLocHwReset();
+    }    
+}
+
+void CsrLocation::_CsrLocUartInit(void)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+
+    pLocInst->in            = 0;
+    pLocInst->out           = 0;
+    memset(pLocInst->serialBuf, 0 , MAX_SERIAL_BUF_LEN);
+    memset(pLocInst->serialPkt, 0 , MAX_SERIAL_PKT_LEN);
+
+    pLocInst->isOspHeader   = FALSE;
+    pLocInst->isNmeaHeader  = FALSE;
+    
+    pLocInst->checksum      = 0;
+    pLocInst->msgSize       = 0;
+    pLocInst->decodeIndex   = 0;
+    pLocInst->protoDetState = STATE_START1;
+    pLocInst->pTimeoutChk->attach(this, &CsrLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT);
+
+    pLocInst->pSerialLoc->baud(pLocInst->baudRate);
+    pLocInst->pSerialLoc->attach(this, &CsrLocation::_CsrLocRxHandler );
+}
+
+void CsrLocation::_CsrLocProcessRawStream(void)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+    CsrUint8    data;
+
+    if(pLocInst->in != pLocInst->out)
+    {
+        data = pLocInst->serialBuf[pLocInst->out++];
+        pLocInst->out &= (MAX_SERIAL_BUF_LEN-1);
+        switch(pLocInst->protoState)
+        {
+        case PROTO_STATE_DET_OSP:
+            _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;
+        default:
+            /* Discard received data */
+            pLocInst->out = pLocInst->in;
+            break;
+        }
+    }
+    else
+    {
+        wait_ms(1);
+    }
+}
+
+void CsrLocation::_CsrLocDetProtoOsp(CsrUint8 data)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+
+    if(pLocInst->bTimeoutFlag)
+    {
+        /* 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;
+            pLocInst->baudRate = BAUDRATE_NMEA;
+            CSR_LOG_INFO("Checking OSP protocol failed, now check NMEA protocol...\r\n");
+            _CsrLocUartInit();
+        }
+        else
+        {
+            pLocInst->protoState = PROTO_STATE_DET_INVALID;
+            CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n");
+            pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1);
+        }
+        return;
+    }
+
+    _CsrLocProcessRawOspStream(data);
+}
+
+void CsrLocation::_CsrLocDetProtoNmea(CsrUint8 data)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+
+    if(pLocInst->bTimeoutFlag)
+    {
+        /* Failed to detect OSP and try to detect NMEA */
+        pLocInst->pSerialLoc->attach(NULL);
+        pLocInst->pTimeoutChk->detach();
+        pLocInst->bTimeoutFlag = FALSE;
+        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;
+    }
+
+    switch (pLocInst->protoDetState)
+    {
+    case STATE_START1:
+        if (NMEA_MSG_HEAD0 == data)
+        {
+            pLocInst->protoDetState = STATE_START2;
+        }
+        break;
+
+    case STATE_START2:
+        if (NMEA_MSG_HEAD1 == data)
+        {
+            pLocInst->protoDetState = STATE_END1;
+        }
+        else if (NMEA_MSG_HEAD0 == data)
+        {
+            pLocInst->protoDetState = STATE_START2;
+        }
+        else
+        {
+            pLocInst->protoDetState = STATE_START1;
+        }
+        break;
+    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);
+
+            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;
+        }
+        break;
+    default:
+        break;
+    }        
+}
+
+void CsrLocation::_CsrLocProcessRawOspStream(CsrUint8 data)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+
+    switch (pLocInst->protoDetState)
+    {
+    case STATE_START1:
+        if (OSP_MSG_HEAD0 == data)
+        {
+            pLocInst->protoDetState = STATE_START2;
+        }
+        break;
+
+    case STATE_START2:
+        if (OSP_MSG_HEAD1 == data)
+        {
+            pLocInst->protoDetState = STATE_SIZE1;
+        }
+        else if (OSP_MSG_HEAD0 == data)
+        {
+            pLocInst->protoDetState = STATE_START2;
+        }
+        else
+        {
+            pLocInst->protoDetState = STATE_START1;
+        }
+        break;
+
+    case STATE_SIZE1:
+        pLocInst->msgSize = data ;
+        pLocInst->msgSize <<= 8;  /* high CsrUint8 */
+        pLocInst->protoDetState = STATE_SIZE2;
+        break;
+
+    case STATE_SIZE2:
+        pLocInst->msgSize += data ;
+        if (MAX_SERIAL_PKT_LEN < pLocInst->msgSize || 0 == pLocInst->msgSize)
+        {
+            if (OSP_MSG_HEAD0 == data)
+            {
+                pLocInst->protoDetState = STATE_START2;
+            }
+            else
+            {
+                pLocInst->protoDetState = STATE_START1;
+            }
+        }
+        else
+        {
+            pLocInst->computedCheckSum = 0;
+            pLocInst->decodeIndex = 0;
+            pLocInst->protoDetState = STATE_PAYLOAD;
+        }
+        break;
+
+    case STATE_PAYLOAD:
+        /* check for a catastrophic error case */
+        if (MAX_SERIAL_PKT_LEN <= pLocInst->decodeIndex)
+        {
+            /* This is really bad.  And should never happen since we 
+             * gurantee that msgSize is always less than RECEIVE_PACKET_SIZE
+             * Change the state back to STATE_START1 and start over */
+            if (OSP_MSG_HEAD0 == data)
+            {
+                pLocInst->protoDetState = STATE_START2;
+            }
+            else
+            {
+                pLocInst->protoDetState = STATE_START1;
+            }
+            break;
+        }
+        /* Store the byte */
+        pLocInst->serialPkt[pLocInst->decodeIndex++] = data;
+        pLocInst->computedCheckSum += data;
+
+        /* Check to see if we've read the full payload */
+        if (0 == (--pLocInst->msgSize))
+        {
+            pLocInst->computedCheckSum &= 0x7FFF;
+            pLocInst->protoDetState = STATE_CHECKSUM1;
+        }
+        break;
+
+    case STATE_CHECKSUM1:
+        pLocInst->checksum = data ;
+        pLocInst->checksum <<= 8;
+        pLocInst->protoDetState = STATE_CHECKSUM2;
+        break;
+
+    case STATE_CHECKSUM2:
+        pLocInst->checksum += data;
+        if (pLocInst->computedCheckSum != pLocInst->checksum)
+        {
+            if (OSP_MSG_HEAD0 == data)
+            {
+                pLocInst->protoDetState = STATE_START2;
+            }
+            else
+            {
+                pLocInst->protoDetState = STATE_START1;
+            }
+        }
+        else
+        {
+            pLocInst->protoDetState = STATE_END1;
+        }
+        break;
+
+    case STATE_END1:
+        if (OSP_MSG_TAIL0 == data)
+        {
+            pLocInst->protoDetState = STATE_END2;
+        }
+        else
+        {               
+            if (OSP_MSG_HEAD0 == data)
+            {
+                pLocInst->protoDetState = STATE_START2;
+            }
+            else
+            {
+                pLocInst->protoDetState = STATE_START1;
+            }
+        }
+        break;
+
+    case STATE_END2:
+        if (OSP_MSG_TAIL1 == data)
+        {
+            pLocInst->pTimeoutChk->detach();
+            pLocInst->bTimeoutFlag = FALSE;
+
+            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();
+            pLocInst->protoDetState = STATE_START1;
+        }
+        else
+        {
+            if (OSP_MSG_HEAD0 == data)
+            {
+                pLocInst->protoDetState = STATE_START2;
+            }
+            else
+            {
+                pLocInst->protoDetState = STATE_START1;
+            }
+        }            
+        break;
+    }   /* switch. */
+    
+}
+
+void CsrLocation::_CsrLocProcessRawOspPkt(void)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+    tOspMsg     *pOspMsg;
+    CsrUint32    msgSize;
+    CsrResult    result;
+    
+    msgSize = _CsrLocCalcMsgSize();
+    if(msgSize > 0)
+    {
+        msgSize += sizeof(tOspMsg);
+        pOspMsg = (tOspMsg *)malloc(msgSize);
+        if(pOspMsg != NULL)
+        {
+            memset(pOspMsg, 0, msgSize);
+        }
+        else
+        {
+            CSR_LOG_INFO("No memory for received OSP message.\r\n");
+            return;
+        }
+    }
+    else
+    {
+        /* discard the unprocessed message */
+        return;
+    }
+
+    result = _CsrLocDecodeOspPkt(pLocInst->serialPkt,pLocInst->decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length);
+    if ( CSR_RESULT_SUCCESS == result)
+    {
+         _CsrLocProcessOspPkt(pOspMsg);
+    }
+
+	free(pOspMsg);
+}
+
+CsrUint32 CsrLocation::_CsrLocCalcMsgSize(void)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+    CsrUint8 	*ptr = pLocInst->serialPkt;
+    CsrUint32 	 msgSize = 0;
+    CsrUint32 	 msgId;
+    CsrUint8 	 mid, sid = 0;
+
+    mid   = BINARY_IMPORT_UINT8(ptr);
+    msgId = OSP_MAKE_MSG_ID(mid,sid);
+
+    if(OSP_MSG_PWR_MODE_RSP == msgId || OSP_MSG_MULTI_CONSTELLATION == msgId)
+    {
+		/* add the sub-id to the message id */
+        sid   = BINARY_IMPORT_UINT8(ptr);
+		msgId = OSP_MAKE_MSG_ID(mid,sid);
+    }
+
+    switch(msgId)
+    {
+    case OSP_MSG_OK_TO_SEND:
+    case OSP_MSG_PWR_MODE_FPM_RSP:
+    case OSP_MSG_PWR_MODE_LPM_RSP:
+    case OSP_MSG_HW_CONFIG_REQ :
+        msgSize = sizeof(CsrUint8);
+        break;
+    case OSP_MSG_SW_VERSION :
+        msgSize = MAX_VERSION_LENGTH;
+        break;
+    case OSP_MSG_GEODETIC_NAVIGATION:
+        msgSize = sizeof(tLocPosResp);
+        break;
+    case OSP_MSG_GNSS_SAT_DATA:
+        msgSize = sizeof(CsrUint8);
+        break;
+    case OSP_MSG_GNSS_NAV_DATA:
+        msgSize = sizeof(CsrUint8);
+        break;
+
+    default :
+        msgSize = 0;
+        break;
+    }   
+
+    return msgSize;
+}
+
+CsrResult CsrLocation::_CsrLocDecodeOspPkt( CsrUint8 *pPayload, CsrUint32 payloadLen, CsrUint32 *pMsgId, void *pMsgData, CsrUint32 *pMsgLen)
+{
+    CsrResult tRet = CSR_RESULT_SUCCESS;
+    CsrUint8 *ptr = pPayload;
+    CsrUint32 i;
+    CsrUint8 mid, sid = 0;
+
+    mid         = BINARY_IMPORT_UINT8(ptr);
+    *pMsgId     = OSP_MAKE_MSG_ID(mid,sid);
+    *pMsgLen    = 0;
+
+    /*   add the sub-id to the message id */
+    if (OSP_MSG_PWR_MODE_RSP == *pMsgId || OSP_MSG_MULTI_CONSTELLATION == *pMsgId)
+    {
+        sid     = BINARY_IMPORT_UINT8(ptr);
+		*pMsgId = OSP_MAKE_MSG_ID(mid,sid);
+    }
+
+    switch (*pMsgId)
+    {
+    case OSP_MSG_SW_VERSION: /* 0x06 */
+        *pMsgLen = BINARY_IMPORT_UINT8(ptr);
+        ptr++;
+		if(*pMsgLen >= MAX_VERSION_LENGTH)
+	    {
+            tRet = CSR_RESULT_FAILURE;
+	    }
+        else
+        {
+            memcpy(pMsgData, ptr, *pMsgLen);
+        }
+        break;
+
+    case OSP_MSG_OK_TO_SEND: /* 0x12 */
+        *((CsrUint8 *)pMsgData) = BINARY_IMPORT_UINT8(ptr);
+        *pMsgLen = sizeof(CsrUint8);
+        break;
+
+    case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */
+    {
+        tLocPosResp *pPos = (tLocPosResp*) pMsgData;
+        CsrUint16   valid;
+
+        valid = BINARY_IMPORT_UINT16(ptr);
+        if(valid != 0)
+        {
+            tRet = CSR_RESULT_FAILURE;
+        }
+        else
+        {
+            *pMsgLen = sizeof(*pPos);
+
+            ptr += 2;
+            pPos->gps_week  = BINARY_IMPORT_UINT16(ptr);
+            pPos->tow       = BINARY_IMPORT_UINT32(ptr);
+            ptr += 12;
+            pPos->lat       = (CsrDouble)BINARY_IMPORT_SINT32(ptr);
+            pPos->lat      *= 1e-7;
+            pPos->lon       = (CsrDouble)BINARY_IMPORT_SINT32(ptr);
+            pPos->lon      *= 1e-7;
+            ptr += 4;
+            pPos->alt       = (CsrDouble)BINARY_IMPORT_SINT32(ptr);
+            pPos->alt      *= 1e-2;
+        }
+        break;
+    }
+    
+    case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */
+    {
+        tLocSvStatus *pSvStatus = &csrLocInst.svStatus;
+
+        *pMsgLen = sizeof(*pSvStatus);
+        
+        ptr += 100;
+        pSvStatus->svUsedInFixMask      = BINARY_IMPORT_UINT32(ptr);
+        pSvStatus->sbasSvUsedInFixMask  = BINARY_IMPORT_UINT32(ptr);
+        pSvStatus->gloSvUsedInFixMask   = BINARY_IMPORT_UINT32(ptr);
+        pSvStatus->qzssSvUsedInFixMask  = BINARY_IMPORT_UINT32(ptr);
+        break;         
+    }
+
+    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;
+
+        *pMsgLen = sizeof(*pSvStatus);
+
+        week        = BINARY_IMPORT_UINT16(ptr);
+        tow         = BINARY_IMPORT_UINT32(ptr);
+        towSubMs    = BINARY_IMPORT_UINT32(ptr);
+        ptr        += 4;
+        info        = BINARY_IMPORT_UINT8(ptr);
+        
+        nMsg        = info & 0x0F;
+        if(nMsg == 1)
+        {
+            memset(pSvStatus, 0, sizeof(tLocSvStatus));
+            pSvStatus->gps_week     = week;
+            pSvStatus->tow          = tow;
+            pSvStatus->tow_sub_ms   = towSubMs;
+        }
+        
+        ptr++;
+        for (i = 0; i < GNSS_SAT_DATA_NUM_OF_SATS; i++)
+        {
+            satInfo = BINARY_IMPORT_UINT16(ptr);
+            az      = BINARY_IMPORT_UINT16(ptr);
+            el      = BINARY_IMPORT_UINT16(ptr);
+            cno     = BINARY_IMPORT_UINT16(ptr);
+            ptr    += 4;
+            
+            gnssType = (CsrUint8)((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->numOfSVs++;
+                    pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn-1); // prn range: 1-32
+                }
+            }
+            else if(2 == gnssType) // GLONASS
+            {
+                index = pSvStatus->numOfGloSVs;
+                if(index < CODEC_GLO_MAX_CHANNELS && cno>0)
+                {
+                    CsrInt16 freqChan = (satInfo & 0X1F00)>>8;
+                    CsrInt16 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->numOfGloSVs++;
+                        pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn-LOC_GLO_FREQ_ID_START);
+                    }
+                }
+            }
+        }
+
+        break;
+    }
+
+    case  OSP_MSG_HW_CONFIG_REQ: /* 0x47 */
+        break;
+
+    case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */
+        break;
+
+    case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */
+        *((CsrUint8 *)pMsgData) = *ptr;
+        *pMsgLen = sizeof(CsrUint8);
+        break;
+
+    default:
+        tRet = CSR_RESULT_FAILURE;
+        break;
+    }
+
+    /* check if length does not match */
+    if(tRet == CSR_RESULT_FAILURE)
+    {
+        *pMsgId = *pMsgLen = 0;
+    }
+
+   return tRet;
+
+} /* CsrUlocCodecSsbDecode() */
+
+void CsrLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg)
+{
+    switch(pOspMsg->msgId)
+    {
+    case OSP_MSG_GEODETIC_NAVIGATION:
+        csrLocInst.appOutCb(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp));
+        break;
+    case OSP_MSG_GNSS_SAT_DATA:
+        break;
+    case OSP_MSG_GNSS_NAV_DATA:
+        csrLocInst.appOutCb(LOC_OUTPUT_SV_STATUS, &csrLocInst.svStatus, sizeof(tLocSvStatus));
+        break;
+    case OSP_MSG_OK_TO_SEND:
+        csrLocInst.engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND;
+        CSR_LOG_INFO("Ok to send %u\r\n", csrLocInst.engStatus);
+        break;
+    case OSP_MSG_SW_VERSION:
+        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.pwrMode == PWR_PTF)
+        {
+            _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
+            _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
+        }
+        else
+        {
+            _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
+            _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
+        }
+        break;
+    case OSP_MSG_PWR_MODE_LPM_RSP:
+        CSR_LOG_INFO("lpm response.\r\n");
+        break;
+    case OSP_MSG_PWR_MODE_FPM_RSP:
+        CSR_LOG_INFO("fpm response.\r\n");
+        break;
+    default:
+        CSR_LOG_INFO("Unknown OSP message 0x%lx.\r\n", pOspMsg->msgId);
+        break;
+    }
+}
+
+void CsrLocation::_CsrLocTimeout(void)
+{
+    csrLocInst.bTimeoutFlag = TRUE;
+}
+
+void CsrLocation::_CsrLocRxHandler(void)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+
+    pLocInst->serialBuf[pLocInst->in++] = pLocInst->pSerialLoc->getc();
+    pLocInst->in &= (MAX_SERIAL_BUF_LEN-1);
+    if(pLocInst->in == pLocInst->out)
+    {
+        CSR_LOG_INFO("rx overwritten.\r\n");
+    }
+}
+
+void CsrLocation::_CsrLocSendData(eSendDataType type)
+{
+    tCsrLocInst *pLocInst = &csrLocInst;
+    CsrUint32 i, size;
+    const CsrUint8 *pData;
+
+    switch(type)
+    {
+    case SEND_DATA_TYPE_OSP_STOP_REQ:
+        pData = sOspStopReq;
+        size  = sizeof(sOspStopReq);
+        break;
+    case SEND_DATA_TYPE_OSP_VER_REQ:
+        pData = sOspVerReq;
+        size  = sizeof(sOspVerReq);
+        break;
+    case SEND_DATA_TYPE_OSP_LPM_REQ:
+        pData = sOspLpmReq;
+        size  = sizeof(sOspLpmReq);
+        break;
+    case SEND_DATA_TYPE_OSP_FPM_REQ:
+        pData = sOspFpmReq;
+        size  = sizeof(sOspFpmReq);
+        break;
+    case SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ:
+        pData = sOspSwitch2NmeaReq;
+        size  = sizeof(sOspSwitch2NmeaReq);
+        break;
+    case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ:
+        pData = (const CsrUint8 *)sNmeaSwitch2OspReq;
+        size  = strlen(sNmeaSwitch2OspReq);
+        break;
+        
+    default:
+        pData = NULL;
+    }
+
+    if(pData != NULL)
+    {
+        for (i = 0; i < size; i++)
+        {
+            while(pLocInst->pSerialLoc->writeable() == 0) {}
+            pLocInst->pSerialLoc->putc(pData[i]);
+        }
+    }
+}
+
+void CsrLocation::_CsrLocHwOnoff(void)
+{
+	csrLocInst.pPinOnoff->write(1);
+	wait_ms(10);
+	csrLocInst.pPinOnoff->write(0);
+	CSR_LOG_INFO("Onoff pulse given.\r\n");
+}
+
+void CsrLocation::_CsrLocHwReset(void)
+{
+	csrLocInst.pPinReset->write(1);
+	wait_ms(10);
+	csrLocInst.pPinReset->write(0);
+	CSR_LOG_INFO("Reset pulse given.\r\n");
+}