Qualcomm Technologies International, Ltd.
Csr location class shows location and satellite information, which supports H13467 + ST F103RB/NXP LCP1549 boards now.
Dependents: CsrLocationDemo CsrLocationDemo
Fork of
CsrLocation
by 
jie zhao
Diff: CsrLocation.cpp
- Revision:
- 12:5fb829ce6b82
- Parent:
- 8:fa29f8455134
- Child:
- 13:1d2f2dccc4ee
- Child:
- 14:bb86a6ca6662
diff -r 10ba3c761444 -r 5fb829ce6b82 CsrLocation.cpp
--- 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);
}
Repository toolbox
| Export to desktop IDE |
Repository details
| Type: | Library |
|---|---|
| Created: | 12 Sep 2014 |
| Imports: | 24 |
| Forks: | 0 |
| Commits: | 21 |
| Dependents: | 2 |
| Dependencies: | 0 |
| Followers: | 52 |
Components
GPS mbed Shield