File content as of revision 20:88db82cf3f0f:

/* 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(RawSerial  &_serialLoc,
                         DigitalOut &_pinOnoff,
                         DigitalOut &_pinReset,
                         DigitalIn  &_wakeup,
                         Serial     *debugP):
    serialLoc(_serialLoc),
    pinOnoff(_pinOnoff),
    pinReset(_pinReset),
    wakeup(_wakeup),
    proto(PROTO_OSP),
    locState(CSR_LOC_STATE_IDLE),
    protoState(PROTO_STATE_DET_INVALID),
    pwrMode(PWR_FULL),
    baudRate(),
    computedCheckSum(),
    checksum(),
    msgSize(),
    decodeIndex(),
    protoDetState(STATE_START1),
    pTimeoutChk(NULL),
    bTimeoutFlag(false),
    engStatus(ENGINE_STATUS_NOTOK2SEND),
    svStatus(),
    serialBuf(),
    serialPkt(),
    in(),
    out(),
    pSerialDebug(debugP)
{
    pTimeoutChk = new Timeout();

    pinReset.write(1);
    pinOnoff.write(0);
}

CSRLocation::~CSRLocation(void)
{
    serialLoc.attach(NULL);

    if (pTimeoutChk != NULL) {
        delete pTimeoutChk;
        pTimeoutChk = NULL;
    }
    pinReset.write(0);
    pinOnoff.write(0);
}

void CSRLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb)
{
    appOutCb   = app_output_cb;
    appEventCb = app_event_cb;
}

void
CSRLocation::outputHandler(uint32_t msgId, void *const pMsgData, uint32_t msgLength)
{
    switch (msgId) {
        case LOC_OUTPUT_LOCATION: {
            const tLocPosResp *pPosRsp      = (const tLocPosResp *)pMsgData;
            lastLocation.version            = 1;
            lastLocation.valid              = true;
            lastLocation.gpsTime.gps_week   = pPosRsp->gpsTime.gps_week;
            lastLocation.gpsTime.tow        = pPosRsp->gpsTime.tow;
            lastLocation.utcTime            = pPosRsp->utcTime;
            lastLocation.lat                = pPosRsp->lat;
            lastLocation.lon                = pPosRsp->lon;
            lastLocation.altitude           = pPosRsp->alt;

            if (locationCallback) {
                locationCallback(&lastLocation);
            }
            break;
        }
        case LOC_OUTPUT_SV_STATUS: {
            const tLocSvStatus *pSvStatus   = (const tLocSvStatus *)pMsgData;
            lastLocation.version            = 1;
            lastLocation.valid              = true;
            lastLocation.numGPSSVs          = pSvStatus->numOfSVs;
            lastLocation.numGLOSVs          = pSvStatus->numOfGloSVs;

            break;
        }

        default:
            break;
    }

    if (appOutCb) {
        appOutCb(msgId, pMsgData, msgLength);
    }
}

void
CSRLocation::eventHandler(eCsrLocEventType event, uint32_t data)
{
    if (appEventCb) {
        appEventCb(event, data);
    }
}

void CSRLocation::reset(void)
{
    _CsrLocHwReset();
    lastLocation.valid = false;
}

void CSRLocation::start()
{
    if (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__);

        /* trigger on_off */
        _CsrLocHwOnoff();

        /* open UART */
        if (proto == PROTO_NMEA) {
            CSR_LOG_INFO("Checking NMEA protocol...\r\n");
            protoState = PROTO_STATE_DET_NMEA;
            baudRate   = BAUDRATE_NMEA;
        } else {
            CSR_LOG_INFO("Checking OSP protocol...\r\n");
            protoState = PROTO_STATE_DET_OSP;
            baudRate   = BAUDRATE_OSP;
        }
        _CsrLocUartInit();

        locState    = CSR_LOC_STATE_RUN;
        bPwrModeRsp = false;
        bVerRsp     = false;
    } else {
        CSR_LOG_INFO("Already started.\r\n");
    }
}

void CSRLocation::process(void)
{
    if (locState == CSR_LOC_STATE_RUN) {
        /* wait and process uart data */
        _CsrLocProcessRawStream();
    }
}

