Csr location class shows location and satellite information, which supports H13467 + ST F103RB/NXP LCP1549 boards now.
Dependents: CsrLocationDemo CsrLocationDemo
Fork of CsrLocation by
Diff: CsrLocation.cpp
- Revision:
- 12:5fb829ce6b82
- Parent:
- 8:fa29f8455134
- Child:
- 13:1d2f2dccc4ee
- Child:
- 14:bb86a6ca6662
--- a/CsrLocation.cpp Tue Oct 28 12:20:42 2014 +0000 +++ b/CsrLocation.cpp Tue Nov 04 08:41:25 2014 +0000 @@ -1,1046 +1,1018 @@ - -/* CsrLocation class for mbed Microcontroller +/* 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}; + 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"; +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) +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) { - memset(&csrLocInst, 0, sizeof(tCsrLocInst)); - csrLocInst.pSerialDebug = pLocConfig->pSerialDebug; - csrLocInst.pSerialLoc = pLocConfig->pSerialLoc; - csrLocInst.pPinOnoff = pLocConfig->pPinOnoff; - csrLocInst.pPinReset = pLocConfig->pPinReset; - csrLocInst.pWakeup = pLocConfig->pWakeup; - csrLocInst.pTimeoutChk = new Timeout(); + pTimeoutChk = new Timeout(); + + pinReset.write(1); + pinOnoff.write(0); +} - csrLocInst.pPinReset->write(1); - csrLocInst.pPinOnoff->write(0); +CSRLocation::~CSRLocation(void) +{ + serialLoc.attach(NULL); + + if (pTimeoutChk != NULL) { + delete pTimeoutChk; + pTimeoutChk = NULL; + } + pinReset.write(0); + pinOnoff.write(0); } -CsrLocation::~CsrLocation(void) +void CSRLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb) { - 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)); - + appOutCb = app_output_cb; + appEventCb = app_event_cb; } -void CsrLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb) +void +CSRLocation::outputHandler(uint32_t msgId, void *const pMsgData, uint32_t msgLength) { - csrLocInst.appOutCb = app_output_cb; - csrLocInst.appEventCb = app_event_cb; + switch (msgId) { + case LOC_OUTPUT_LOCATION: { + const tLocPosResp *pPosRsp = (const tLocPosResp *)pMsgData; + lastLocation.version = 1; + lastLocation.valid = true; + 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; + lastLocation.u.gpsTime.gps_week = pSvStatus->gps_week; + lastLocation.u.gpsTime.tow = pSvStatus->tow; + + break; + } + + default: + break; + } + + if (appOutCb) { + appOutCb(msgId, pMsgData, msgLength); + } } -void CsrLocation::CsrLocReset(void) +void +CSRLocation::eventHandler(eCsrLocEventType event, uint32_t data) { - _CsrLocHwReset(); + if (appEventCb) { + appEventCb(event, data); + } } -void CsrLocation::CsrLocStart(ePowerMode pwrMode, eProto proto) +void CSRLocation::reset(void) { - csrLocInst.pwrMode = pwrMode; + _CsrLocHwReset(); + lastLocation.valid = false; +} - if(csrLocInst.locState == CSR_LOC_STATE_IDLE) - { +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__); - csrLocInst.proto = proto; - /* open UART */ - if(proto == PROTO_NMEA) - { + if (proto == PROTO_NMEA) { CSR_LOG_INFO("Checking NMEA protocol...\r\n"); - csrLocInst.protoState = PROTO_STATE_DET_NMEA; - csrLocInst.baudRate = BAUDRATE_NMEA; - } - else - { + protoState = PROTO_STATE_DET_NMEA; + baudRate = BAUDRATE_NMEA; + } else { CSR_LOG_INFO("Checking OSP protocol...\r\n"); - csrLocInst.protoState = PROTO_STATE_DET_OSP; - csrLocInst.baudRate = BAUDRATE_OSP; + protoState = PROTO_STATE_DET_OSP; + baudRate = BAUDRATE_OSP; } _CsrLocUartInit(); /* trigger on_off */ _CsrLocHwOnoff(); - csrLocInst.locState = CSR_LOC_STATE_RUN; - csrLocInst.bPwrModeRsp = false; - csrLocInst.bVerRsp = false; - - } - else - { + locState = CSR_LOC_STATE_RUN; + bPwrModeRsp = false; + bVerRsp = false; + } else { CSR_LOG_INFO("Already started.\r\n"); } } -void CsrLocation::CsrLocUpdate(void) +void CSRLocation::process(void) { - if(csrLocInst.locState == CSR_LOC_STATE_RUN) - { + if (locState == CSR_LOC_STATE_RUN) { /* wait and process uart data */ _CsrLocProcessRawStream(); } } -void CsrLocation::CsrLocStop(void) +void CSRLocation::stop(void) { - csrLocInst.pSerialLoc->attach(NULL); + if (locState == CSR_LOC_STATE_IDLE) { + return; + } + + serialLoc.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) - { + 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(csrLocInst.proto == PROTO_NMEA) - { + if (proto == PROTO_NMEA) { _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ); - } - else - { + } else { _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ); } wait_ms(200); - if(_CsrLocIsWakeup()) - { + if (_CsrLocIsWakeup()) { _CsrLocHwOnoff(); - wait_ms(300); - if(_CsrLocIsWakeup()) - { + wait_ms(300); + if (_CsrLocIsWakeup()) { _CsrLocHwReset(); } } - - csrLocInst.locState = CSR_LOC_STATE_IDLE; - csrLocInst.appEventCb(CSR_LOC_EVENT_STOP_RESULT, 0); + locState = CSR_LOC_STATE_IDLE; + eventHandler(CSR_LOC_EVENT_STOP_RESULT, 0); } -void CsrLocation::CsrLocLpmGetPos(void) +uint32_t CSRLocation::ioctl(uint32_t command, void *arg) { - if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND) + 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"); + } } - 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) +void CSRLocation::CsrLocDebugSwitch2Nmea(void) { - if(csrLocInst.locState == CSR_LOC_STATE_RUN) - { + if (locState == CSR_LOC_STATE_RUN) { _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ); wait_ms(200); _CsrLocHwReset(); - } + } } -void CsrLocation::_CsrLocUartInit(void) +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); + in = 0; + out = 0; + memset(serialBuf, 0, MAX_SERIAL_BUF_LEN); + memset(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); + checksum = 0; + msgSize = 0; + decodeIndex = 0; + protoDetState = STATE_START1; + pTimeoutChk->attach(this, &CSRLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT); - pLocInst->pSerialLoc->baud(pLocInst->baudRate); - pLocInst->pSerialLoc->attach(this, &CsrLocation::_CsrLocRxHandler ); + serialLoc.baud(baudRate); + serialLoc.attach(this, &CSRLocation::_CsrLocRxHandler ); } -void CsrLocation::_CsrLocProcessRawStream(void) +void CSRLocation::_CsrLocProcessRawStream(void) { - tCsrLocInst *pLocInst = &csrLocInst; - uint8_t data; + 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) - { + if (bTimeoutFlag) { + pTimeoutChk->detach(); + bTimeoutFlag = false; + if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) { _CsrLocDetProtoOspTimeout(); - } - else if(pLocInst->protoState == PROTO_STATE_DET_NMEA || pLocInst->protoState == PROTO_STATE_DET_NMEA_FROM_OSP) - { + } 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", pLocInst->protoState); + } else { + CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", 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) - { + if (in != out) { + 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); - } - else - { - _CsrLocProcessRawOspStream(data); - } - break; - default: - /* Discard received data */ - //pLocInst->out = pLocInst->in; - break; + 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) +void CSRLocation::_CsrLocDetProtoOspTimeout(void) { - tCsrLocInst *pLocInst = &csrLocInst; - - if(pLocInst->proto == PROTO_NMEA) - { + if (proto == PROTO_NMEA) { /* Failed to detect OSP */ - pLocInst->pSerialLoc->attach(NULL); - pLocInst->protoState = PROTO_STATE_DET_INVALID; - pLocInst->baudRate = BAUDRATE_NMEA; + serialLoc.attach(NULL); + protoState = PROTO_STATE_DET_INVALID; + baudRate = BAUDRATE_NMEA; CSR_LOG_INFO("Checking OSP failed.\r\n"); - pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); - } - else - { + eventHandler(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; + 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 - { - pLocInst->protoState = PROTO_STATE_DET_INVALID; + } else { + protoState = PROTO_STATE_DET_INVALID; CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n"); - pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); + eventHandler(CSR_LOC_EVENT_START_RESULT, 1); } } } -void CsrLocation::_CsrLocDetProtoNmeaTimeout(void) +void CSRLocation::_CsrLocDetProtoNmeaTimeout(void) { - tCsrLocInst *pLocInst = &csrLocInst; - CSR_LOG_INFO("Checking NMEA protocol failed\r\n"); - if(pLocInst->proto == PROTO_NMEA) - { + if (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; + 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 - { - 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 */ + 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); } - 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) +{ + 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::_CsrLocProcessRawNmeaStream(uint8_t data) +void CSRLocation::_CsrLocProcessRawOspStream(uint8_t data) { - tCsrLocInst *pLocInst = &csrLocInst; - - switch (pLocInst->protoDetState) - { - case STATE_START1: - if (NMEA_MSG_HEAD0 == data) - { - pLocInst->protoDetState = STATE_START2; - } - break; + 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_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); + 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; - 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(); + 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; + } } - } - 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; - } + break; + } /* switch. */ } -void CsrLocation::_CsrLocProcessRawOspStream(uint8_t data) +void CSRLocation::_CsrLocProcessRawNmeaPkt(void) { - 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; + /* report NMEA */ + outputHandler(LOC_OUTPUT_NMEA, serialPkt, strlen((char *)serialPkt)); - 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) - { - 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; +#if 0 tLocPosResp pos; const char *pNmeaGga = "GPGGA"; float deg, min; - char ns, ew; - int svUsed; - float horDop; + char ns, ew; + int svUsed; + float horDop; int valid; int i, cnt; - if(strncmp((char *)pLocInst->serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) - { + + if (strncmp((char *)serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) { cnt = 0; - for(i = 0; i < (int)strlen((char *)pLocInst->serialPkt); i++) - { - if(pLocInst->serialPkt[i] == ',') - { + for (i = 0; i < (int)strlen((char *)serialPkt); i++) { + if (serialPkt[i] == ',') { cnt++; - if(cnt == 6) - { + if (cnt == 6) { break; } } } - if(cnt != 6) - { + if (cnt != 6) { return; } i++; - sscanf((char *)(&pLocInst->serialPkt[i]), "%d,", &valid); - if(valid == 0) - { + sscanf((char *)(&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; + if (sscanf((char *)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)); + outputHandler(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp)); } } +#endif } -void CsrLocation::_CsrLocProcessRawOspPkt(void) +void CSRLocation::_CsrLocProcessRawOspPkt(void) { - tCsrLocInst *pLocInst = &csrLocInst; - tOspMsg *pOspMsg; - uint32_t msgSize; - CsrResult result; - + tOspMsg *pOspMsg; + uint32_t msgSize; + CsrResult result; + + static uint8_t buffer[256]; + static unsigned maxMessageSize = 0; + msgSize = _CsrLocCalcMsgSize(); - if(msgSize > 0) - { + if (msgSize > 0) { msgSize += sizeof(tOspMsg); - pOspMsg = (tOspMsg *)malloc(msgSize); - if(pOspMsg != NULL) - { - memset(pOspMsg, 0, msgSize); + if (msgSize > maxMessageSize) { + maxMessageSize = msgSize; + CSR_LOG_INFO("max message size %u\r\n", maxMessageSize); } - else - { - CSR_LOG_INFO("No memory for received OSP message.\r\n"); - return; - } - } - else - { + pOspMsg = (tOspMsg *)buffer; + memset(pOspMsg, 0, msgSize); + } 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); + result = _CsrLocDecodeOspPkt(serialPkt, decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length); + if (CSR_RESULT_SUCCESS == result) { + _CsrLocProcessOspPkt(pOspMsg); } - - free(pOspMsg); } -uint32_t CsrLocation::_CsrLocCalcMsgSize(void) +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; + 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); + 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 */ + 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); + 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; + 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; - } + default: + msgSize = 0; + break; + } return msgSize; } -CsrResult CsrLocation::_CsrLocDecodeOspPkt( uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen) +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; + 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; + 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) - { + if ((OSP_MSG_PWR_MODE_RSP == *pMsgId) || (OSP_MSG_MULTI_CONSTELLATION == *pMsgId)) { sid = BINARY_IMPORT_UINT8(ptr); - *pMsgId = OSP_MAKE_MSG_ID(mid,sid); + *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; + 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_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); - 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; + 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; } - else + + case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */ { - *pMsgLen = sizeof(*pPos); + tLocSvStatus *pSvStatus = &svStatus; + + *pMsgLen = sizeof(*pSvStatus); - 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; + 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; } - 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++) + case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */ { - 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 - } + 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; } - 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; + + 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); } - 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; } - break; - } + case OSP_MSG_HW_CONFIG_REQ: /* 0x47 */ + break; - case OSP_MSG_HW_CONFIG_REQ: /* 0x47 */ - break; - - case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */ - 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; + case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */ + *((uint8_t *)pMsgData) = *ptr; + *pMsgLen = sizeof(uint8_t); + break; - default: - tRet = CSR_RESULT_FAILURE; - break; + default: + tRet = CSR_RESULT_FAILURE; + break; } /* check if length does not match */ - if(tRet == CSR_RESULT_FAILURE) - { + if (tRet == CSR_RESULT_FAILURE) { *pMsgId = *pMsgLen = 0; } - return tRet; - + return tRet; } /* CsrUlocCodecSsbDecode() */ -void CsrLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg) +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) - { - CSR_LOG_INFO("Send PTF command.\r\n"); - _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ); + 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); } - else - { - CSR_LOG_INFO("Send FPM command.\r\n"); - _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_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: - 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; + 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) +void CSRLocation::_CsrLocTimeout(void) { - csrLocInst.bTimeoutFlag = true; + bTimeoutFlag = true; } -void CsrLocation::_CsrLocRxHandler(void) +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); + 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) +void CSRLocation::_CsrLocSendData(eSendDataType type) { - tCsrLocInst *pLocInst = &csrLocInst; - uint32_t i, size; + 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; + 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]); + if (pData != NULL) { + for (i = 0; i < size; i++) { + serialLoc.putc(pData[i]); } } } -void CsrLocation::_CsrLocHwOnoff(void) +void CSRLocation::_CsrLocHwOnoff(void) { - csrLocInst.pPinOnoff->write(1); - wait_ms(100); - csrLocInst.pPinOnoff->write(0); - CSR_LOG_INFO("Onoff pulse given.\r\n"); + 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"); } -void CsrLocation::_CsrLocHwReset(void) +bool CSRLocation::_CsrLocIsWakeup(void) { - csrLocInst.pPinReset->write(0); - wait_ms(100); - csrLocInst.pPinReset->write(1); - CSR_LOG_INFO("Reset pulse given.\r\n"); + CSR_LOG_INFO("Is wakeup %d.\r\n", wakeup.read()); + return wakeup.read() != 0 ? true : false; } -bool CsrLocation::_CsrLocIsWakeup(void) +bool CSRLocation::setPowerMode(GPSProvider::PowerMode_t _pwrMode) { - CSR_LOG_INFO("Is wakeup %d.\r\n", csrLocInst.pWakeup->read()); - return csrLocInst.pWakeup->read() != 0 ? true : false; + switch (_pwrMode) { + case GPSProvider::POWER_FULL: + pwrMode = PWR_FULL; + break; + case GPSProvider::POWER_LOW: + default: + pwrMode = PWR_PTF; /* push to fix */ + break; + } + + return (true); }