Csr location class shows location and satellite information, which supports H13467 + ST F103RB/NXP LCP1549 boards now.
Dependents: CsrLocationDemo CsrLocationDemo
Fork of CsrLocation by
CsrLocation.cpp
- Committer:
- zhjcpi
- Date:
- 2014-11-27
- Revision:
- 20:88db82cf3f0f
- Parent:
- 18:5d72465991f5
File content as of revision 20:88db82cf3f0f:
/* CSRLocation class for mbed Microcontroller * Copyright 2014 CSR plc */ #include "mbed.h" #include "CsrLocation.h" static uint8_t sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3}; static uint8_t sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3}; static uint8_t sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x63, 0xB0, 0xB3}; static uint8_t sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3}; static uint8_t sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x12, 0xC0, 0x01, 0x64, 0xB0, 0xB3}; static char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n"; static char sNmeaStopReq[] = "$PSRF117,16*0B\r\n"; CSRLocation::CSRLocation(RawSerial &_serialLoc, DigitalOut &_pinOnoff, DigitalOut &_pinReset, DigitalIn &_wakeup, Serial *debugP): serialLoc(_serialLoc), pinOnoff(_pinOnoff), pinReset(_pinReset), wakeup(_wakeup), proto(PROTO_OSP), locState(CSR_LOC_STATE_IDLE), protoState(PROTO_STATE_DET_INVALID), pwrMode(PWR_FULL), baudRate(), computedCheckSum(), checksum(), msgSize(), decodeIndex(), protoDetState(STATE_START1), pTimeoutChk(NULL), bTimeoutFlag(false), engStatus(ENGINE_STATUS_NOTOK2SEND), svStatus(), serialBuf(), serialPkt(), in(), out(), pSerialDebug(debugP) { pTimeoutChk = new Timeout(); pinReset.write(1); pinOnoff.write(0); } CSRLocation::~CSRLocation(void) { serialLoc.attach(NULL); if (pTimeoutChk != NULL) { delete pTimeoutChk; pTimeoutChk = NULL; } pinReset.write(0); pinOnoff.write(0); } void CSRLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb) { appOutCb = app_output_cb; appEventCb = app_event_cb; } void CSRLocation::outputHandler(uint32_t msgId, void *const pMsgData, uint32_t msgLength) { switch (msgId) { case LOC_OUTPUT_LOCATION: { const tLocPosResp *pPosRsp = (const tLocPosResp *)pMsgData; lastLocation.version = 1; lastLocation.valid = true; lastLocation.gpsTime.gps_week = pPosRsp->gpsTime.gps_week; lastLocation.gpsTime.tow = pPosRsp->gpsTime.tow; lastLocation.utcTime = pPosRsp->utcTime; lastLocation.lat = pPosRsp->lat; lastLocation.lon = pPosRsp->lon; lastLocation.altitude = pPosRsp->alt; if (locationCallback) { locationCallback(&lastLocation); } break; } case LOC_OUTPUT_SV_STATUS: { const tLocSvStatus *pSvStatus = (const tLocSvStatus *)pMsgData; lastLocation.version = 1; lastLocation.valid = true; lastLocation.numGPSSVs = pSvStatus->numOfSVs; lastLocation.numGLOSVs = pSvStatus->numOfGloSVs; break; } default: break; } if (appOutCb) { appOutCb(msgId, pMsgData, msgLength); } } void CSRLocation::eventHandler(eCsrLocEventType event, uint32_t data) { if (appEventCb) { appEventCb(event, data); } } void CSRLocation::reset(void) { _CsrLocHwReset(); lastLocation.valid = false; } void CSRLocation::start() { if (locState == CSR_LOC_STATE_IDLE) { /* Csr Location SDK version */ CSR_LOG_INFO("==== CSR LOC SDK version: %s Date: %s ====\r\n", CSR_LOC_SDK_VER, __DATE__ " " __TIME__); /* trigger on_off */ _CsrLocHwOnoff(); /* open UART */ if (proto == PROTO_NMEA) { CSR_LOG_INFO("Checking NMEA protocol...\r\n"); protoState = PROTO_STATE_DET_NMEA; baudRate = BAUDRATE_NMEA; } else { CSR_LOG_INFO("Checking OSP protocol...\r\n"); protoState = PROTO_STATE_DET_OSP; baudRate = BAUDRATE_OSP; } _CsrLocUartInit(); locState = CSR_LOC_STATE_RUN; bPwrModeRsp = false; bVerRsp = false; } else { CSR_LOG_INFO("Already started.\r\n"); } } void CSRLocation::process(void) { if (locState == CSR_LOC_STATE_RUN) { /* wait and process uart data */ _CsrLocProcessRawStream(); } } void CSRLocation::stop(void) { if (locState == CSR_LOC_STATE_IDLE) { return; } serialLoc.attach(NULL); CSR_LOG_INFO("Stop command processed.\r\n"); if ((locState == CSR_LOC_STATE_RUN) && (pwrMode == PWR_PTF) && (engStatus == ENGINE_STATUS_NOTOK2SEND)) { /* in sleep mode, trigger on_off firstly */ _CsrLocHwOnoff(); wait_ms(500); } if (proto == PROTO_NMEA) { _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ); } else { _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ); } wait_ms(200); if (_CsrLocIsWakeup()) { _CsrLocHwOnoff(); wait_ms(300); if (_CsrLocIsWakeup()) { _CsrLocHwReset(); } } locState = CSR_LOC_STATE_IDLE; eventHandler(CSR_LOC_EVENT_STOP_RESULT, 0); } uint32_t CSRLocation::ioctl(uint32_t command, void *arg) { uint32_t ret = 0; CSR_LOG_INFO("ioctl command 0x%lx\r\n", command); switch(command) { case CSR_IOCTL_CMD_PROTO_NMEA: CSR_LOG_INFO("set NMEA protocol.\r\n"); proto = PROTO_NMEA; break; case CSR_IOCTL_CMD_PROTO_OSP: CSR_LOG_INFO("set OSP protocol.\r\n"); proto = PROTO_OSP; break; case CSR_IOCTL_CMD_WAKEUP_STATUS: CSR_LOG_INFO("wakeup status : %d.\r\n", wakeup.read()); break; case CSR_IOCTL_CMD_ONOFF_ON: CSR_LOG_INFO("onoff ON.\r\n"); pinOnoff.write(1); break; case CSR_IOCTL_CMD_ONOFF_OFF: CSR_LOG_INFO("onoff OFF.\r\n"); pinOnoff.write(0); break; case CSR_IOCTL_CMD_ONOFF_PULSE: CSR_LOG_INFO("onoff pulse.\r\n"); pinOnoff.write(1); wait_ms(100); pinOnoff.write(0); break; case CSR_IOCTL_CMD_RESET_ON: CSR_LOG_INFO("reset ON.\r\n"); pinReset.write(1); break; case CSR_IOCTL_CMD_RESET_OFF: CSR_LOG_INFO("reset OFF.\r\n"); pinReset.write(0); break; default: CSR_LOG_INFO("Unknown ioctl command 0x%lx\r\n", command); ret = 0xFFFFFFFF; break; } return ret; } void CSRLocation::lpmGetImmediateLocation(void) { if ((locState == CSR_LOC_STATE_RUN) && (pwrMode == PWR_PTF) && (engStatus == ENGINE_STATUS_NOTOK2SEND)) { CSR_LOG_INFO("LpmGetPos "); _CsrLocHwOnoff(); } else { if (locState != CSR_LOC_STATE_RUN) { CSR_LOG_INFO("Not in run state.\r\n"); } else if (pwrMode != PWR_PTF) { CSR_LOG_INFO("Not in low power PTF mode.\r\n"); } } } void CSRLocation::CsrLocDebugSwitch2Nmea(void) { if (locState == CSR_LOC_STATE_RUN) { _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ); wait_ms(200); _CsrLocHwReset(); } } void CSRLocation::_CsrLocUartInit(void) { in = 0; out = 0; memset(serialBuf, 0, MAX_SERIAL_BUF_LEN); memset(serialPkt, 0, MAX_SERIAL_PKT_LEN); checksum = 0; msgSize = 0; decodeIndex = 0; protoDetState = STATE_START1; pTimeoutChk->attach(this, &CSRLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT); serialLoc.baud(baudRate); serialLoc.attach(this, &CSRLocation::_CsrLocRxHandler ); } void CSRLocation::_CsrLocProcessRawStream(void) { uint8_t data; if (bTimeoutFlag) { pTimeoutChk->detach(); bTimeoutFlag = false; if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) { _CsrLocDetProtoOspTimeout(); } else if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) { _CsrLocDetProtoNmeaTimeout(); } else { CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", protoState); } return; } while (in != out) { if (bTimeoutFlag) { pTimeoutChk->detach(); bTimeoutFlag = false; if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) { _CsrLocDetProtoOspTimeout(); } else if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) { _CsrLocDetProtoNmeaTimeout(); } else { CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", protoState); } return; } data = serialBuf[out++]; out &= (MAX_SERIAL_BUF_LEN - 1); switch (protoState) { case PROTO_STATE_DET_OSP: case PROTO_STATE_DET_OSP_FROM_NMEA: _CsrLocProcessRawOspStream(data); break; case PROTO_STATE_DET_NMEA: case PROTO_STATE_DET_NMEA_FROM_OSP: _CsrLocProcessRawNmeaStream(data); break; case PROTO_STATE_DET_OK: if (proto == PROTO_NMEA) { _CsrLocProcessRawNmeaStream(data); } else { _CsrLocProcessRawOspStream(data); } break; default: /* Discard received data */ //out = pLocInst->in; break; } } } void CSRLocation::_CsrLocDetProtoOspTimeout(void) { if (proto == PROTO_NMEA) { /* Failed to detect OSP */ serialLoc.attach(NULL); protoState = PROTO_STATE_DET_INVALID; baudRate = BAUDRATE_NMEA; CSR_LOG_INFO("Checking OSP failed.\r\n"); eventHandler(CSR_LOC_EVENT_START_RESULT, 1); } else { /* Failed to detect OSP and try to detect NMEA */ serialLoc.attach(NULL); if (protoState == PROTO_STATE_DET_OSP) { protoState = PROTO_STATE_DET_NMEA; baudRate = BAUDRATE_NMEA; CSR_LOG_INFO("Checking OSP protocol failed, now check NMEA protocol...\r\n"); _CsrLocUartInit(); } else { protoState = PROTO_STATE_DET_INVALID; CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n"); eventHandler(CSR_LOC_EVENT_START_RESULT, 1); } } } void CSRLocation::_CsrLocDetProtoNmeaTimeout(void) { CSR_LOG_INFO("Checking NMEA protocol failed\r\n"); if (proto == PROTO_NMEA) { /* Failed to detect NMEA and try to detect OSP */ serialLoc.attach(NULL); if (protoState == PROTO_STATE_DET_NMEA) { protoState = PROTO_STATE_DET_OSP; baudRate = BAUDRATE_OSP; CSR_LOG_INFO("Checking NMEA protocol failed, now check OSP protocol...\r\n"); _CsrLocUartInit(); } else { protoState = PROTO_STATE_DET_INVALID; CSR_LOG_INFO("Checking switched NMEA protocol failed.\r\n"); // eventHandler(CSR_LOC_EVENT_START_RESULT, 1); } } else { /* Failed to detect NEMA */ serialLoc.attach(NULL); protoState = PROTO_STATE_DET_INVALID; baudRate = BAUDRATE_OSP; CSR_LOG_INFO("Checking NMEA failed.\r\n"); eventHandler(CSR_LOC_EVENT_START_RESULT, 1); } } void CSRLocation::_CsrLocProcessRawNmeaStream(uint8_t data) { switch (protoDetState) { case STATE_START1: if (NMEA_MSG_HEAD0 == data) { protoDetState = STATE_START2; } break; case STATE_START2: if (NMEA_MSG_HEAD1 == data) { protoDetState = STATE_END1; decodeIndex = 0; serialPkt[decodeIndex++] = data; } else if (NMEA_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { protoDetState = STATE_START1; } break; case STATE_END1: if (NMEA_MSG_TAIL0 == data) { pTimeoutChk->detach(); bTimeoutFlag = false; if (proto == PROTO_NMEA) { protoDetState = STATE_START1; if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) { CSR_LOG_INFO("Checking NMEA protocol OK.\r\n"); protoState = PROTO_STATE_DET_OK; eventHandler(CSR_LOC_EVENT_START_RESULT, 0); } serialPkt[decodeIndex++] = '\0'; _CsrLocProcessRawNmeaPkt(); } else { serialLoc.attach(NULL); CSR_LOG_INFO("Checking NMEA protocol OK, switching to OSP...\r\n"); _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ); wait_ms(100); protoState = PROTO_STATE_DET_OSP_FROM_NMEA; baudRate = BAUDRATE_OSP; CSR_LOG_INFO("Checking switched OSP protocol...\r\n"); _CsrLocUartInit(); } } else if (NMEA_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { if (decodeIndex < (MAX_SERIAL_PKT_LEN - 2)) { serialPkt[decodeIndex++] = data; } else { protoDetState = STATE_START1; } } break; default: break; } } void CSRLocation::_CsrLocProcessRawOspStream(uint8_t data) { switch (protoDetState) { case STATE_START1: if (OSP_MSG_HEAD0 == data) { protoDetState = STATE_START2; } break; case STATE_START2: if (OSP_MSG_HEAD1 == data) { protoDetState = STATE_SIZE1; } else if (OSP_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { protoDetState = STATE_START1; } break; case STATE_SIZE1: msgSize = data; msgSize <<= 8; /* high uint8_t */ protoDetState = STATE_SIZE2; break; case STATE_SIZE2: msgSize += data; if ((MAX_SERIAL_PKT_LEN < msgSize) || (0 == msgSize)) { if (OSP_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { protoDetState = STATE_START1; } } else { computedCheckSum = 0; decodeIndex = 0; protoDetState = STATE_PAYLOAD; } break; case STATE_PAYLOAD: /* check for a catastrophic error case */ if (MAX_SERIAL_PKT_LEN <= decodeIndex) { /* This is really bad. And should never happen since we * gurantee that msgSize is always less than RECEIVE_PACKET_SIZE * Change the state back to STATE_START1 and start over */ if (OSP_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { protoDetState = STATE_START1; } break; } /* Store the byte */ serialPkt[decodeIndex++] = data; computedCheckSum += data; /* Check to see if we've read the full payload */ if (0 == (--msgSize)) { computedCheckSum &= 0x7FFF; protoDetState = STATE_CHECKSUM1; } break; case STATE_CHECKSUM1: checksum = data; checksum <<= 8; protoDetState = STATE_CHECKSUM2; break; case STATE_CHECKSUM2: checksum += data; if (computedCheckSum != checksum) { if (OSP_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { protoDetState = STATE_START1; } } else { protoDetState = STATE_END1; } break; case STATE_END1: if (OSP_MSG_TAIL0 == data) { protoDetState = STATE_END2; } else { if (OSP_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { protoDetState = STATE_START1; } } break; case STATE_END2: if (OSP_MSG_TAIL1 == data) { pTimeoutChk->detach(); bTimeoutFlag = false; protoDetState = STATE_START1; if (proto == PROTO_NMEA) { serialLoc.attach(NULL); CSR_LOG_INFO("Checking OSP protocol OK, switching to NMEA...\r\n"); _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ); wait_ms(100); protoState = PROTO_STATE_DET_NMEA_FROM_OSP; baudRate = BAUDRATE_NMEA; CSR_LOG_INFO("Checking switched NMEA protocol...\r\n"); _CsrLocUartInit(); } else { if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) { CSR_LOG_INFO("Checking OSP protocol OK.\r\n"); protoState = PROTO_STATE_DET_OK; eventHandler(CSR_LOC_EVENT_START_RESULT, 0); } _CsrLocProcessRawOspPkt(); } } else { if (OSP_MSG_HEAD0 == data) { protoDetState = STATE_START2; } else { protoDetState = STATE_START1; } } break; } /* switch. */ } void CSRLocation::_CsrLocProcessRawNmeaPkt(void) { /* report NMEA */ outputHandler(LOC_OUTPUT_NMEA, serialPkt, strlen((char *)serialPkt)); #if 0 tLocPosResp pos; const char *pNmeaGga = "GPGGA"; float deg, min; char ns, ew; int svUsed; float horDop; int valid; int i, cnt; if (strncmp((char *)serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) { cnt = 0; for (i = 0; i < (int)strlen((char *)serialPkt); i++) { if (serialPkt[i] == ',') { cnt++; if (cnt == 6) { break; } } } if (cnt != 6) { return; } i++; sscanf((char *)(&serialPkt[i]), "%d,", &valid); if (valid == 0) { return; } /* Parse GPGGA and output position information */ memset(&pos, 0, sizeof(tLocPosResp)); if (sscanf((char *)serialPkt, "GPGGA,%f,%lf,%c,%lf,%c,%d,%d,%f,%lf", &pos.utcTime, &pos.lat, &ns, &pos.lon, &ew, &valid, &svUsed, &horDop, &pos.alt) >= 1) { if (ns == 'S') { pos.lat *= -1.0; } if (ew == 'W') { pos.lon *= -1.0; } deg = (float)(static_cast<int>(pos.lat * 0.01f)); min = pos.lat - (deg * 100.0f); pos.lat = deg + min / 60.0f; deg = (float)(static_cast<int>(pos.lon * 0.01f)); min = pos.lon - (deg * 100.0f); pos.lon = deg + min / 60.0f; outputHandler(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp)); } } #endif } void CSRLocation::_CsrLocProcessRawOspPkt(void) { tOspMsg *pOspMsg; uint32_t msgSize; CsrResult result; static uint8_t buffer[512]; static unsigned maxMessageSize = 0; msgSize = _CsrLocCalcMsgSize(); if (msgSize > 0) { msgSize += sizeof(tOspMsg); if (msgSize > maxMessageSize) { maxMessageSize = msgSize; CSR_LOG_INFO("max message size %u\r\n", maxMessageSize); } pOspMsg = (tOspMsg *)buffer; memset(pOspMsg, 0, msgSize); } else { /* discard the unprocessed message */ return; } result = _CsrLocDecodeOspPkt(serialPkt, decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length); if (CSR_RESULT_SUCCESS == result) { _CsrLocProcessOspPkt(pOspMsg); } } uint32_t CSRLocation::_CsrLocCalcMsgSize(void) { uint8_t *ptr = serialPkt; uint32_t msgSize = 0; uint32_t msgId; uint8_t mid, sid = 0; mid = BINARY_IMPORT_UINT8(ptr); msgId = OSP_MAKE_MSG_ID(mid, sid); if ((OSP_MSG_PWR_MODE_RSP == msgId) || (OSP_MSG_MULTI_CONSTELLATION == msgId)) { /* add the sub-id to the message id */ sid = BINARY_IMPORT_UINT8(ptr); msgId = OSP_MAKE_MSG_ID(mid, sid); } switch (msgId) { case OSP_MSG_OK_TO_SEND: case OSP_MSG_PWR_MODE_FPM_RSP: case OSP_MSG_PWR_MODE_LPM_RSP: case OSP_MSG_HW_CONFIG_REQ: msgSize = sizeof(uint8_t); break; case OSP_MSG_SW_VERSION: msgSize = MAX_VERSION_LENGTH; break; case OSP_MSG_GEODETIC_NAVIGATION: msgSize = sizeof(tLocPosResp); break; case OSP_MSG_GNSS_SAT_DATA: msgSize = sizeof(uint8_t); break; case OSP_MSG_GNSS_NAV_DATA: msgSize = sizeof(uint8_t); break; default: msgSize = 0; break; } return msgSize; } CsrResult CSRLocation::_CsrLocDecodeOspPkt(uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen) { CsrResult tRet = CSR_RESULT_SUCCESS; uint8_t *ptr = pPayload; uint32_t i; uint8_t mid, sid = 0; mid = BINARY_IMPORT_UINT8(ptr); *pMsgId = OSP_MAKE_MSG_ID(mid, sid); *pMsgLen = 0; /* add the sub-id to the message id */ if ((OSP_MSG_PWR_MODE_RSP == *pMsgId) || (OSP_MSG_MULTI_CONSTELLATION == *pMsgId)) { sid = BINARY_IMPORT_UINT8(ptr); *pMsgId = OSP_MAKE_MSG_ID(mid, sid); } switch (*pMsgId) { case OSP_MSG_SW_VERSION: /* 0x06 */ *pMsgLen = BINARY_IMPORT_UINT8(ptr); ptr++; if (*pMsgLen >= MAX_VERSION_LENGTH) { tRet = CSR_RESULT_FAILURE; } else { memcpy(pMsgData, ptr, *pMsgLen); } break; case OSP_MSG_OK_TO_SEND: /* 0x12 */ *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr); *pMsgLen = sizeof(uint8_t); break; case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */ { tLocPosResp *pPos = (tLocPosResp *) pMsgData; uint16_t valid; valid = BINARY_IMPORT_UINT16(ptr); if (valid != 0) { tRet = CSR_RESULT_FAILURE; } else { *pMsgLen = sizeof(*pPos); ptr += 2; pPos->gpsTime.gps_week = BINARY_IMPORT_UINT16(ptr); pPos->gpsTime.tow = BINARY_IMPORT_UINT32(ptr); pPos->utcTime = CSR_ULOC_UTC_GPS_OFFSET_MS + (uint64_t)pPos->gpsTime.gps_week * (uint64_t)CSR_ULOC_SEC_IN_WEEK_MS + (uint64_t)pPos->gpsTime.tow - (uint64_t)CSR_ULOC_UTC_LEAP_OFFSET_MS; ptr += 12; pPos->lat = (double)BINARY_IMPORT_SINT32(ptr); pPos->lat *= 1e-7; pPos->lon = (double)BINARY_IMPORT_SINT32(ptr); pPos->lon *= 1e-7; ptr += 4; pPos->alt = (double)BINARY_IMPORT_SINT32(ptr); pPos->alt *= 1e-2; } break; } case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */ { tLocSvStatus *pSvStatus = &svStatus; *pMsgLen = sizeof(*pSvStatus); ptr += 100; pSvStatus->svUsedInFixMask = BINARY_IMPORT_UINT32(ptr); pSvStatus->sbasSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); pSvStatus->gloSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); pSvStatus->qzssSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); break; } case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */ { tLocSvStatus *pSvStatus = &svStatus; uint16_t week; uint32_t tow; uint32_t towSubMs; uint8_t info; int32_t nMsg = 0; uint16_t satInfo; uint16_t az; uint16_t el; uint16_t cno; uint8_t gnssType; uint16_t index = 0; *pMsgLen = sizeof(*pSvStatus); week = BINARY_IMPORT_UINT16(ptr); tow = BINARY_IMPORT_UINT32(ptr); towSubMs = BINARY_IMPORT_UINT32(ptr); ptr += 4; info = BINARY_IMPORT_UINT8(ptr); nMsg = info & 0x0F; if (nMsg == 1) { memset(pSvStatus, 0, sizeof(tLocSvStatus)); pSvStatus->gps_week = week; pSvStatus->tow = tow; pSvStatus->tow_sub_ms = towSubMs; } ptr++; for (i = 0; i < GNSS_SAT_DATA_NUM_OF_SATS; i++) { satInfo = BINARY_IMPORT_UINT16(ptr); az = BINARY_IMPORT_UINT16(ptr); el = BINARY_IMPORT_UINT16(ptr); cno = BINARY_IMPORT_UINT16(ptr); ptr += 4; gnssType = (uint8_t)((satInfo >> 13) & 0x0003); if ((0 == gnssType) || (1 == gnssType)) { // GPS, SBAS, QZSS index = pSvStatus->numOfSVs; if ((index < LOC_MAX_GNSS_SVS) && (cno >0)) { pSvStatus->svList[index].prn = (uint8_t)(satInfo & 0xFF); pSvStatus->svList[index].cno = (float)(cno / 10.0); // Scale: 10 pSvStatus->svList[index].elevation = (float)(el / 10.0); // Scale: 10 pSvStatus->svList[index].azimuth = (float)(az / 10.0); // Scale: 10 pSvStatus->numOfSVs++; pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn - 1); // prn range: 1-32 } } else if (2 == gnssType) { // GLONASS index = pSvStatus->numOfGloSVs; if ((index < CODEC_GLO_MAX_CHANNELS) && (cno>0)) { int16_t freqChan = (satInfo & 0X1F00) >> 8; int16_t slotNum = (satInfo & 0X00FF); if (slotNum > 0) { if (freqChan & 0X0010) { freqChan |= 0xFFE0; } pSvStatus->gloSvList[index].prn = (uint8_t)(freqChan + LOC_GLO_FREQ_OFFSET); pSvStatus->gloSvList[index].sno = (uint8_t)slotNum; pSvStatus->gloSvList[index].cno = (float)(cno / 10.0); // Scale: 10 pSvStatus->gloSvList[index].elevation = (float)(el / 10.0); // Scale: 10 pSvStatus->gloSvList[index].azimuth = (float)(az / 10.0); // Scale: 10 pSvStatus->numOfGloSVs++; pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn - LOC_GLO_FREQ_ID_START); } } } } break; } case OSP_MSG_HW_CONFIG_REQ: /* 0x47 */ break; case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */ break; case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */ *((uint8_t *)pMsgData) = *ptr; *pMsgLen = sizeof(uint8_t); break; default: tRet = CSR_RESULT_FAILURE; break; } /* check if length does not match */ if (tRet == CSR_RESULT_FAILURE) { *pMsgId = *pMsgLen = 0; } return tRet; } /* CsrUlocCodecSsbDecode() */ void CSRLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg) { switch (pOspMsg->msgId) { case OSP_MSG_GEODETIC_NAVIGATION: outputHandler(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp)); break; case OSP_MSG_GNSS_SAT_DATA: break; case OSP_MSG_GNSS_NAV_DATA: outputHandler(LOC_OUTPUT_SV_STATUS, &svStatus, sizeof(tLocSvStatus)); break; case OSP_MSG_OK_TO_SEND: engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND; CSR_LOG_INFO("Ok to send %u\r\n", engStatus); break; case OSP_MSG_SW_VERSION: bVerRsp = true; CSR_LOG_INFO("Ver: %s\r\n", pOspMsg->payload); break; case OSP_MSG_HW_CONFIG_REQ: CSR_LOG_INFO("hw config req.\r\n"); if (!bVerRsp) { _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ); } if (!bPwrModeRsp) { if (pwrMode == PWR_PTF) { CSR_LOG_INFO("Send PTF command.\r\n"); _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ); } else { CSR_LOG_INFO("Send FPM command.\r\n"); _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ); } } break; case OSP_MSG_PWR_MODE_LPM_RSP: bPwrModeRsp = true; CSR_LOG_INFO("lpm response.\r\n"); break; case OSP_MSG_PWR_MODE_FPM_RSP: bPwrModeRsp = true; CSR_LOG_INFO("fpm response.\r\n"); break; default: CSR_LOG_INFO("Unknown OSP message 0x%lx.\r\n", pOspMsg->msgId); break; } } void CSRLocation::_CsrLocTimeout(void) { bTimeoutFlag = true; } void CSRLocation::_CsrLocRxHandler(void) { serialBuf[in++] = serialLoc.getc(); in &= (MAX_SERIAL_BUF_LEN - 1); if (in == out) { CSR_LOG_INFO("rx overwritten %lu %lu.\r\n", in, out); } } void CSRLocation::_CsrLocSendData(eSendDataType type) { uint32_t i, size; const uint8_t *pData; switch (type) { case SEND_DATA_TYPE_OSP_STOP_REQ: pData = sOspStopReq; size = sizeof(sOspStopReq); break; case SEND_DATA_TYPE_OSP_VER_REQ: pData = sOspVerReq; size = sizeof(sOspVerReq); break; case SEND_DATA_TYPE_OSP_LPM_REQ: pData = sOspLpmReq; size = sizeof(sOspLpmReq); break; case SEND_DATA_TYPE_OSP_FPM_REQ: pData = sOspFpmReq; size = sizeof(sOspFpmReq); break; case SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ: pData = sOspSwitch2NmeaReq; size = sizeof(sOspSwitch2NmeaReq); break; case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ: pData = (const uint8_t *)sNmeaSwitch2OspReq; size = strlen(sNmeaSwitch2OspReq); break; case SEND_DATA_TYPE_NMEA_STOP_REQ: pData = (const uint8_t *)sNmeaStopReq; size = strlen(sNmeaStopReq); break; default: pData = NULL; } if (pData != NULL) { for (i = 0; i < size; i++) { serialLoc.putc(pData[i]); } } } void CSRLocation::_CsrLocHwOnoff(void) { pinOnoff.write(1); wait_ms(100); pinOnoff.write(0); CSR_LOG_INFO("Onoff pulse given.\r\n"); } void CSRLocation::_CsrLocHwReset(void) { pinReset.write(0); wait_ms(100); pinReset.write(1); CSR_LOG_INFO("Reset pulse given.\r\n"); } bool CSRLocation::_CsrLocIsWakeup(void) { CSR_LOG_INFO("Is wakeup %d.\r\n", wakeup.read()); return wakeup.read() != 0 ? true : false; } bool CSRLocation::setPowerMode(GPSProvider::PowerMode_t _pwrMode) { switch (_pwrMode) { case GPSProvider::POWER_FULL: pwrMode = PWR_FULL; break; case GPSProvider::POWER_LOW: default: pwrMode = PWR_PTF; /* push to fix */ break; } return (true); }