void CSRLocation::stop(void)
{
    if (locState == CSR_LOC_STATE_IDLE) {
        return;
    }

    serialLoc.attach(NULL);
    CSR_LOG_INFO("Stop command processed.\r\n");

    if ((locState == CSR_LOC_STATE_RUN) && (pwrMode == PWR_PTF) && (engStatus == ENGINE_STATUS_NOTOK2SEND)) {
        /* in sleep mode, trigger on_off firstly */
        _CsrLocHwOnoff();
        wait_ms(500);
    }
    if (proto == PROTO_NMEA) {
        _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ);
    } else {
        _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ);
    }
    wait_ms(200);
    if (_CsrLocIsWakeup()) {
        _CsrLocHwOnoff();
        wait_ms(300);
        if (_CsrLocIsWakeup()) {
            _CsrLocHwReset();
        }
    }

    locState = CSR_LOC_STATE_IDLE;
    eventHandler(CSR_LOC_EVENT_STOP_RESULT, 0);
}

uint32_t CSRLocation::ioctl(uint32_t command, void *arg)
{
    uint32_t ret = 0;
    
    CSR_LOG_INFO("ioctl command 0x%lx\r\n", command);
    switch(command)
    {
    case CSR_IOCTL_CMD_PROTO_NMEA:
        CSR_LOG_INFO("set NMEA protocol.\r\n");
        proto = PROTO_NMEA;
        break;
    case CSR_IOCTL_CMD_PROTO_OSP:
        CSR_LOG_INFO("set OSP protocol.\r\n");
        proto = PROTO_OSP;
        break;
    case CSR_IOCTL_CMD_WAKEUP_STATUS:
        CSR_LOG_INFO("wakeup status : %d.\r\n", wakeup.read());
        break;
    case CSR_IOCTL_CMD_ONOFF_ON:
        CSR_LOG_INFO("onoff ON.\r\n");
        pinOnoff.write(1);
        break;
    case CSR_IOCTL_CMD_ONOFF_OFF:
        CSR_LOG_INFO("onoff OFF.\r\n");
        pinOnoff.write(0);
        break;
    case CSR_IOCTL_CMD_ONOFF_PULSE:
        CSR_LOG_INFO("onoff pulse.\r\n");
        pinOnoff.write(1);
        wait_ms(100);
        pinOnoff.write(0);
        break;
    case CSR_IOCTL_CMD_RESET_ON:
        CSR_LOG_INFO("reset ON.\r\n");
        pinReset.write(1);
        break;
    case CSR_IOCTL_CMD_RESET_OFF:
        CSR_LOG_INFO("reset OFF.\r\n");
        pinReset.write(0);
        break;

    default:
        CSR_LOG_INFO("Unknown ioctl command 0x%lx\r\n", command);
        ret = 0xFFFFFFFF;
        break;
    }
    
    return ret;
}

void CSRLocation::lpmGetImmediateLocation(void)
{
    if ((locState == CSR_LOC_STATE_RUN) && (pwrMode == PWR_PTF) && (engStatus == ENGINE_STATUS_NOTOK2SEND)) {
        CSR_LOG_INFO("LpmGetPos ");
        _CsrLocHwOnoff();
    } else {
        if (locState != CSR_LOC_STATE_RUN) {
            CSR_LOG_INFO("Not in run state.\r\n");
        } else if (pwrMode != PWR_PTF) {
            CSR_LOG_INFO("Not in low power PTF mode.\r\n");
        }
    }
}

void CSRLocation::CsrLocDebugSwitch2Nmea(void)
{
    if (locState == CSR_LOC_STATE_RUN) {
        _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ);
        wait_ms(200);
        _CsrLocHwReset();
    }
}

void CSRLocation::_CsrLocUartInit(void)
{
    in  = 0;
    out = 0;
    memset(serialBuf, 0, MAX_SERIAL_BUF_LEN);
    memset(serialPkt, 0, MAX_SERIAL_PKT_LEN);

    checksum      = 0;
    msgSize       = 0;
    decodeIndex   = 0;
    protoDetState = STATE_START1;
    pTimeoutChk->attach(this, &CSRLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT);

    serialLoc.baud(baudRate);
    serialLoc.attach(this, &CSRLocation::_CsrLocRxHandler );
}

void CSRLocation::_CsrLocProcessRawStream(void)
{
    uint8_t data;

    if (bTimeoutFlag) {
        pTimeoutChk->detach();
        bTimeoutFlag = false;
        if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) {
            _CsrLocDetProtoOspTimeout();
        } else if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) {
            _CsrLocDetProtoNmeaTimeout();
        } else {
            CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", protoState);
        }
        return;
    }

    while (in != out) {
        if (bTimeoutFlag) {
            pTimeoutChk->detach();
            bTimeoutFlag = false;
            if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) {
                _CsrLocDetProtoOspTimeout();
            } else if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) {
                _CsrLocDetProtoNmeaTimeout();
            } else {
                CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", protoState);
            }
            return;
        }

        data = serialBuf[out++];
        out &= (MAX_SERIAL_BUF_LEN - 1);
        switch (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 (proto == PROTO_NMEA) {
                    _CsrLocProcessRawNmeaStream(data);
                } else {
                    _CsrLocProcessRawOspStream(data);
                }
                break;
            default:
                /* Discard received data */
                //out = pLocInst->in;
                break;
        }
    }
}

