File content as of revision 5:219dfc2905dc:

/* CsrLocation class for mbed Microcontroller
 * Copyright 2014 CSR plc
 */


#include "mbed.h"
#include "CsrLocation.h"


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 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 char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n";
static char sNmeaStopReq[] = "$PSRF117,16*0B\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();
}

eCsrLocState CsrLocation::CsrLocGetState(void)
{
	return csrLocInst.locState;
}

void CsrLocation::CsrLocStart(ePowerMode pwrMode, eProto proto)
{
    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__);

        csrLocInst.proto = proto;
        
        /* open UART */
        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 */
        _CsrLocHwOnoff();
		if(csrLocInst.pwrMode == PWR_PTF)
		{
			_CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
		}

        csrLocInst.locState = CSR_LOC_STATE_RUN;
    	csrLocInst.bPwrModeRsp = false;
    	csrLocInst.bVerRsp = false;
    	
    }
    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);
    }
    if(csrLocInst.proto == PROTO_NMEA)
    {
        _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ);
    }
    else
    {
   		if(csrLocInst.pwrMode == PWR_PTF)
		{
    		CSR_LOG_INFO("Stop with fpm.\r\n");
			_CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
		}

        _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();
    }
    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)
{
    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->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;
    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++];
        pLocInst->out &= (MAX_SERIAL_BUF_LEN-1);
        switch(pLocInst->protoState)
        {
        case PROTO_STATE_DET_OSP:
        case PROTO_STATE_DET_OSP_FROM_NMEA:
            _CsrLocProcessRawOspStream(data);
            break;
        case PROTO_STATE_DET_NMEA:
        case PROTO_STATE_DET_NMEA_FROM_OSP:
            _CsrLocProcessRawNmeaStream(data);
            break;
        case PROTO_STATE_DET_OK:
            if(pLocInst->proto == PROTO_NMEA)
            {
                _CsrLocProcessRawNmeaStream(data);
            }
            else
            {
                _CsrLocProcessRawOspStream(data);
            }
            break;
        default:
            /* Discard received data */
            //pLocInst->out = pLocInst->in;
            break;
        }
    }
}

void CsrLocation::_CsrLocDetProtoOspTimeout(void)
{
    tCsrLocInst *pLocInst = &csrLocInst;

    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);
        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);
        }
    }
}

void CsrLocation::_CsrLocDetProtoNmeaTimeout(void)
{
    tCsrLocInst *pLocInst = &csrLocInst;

    CSR_LOG_INFO("Checking NMEA protocol failed\r\n");

    if(pLocInst->proto == PROTO_NMEA)
    {
        /* Failed to detect NMEA and try to detect OSP */
        pLocInst->pSerialLoc->attach(NULL);
        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);
    }
}

void CsrLocation::_CsrLocProcessRawNmeaStream(uint8_t data)
{
    tCsrLocInst *pLocInst = &csrLocInst;
    
    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;
            pLocInst->decodeIndex = 0;
            pLocInst->serialPkt[pLocInst->decodeIndex++] = data;
        }
        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->pTimeoutChk->detach();
            pLocInst->bTimeoutFlag = false;
            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->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;
    }        
}

void CsrLocation::_CsrLocProcessRawOspStream(uint8_t 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 uint8_t */
        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;
            pLocInst->protoDetState = STATE_START1;

            if(pLocInst->proto == PROTO_NMEA)
            {
                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();

            }
            else
            {
                if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA)
                {
                    if(pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA)
                    {
                        if(!csrLocInst.bPwrModeRsp)
				        {
				            if(csrLocInst.pwrMode == PWR_PTF)
				            {
				                _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
				            }
				            else
				            {
				                _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
				            }
				        }
                	}
                    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
        {
            if (OSP_MSG_HEAD0 == data)
            {
                pLocInst->protoDetState = STATE_START2;
            }
            else
            {
                pLocInst->protoDetState = STATE_START1;
            }
        }            
        break;
    }   /* switch. */
    
}

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;
    tOspMsg     *pOspMsg;
    uint32_t    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);
}

uint32_t CsrLocation::_CsrLocCalcMsgSize(void)
{
    tCsrLocInst *pLocInst = &csrLocInst;
    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);

    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(uint8_t);
        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(uint8_t);
        break;
    case OSP_MSG_GNSS_NAV_DATA:
        msgSize = sizeof(uint8_t);
        break;

    default :
        msgSize = 0;
        break;
    }   

    return msgSize;
}

CsrResult CsrLocation::_CsrLocDecodeOspPkt( uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen)
{
    CsrResult tRet = CSR_RESULT_SUCCESS;
    uint8_t *ptr = pPayload;
    uint32_t i;
    uint8_t 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 */
        *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr);
        *pMsgLen = sizeof(uint8_t);
        break;

    case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */
    {
        tLocPosResp *pPos = (tLocPosResp*) pMsgData;
        uint16_t   valid;

        valid = BINARY_IMPORT_UINT16(ptr);
        if(valid != 0)
        {
            tRet = CSR_RESULT_FAILURE;
        }
        else
        {
            *pMsgLen = sizeof(*pPos);

            ptr += 2;
            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;
            pPos->lon       = (double)BINARY_IMPORT_SINT32(ptr);
            pPos->lon      *= 1e-7;
            ptr += 4;
            pPos->alt       = (double)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;
        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);

        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 = (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 = (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
                }
            }
            else if(2 == gnssType) // GLONASS
            {
                index = pSvStatus->numOfGloSVs;
                if(index < CODEC_GLO_MAX_CHANNELS && cno>0)
                {
                    int16_t freqChan = (satInfo & 0X1F00)>>8;
                    int16_t slotNum  = (satInfo & 0X00FF);
                    if(slotNum > 0)
                    {
                        if(freqChan & 0X0010)
                        {
                            freqChan |= 0xFFE0;
                        }
                        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);
                    }
                }
            }
        }

        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 */
        *((uint8_t *)pMsgData) = *ptr;
        *pMsgLen = sizeof(uint8_t);
        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:
    	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)
    	{
            _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
        }
    
        if(!csrLocInst.bPwrModeRsp)
        {
            if(csrLocInst.pwrMode == PWR_PTF)
            {
                _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
            }
            else
            {
                _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
            }
        }
        break;
    case OSP_MSG_PWR_MODE_LPM_RSP:
    	csrLocInst.bPwrModeRsp = true;
        CSR_LOG_INFO("lpm response.\r\n");
        break;
    case OSP_MSG_PWR_MODE_FPM_RSP:
    	csrLocInst.bPwrModeRsp = true;
        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 %lu %lu.\r\n", pLocInst->in, pLocInst->out);
    }
}

void CsrLocation::_CsrLocSendData(eSendDataType type)
{
    tCsrLocInst *pLocInst = &csrLocInst;
    uint32_t i, size;
    const uint8_t *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 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;
    }

    if(pData != NULL)
    {
        for (i = 0; i < size; i++)
        {
            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");
}