void CSRLocation::_CsrLocDetProtoOspTimeout(void)
{
    if (proto == PROTO_NMEA) {
        /* Failed to detect OSP */
        serialLoc.attach(NULL);
        protoState = PROTO_STATE_DET_INVALID;
        baudRate   = BAUDRATE_NMEA;
        CSR_LOG_INFO("Checking OSP failed.\r\n");
        eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
    } else {
        /* Failed to detect OSP and try to detect NMEA */
        serialLoc.attach(NULL);
        if (protoState == PROTO_STATE_DET_OSP) {
            protoState = PROTO_STATE_DET_NMEA;
            baudRate   = BAUDRATE_NMEA;
            CSR_LOG_INFO("Checking OSP protocol failed, now check NMEA protocol...\r\n");
            _CsrLocUartInit();
        } else {
            protoState = PROTO_STATE_DET_INVALID;
            CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n");
            eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
        }
    }
}

void CSRLocation::_CsrLocDetProtoNmeaTimeout(void)
{
    CSR_LOG_INFO("Checking NMEA protocol failed\r\n");

    if (proto == PROTO_NMEA) {
        /* Failed to detect NMEA and try to detect OSP */
        serialLoc.attach(NULL);
        if (protoState == PROTO_STATE_DET_NMEA) {
            protoState = PROTO_STATE_DET_OSP;
            baudRate   = BAUDRATE_OSP;
            CSR_LOG_INFO("Checking NMEA protocol failed, now check OSP protocol...\r\n");
            _CsrLocUartInit();
        } else {
            protoState = PROTO_STATE_DET_INVALID;
            CSR_LOG_INFO("Checking switched NMEA protocol failed.\r\n");
            //            eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
        }
    } else {
        /* Failed to detect NEMA */
        serialLoc.attach(NULL);
        protoState = PROTO_STATE_DET_INVALID;
        baudRate   = BAUDRATE_OSP;
        CSR_LOG_INFO("Checking NMEA failed.\r\n");
        eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
    }
}

void CSRLocation::_CsrLocProcessRawNmeaStream(uint8_t data)
{
    switch (protoDetState) {
        case STATE_START1:
            if (NMEA_MSG_HEAD0 == data) {
                protoDetState = STATE_START2;
            }
            break;

        case STATE_START2:
            if (NMEA_MSG_HEAD1 == data) {
                protoDetState            = STATE_END1;
                decodeIndex              = 0;
                serialPkt[decodeIndex++] = data;
            } else if (NMEA_MSG_HEAD0 == data) {
                protoDetState = STATE_START2;
            } else {
                protoDetState = STATE_START1;
            }
            break;

        case STATE_END1:
            if (NMEA_MSG_TAIL0 == data) {
                pTimeoutChk->detach();
                bTimeoutFlag = false;
                if (proto == PROTO_NMEA) {
                    protoDetState = STATE_START1;
                    if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) {
                        CSR_LOG_INFO("Checking NMEA protocol OK.\r\n");
                        protoState = PROTO_STATE_DET_OK;
                        eventHandler(CSR_LOC_EVENT_START_RESULT, 0);
                    }

                    serialPkt[decodeIndex++] = '\0';
                    _CsrLocProcessRawNmeaPkt();
                } else {
                    serialLoc.attach(NULL);

                    CSR_LOG_INFO("Checking NMEA protocol OK, switching to OSP...\r\n");
                    _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ);
                    wait_ms(100);
                    protoState = PROTO_STATE_DET_OSP_FROM_NMEA;
                    baudRate   = BAUDRATE_OSP;
                    CSR_LOG_INFO("Checking switched OSP protocol...\r\n");
                    _CsrLocUartInit();
                }
            } else if (NMEA_MSG_HEAD0 == data) {
                protoDetState = STATE_START2;
            } else {
                if (decodeIndex < (MAX_SERIAL_PKT_LEN - 2)) {
                    serialPkt[decodeIndex++] = data;
                } else {
                    protoDetState = STATE_START1;
                }
            }
            break;

        default:
            break;
    }
}

void CSRLocation::_CsrLocProcessRawOspStream(uint8_t data)
{
    switch (protoDetState) {
        case STATE_START1:
            if (OSP_MSG_HEAD0 == data) {
                protoDetState = STATE_START2;
            }
            break;

        case STATE_START2:
            if (OSP_MSG_HEAD1 == data) {
                protoDetState = STATE_SIZE1;
            } else if (OSP_MSG_HEAD0 == data) {
                protoDetState = STATE_START2;
            } else {
                protoDetState = STATE_START1;
            }
            break;

        case STATE_SIZE1:
            msgSize       = data;
            msgSize     <<= 8; /* high uint8_t */
            protoDetState = STATE_SIZE2;
            break;

        case STATE_SIZE2:
            msgSize += data;
            if ((MAX_SERIAL_PKT_LEN < msgSize) || (0 == msgSize)) {
                if (OSP_MSG_HEAD0 == data) {
                    protoDetState = STATE_START2;
                } else {
                    protoDetState = STATE_START1;
                }
            } else {
                computedCheckSum = 0;
                decodeIndex      = 0;
                protoDetState    = STATE_PAYLOAD;
            }
            break;

        case STATE_PAYLOAD:
            /* check for a catastrophic error case */
            if (MAX_SERIAL_PKT_LEN <= 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) {
                    protoDetState = STATE_START2;
                } else {
                    protoDetState = STATE_START1;
                }
                break;
            }
            /* Store the byte */
            serialPkt[decodeIndex++] = data;
            computedCheckSum        += data;

            /* Check to see if we've read the full payload */
            if (0 == (--msgSize)) {
                computedCheckSum &= 0x7FFF;
                protoDetState     = STATE_CHECKSUM1;
            }
            break;

        case STATE_CHECKSUM1:
            checksum      = data;
            checksum    <<= 8;
            protoDetState = STATE_CHECKSUM2;
            break;

        case STATE_CHECKSUM2:
            checksum += data;
            if (computedCheckSum != checksum) {
                if (OSP_MSG_HEAD0 == data) {
                    protoDetState = STATE_START2;
                } else {
                    protoDetState = STATE_START1;
                }
            } else {
                protoDetState = STATE_END1;
            }
            break;

        case STATE_END1:
            if (OSP_MSG_TAIL0 == data) {
                protoDetState = STATE_END2;
            } else {
                if (OSP_MSG_HEAD0 == data) {
                    protoDetState = STATE_START2;
                } else {
                    protoDetState = STATE_START1;
                }
            }
            break;

        case STATE_END2:
            if (OSP_MSG_TAIL1 == data) {
                pTimeoutChk->detach();
                bTimeoutFlag  = false;
                protoDetState = STATE_START1;

                if (proto == PROTO_NMEA) {
                    serialLoc.attach(NULL);

                    CSR_LOG_INFO("Checking OSP protocol OK, switching to NMEA...\r\n");
                    _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ);
                    wait_ms(100);
                    protoState = PROTO_STATE_DET_NMEA_FROM_OSP;
                    baudRate   = BAUDRATE_NMEA;
                    CSR_LOG_INFO("Checking switched NMEA protocol...\r\n");
                    _CsrLocUartInit();
                } else {
                    if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) {
                        CSR_LOG_INFO("Checking OSP protocol OK.\r\n");
                        protoState = PROTO_STATE_DET_OK;
                        eventHandler(CSR_LOC_EVENT_START_RESULT, 0);
                    }

                    _CsrLocProcessRawOspPkt();
                }
            } else {
                if (OSP_MSG_HEAD0 == data) {
                    protoDetState = STATE_START2;
                } else {
                    protoDetState = STATE_START1;
                }
            }
            break;
    }   /* switch. */
}

void CSRLocation::_CsrLocProcessRawNmeaPkt(void)
{
    /* report NMEA */
    outputHandler(LOC_OUTPUT_NMEA, serialPkt, strlen((char *)serialPkt));

#if 0    
    tLocPosResp pos;
    const char *pNmeaGga = "GPGGA";
    float       deg, min;
    char        ns, ew;
    int         svUsed;
    float       horDop;
    int         valid;
    int         i, cnt;


    if (strncmp((char *)serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) {
        cnt = 0;
        for (i = 0; i < (int)strlen((char *)serialPkt); i++) {
            if (serialPkt[i] == ',') {
                cnt++;
                if (cnt == 6) {
                    break;
                }
            }
        }
        if (cnt != 6) {
            return;
        }
        i++;
        sscanf((char *)(&serialPkt[i]), "%d,", &valid);
        if (valid == 0) {
            return;
        }
        
        /* Parse GPGGA and output position information */
        memset(&pos, 0, sizeof(tLocPosResp));
        if (sscanf((char *)serialPkt, "GPGGA,%f,%lf,%c,%lf,%c,%d,%d,%f,%lf", &pos.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;

            outputHandler(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp));
        }
    }
#endif
}

void CSRLocation::_CsrLocProcessRawOspPkt(void)
{
    tOspMsg  *pOspMsg;
    uint32_t  msgSize;
    CsrResult result;

    static uint8_t buffer[512];
    static unsigned maxMessageSize = 0;

    msgSize = _CsrLocCalcMsgSize();
    if (msgSize > 0) {
        msgSize += sizeof(tOspMsg);
        if (msgSize > maxMessageSize) {
            maxMessageSize = msgSize;
            CSR_LOG_INFO("max message size %u\r\n", maxMessageSize);
        }
        pOspMsg  = (tOspMsg *)buffer;
        memset(pOspMsg, 0, msgSize);
    } else {
        /* discard the unprocessed message */
        return;
    }

    result = _CsrLocDecodeOspPkt(serialPkt, decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length);
    if (CSR_RESULT_SUCCESS == result) {
        _CsrLocProcessOspPkt(pOspMsg);
    }
}

uint32_t CSRLocation::_CsrLocCalcMsgSize(void)
{
    uint8_t *ptr     = 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->gpsTime.gps_week   = BINARY_IMPORT_UINT16(ptr);
                pPos->gpsTime.tow        = BINARY_IMPORT_UINT32(ptr);
                pPos->utcTime            = CSR_ULOC_UTC_GPS_OFFSET_MS + (uint64_t)pPos->gpsTime.gps_week * (uint64_t)CSR_ULOC_SEC_IN_WEEK_MS + (uint64_t)pPos->gpsTime.tow - (uint64_t)CSR_ULOC_UTC_LEAP_OFFSET_MS;
                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 = &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 = &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:
            outputHandler(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp));
            break;
        case OSP_MSG_GNSS_SAT_DATA:
            break;
        case OSP_MSG_GNSS_NAV_DATA:
            outputHandler(LOC_OUTPUT_SV_STATUS, &svStatus, sizeof(tLocSvStatus));
            break;
        case OSP_MSG_OK_TO_SEND:
            engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND;
            CSR_LOG_INFO("Ok to send %u\r\n", engStatus);
            break;
        case OSP_MSG_SW_VERSION:
            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 (!bVerRsp) {
                _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
            }

            if (!bPwrModeRsp) {
                if (pwrMode == PWR_PTF) {
                    CSR_LOG_INFO("Send PTF command.\r\n");
                    _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
                } else {
                    CSR_LOG_INFO("Send FPM command.\r\n");
                    _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
                }
            }
            break;
        case OSP_MSG_PWR_MODE_LPM_RSP:
            bPwrModeRsp = true;
            CSR_LOG_INFO("lpm response.\r\n");
            break;
        case OSP_MSG_PWR_MODE_FPM_RSP:
            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)
{
    bTimeoutFlag = true;
}

void CSRLocation::_CsrLocRxHandler(void)
{
    serialBuf[in++] = serialLoc.getc();
    in             &= (MAX_SERIAL_BUF_LEN - 1);
    if (in == out) {
        CSR_LOG_INFO("rx overwritten %lu %lu.\r\n", in, out);
    }
}

void CSRLocation::_CsrLocSendData(eSendDataType type)
{
    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++) {
            serialLoc.putc(pData[i]);
        }
    }
}

void CSRLocation::_CsrLocHwOnoff(void)
{
    pinOnoff.write(1);
    wait_ms(100);
    pinOnoff.write(0);
    CSR_LOG_INFO("Onoff pulse given.\r\n");
}

void CSRLocation::_CsrLocHwReset(void)
{
    pinReset.write(0);
    wait_ms(100);
    pinReset.write(1);
    CSR_LOG_INFO("Reset pulse given.\r\n");
}

bool CSRLocation::_CsrLocIsWakeup(void)
{
    CSR_LOG_INFO("Is wakeup %d.\r\n", wakeup.read());
    return wakeup.read() != 0 ? true : false;
}

bool CSRLocation::setPowerMode(GPSProvider::PowerMode_t _pwrMode)
{
    switch (_pwrMode) {
        case GPSProvider::POWER_FULL:
            pwrMode = PWR_FULL;
            break;
        case GPSProvider::POWER_LOW:
        default:
            pwrMode = PWR_PTF; /* push to fix */
            break;
    }

    return (true);
}