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-05-21
- Revision:
- 3:71690f7bb480
- Parent:
- 2:d4fe184925f2
- Child:
- 4:0d9b711fb646
File content as of revision 3:71690f7bb480:
/* CsrLocation class for mbed Microcontroller * Copyright 2014 CSR plc */ #include "mbed.h" #include "CsrLocation.h" static uint8_t sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3}; static uint8_t sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3}; static uint8_t sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x63, 0xB0, 0xB3}; static uint8_t sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3}; static uint8_t sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x12, 0xC0, 0x01, 0x64, 0xB0, 0xB3}; static char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n"; static char sNmeaStopReq[] = "$PSRF117,16*0B\r\n"; CsrLocation::CsrLocation(tCsrLocConfig *pLocConfig) { memset(&csrLocInst, 0, sizeof(tCsrLocInst)); csrLocInst.pSerialDebug = pLocConfig->pSerialDebug; csrLocInst.pSerialLoc = pLocConfig->pSerialLoc; csrLocInst.pPinOnoff = pLocConfig->pPinOnoff; csrLocInst.pPinReset = pLocConfig->pPinReset; csrLocInst.pTimeoutChk = new Timeout(); csrLocInst.pPinReset->write(1); csrLocInst.pPinOnoff->write(0); } CsrLocation::~CsrLocation(void) { if(csrLocInst.pTimeoutChk != NULL) { delete csrLocInst.pTimeoutChk; csrLocInst.pTimeoutChk = NULL; } csrLocInst.pSerialLoc->attach(NULL); csrLocInst.pPinReset->write(0); csrLocInst.pPinOnoff->write(0); memset(&csrLocInst, 0, sizeof(tCsrLocInst)); } void CsrLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb) { csrLocInst.appOutCb = app_output_cb; csrLocInst.appEventCb = app_event_cb; } void CsrLocation::CsrLocReset(void) { _CsrLocHwReset(); } void CsrLocation::CsrLocStart(ePowerMode pwrMode, eProto proto) { csrLocInst.pwrMode = pwrMode; if(csrLocInst.locState == CSR_LOC_STATE_IDLE) { /* Csr Location SDK version */ CSR_LOG_INFO("==== CSR LOC SDK version: %s Date: %s ====\r\n", CSR_LOC_SDK_VER, __DATE__ " " __TIME__); csrLocInst.proto = proto; /* open UART */ if(proto == PROTO_NMEA) { CSR_LOG_INFO("Checking NMEA protocol...\r\n"); csrLocInst.protoState = PROTO_STATE_DET_NMEA; csrLocInst.baudRate = BAUDRATE_NMEA; } else { CSR_LOG_INFO("Checking OSP protocol...\r\n"); csrLocInst.protoState = PROTO_STATE_DET_OSP; csrLocInst.baudRate = BAUDRATE_OSP; } _CsrLocUartInit(); /* trigger on_off */ _CsrLocHwOnoff(); csrLocInst.locState = CSR_LOC_STATE_RUN; csrLocInst.bPwrModeRsp = false; csrLocInst.bVerRsp = false; } else { CSR_LOG_INFO("Already started.\r\n"); } } void CsrLocation::CsrLocUpdate(void) { if(csrLocInst.locState == CSR_LOC_STATE_RUN) { /* wait and process uart data */ _CsrLocProcessRawStream(); } } void CsrLocation::CsrLocStop(void) { csrLocInst.pSerialLoc->attach(NULL); CSR_LOG_INFO("Stop command processed.\r\n"); if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND) { /* in sleep mode, trigger on_off firstly */ _CsrLocHwOnoff(); wait_ms(500); } if(csrLocInst.proto == PROTO_NMEA) { _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ); } else { _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ); } wait_ms(10); _CsrLocHwReset(); csrLocInst.locState = CSR_LOC_STATE_IDLE; csrLocInst.appEventCb(CSR_LOC_EVENT_STOP_RESULT, 0); } void CsrLocation::CsrLocLpmGetPos(void) { if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND) { CSR_LOG_INFO("LpmGetPos "); _CsrLocHwOnoff(); } else { if(csrLocInst.locState != CSR_LOC_STATE_RUN) { CSR_LOG_INFO("Not in run state.\r\n"); } else if(csrLocInst.pwrMode != PWR_PTF) { CSR_LOG_INFO("Not in low power PTF mode.\r\n"); } } } void CsrLocation::CsrLocDebugSwitch2Nmea(void) { if(csrLocInst.locState == CSR_LOC_STATE_RUN) { _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ); wait_ms(200); _CsrLocHwReset(); } } void CsrLocation::_CsrLocUartInit(void) { tCsrLocInst *pLocInst = &csrLocInst; pLocInst->in = 0; pLocInst->out = 0; memset(pLocInst->serialBuf, 0 , MAX_SERIAL_BUF_LEN); memset(pLocInst->serialPkt, 0 , MAX_SERIAL_PKT_LEN); pLocInst->checksum = 0; pLocInst->msgSize = 0; pLocInst->decodeIndex = 0; pLocInst->protoDetState = STATE_START1; pLocInst->pTimeoutChk->attach(this, &CsrLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT); pLocInst->pSerialLoc->baud(pLocInst->baudRate); pLocInst->pSerialLoc->attach(this, &CsrLocation::_CsrLocRxHandler ); } void CsrLocation::_CsrLocProcessRawStream(void) { tCsrLocInst *pLocInst = &csrLocInst; uint8_t data; if(pLocInst->bTimeoutFlag) { pLocInst->pTimeoutChk->detach(); pLocInst->bTimeoutFlag = false; if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA) { _CsrLocDetProtoOspTimeout(); } else if(pLocInst->protoState == PROTO_STATE_DET_NMEA || pLocInst->protoState == PROTO_STATE_DET_NMEA_FROM_OSP) { _CsrLocDetProtoNmeaTimeout(); } else { CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", pLocInst->protoState); } return; } if(pLocInst->in != pLocInst->out) { data = pLocInst->serialBuf[pLocInst->out++]; pLocInst->out &= (MAX_SERIAL_BUF_LEN-1); switch(pLocInst->protoState) { case PROTO_STATE_DET_OSP: case PROTO_STATE_DET_OSP_FROM_NMEA: _CsrLocProcessRawOspStream(data); break; case PROTO_STATE_DET_NMEA: case PROTO_STATE_DET_NMEA_FROM_OSP: _CsrLocProcessRawNmeaStream(data); break; case PROTO_STATE_DET_OK: if(pLocInst->proto == PROTO_NMEA) { _CsrLocProcessRawNmeaStream(data); } else { _CsrLocProcessRawOspStream(data); } break; default: /* Discard received data */ //pLocInst->out = pLocInst->in; break; } } } void CsrLocation::_CsrLocDetProtoOspTimeout(void) { tCsrLocInst *pLocInst = &csrLocInst; if(pLocInst->proto == PROTO_NMEA) { /* Failed to detect OSP */ pLocInst->pSerialLoc->attach(NULL); pLocInst->protoState = PROTO_STATE_DET_INVALID; pLocInst->baudRate = BAUDRATE_NMEA; CSR_LOG_INFO("Checking OSP failed.\r\n"); pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); } else { /* Failed to detect OSP and try to detect NMEA */ pLocInst->pSerialLoc->attach(NULL); if(pLocInst->protoState == PROTO_STATE_DET_OSP) { pLocInst->protoState = PROTO_STATE_DET_NMEA; pLocInst->baudRate = BAUDRATE_NMEA; CSR_LOG_INFO("Checking OSP protocol failed, now check NMEA protocol...\r\n"); _CsrLocUartInit(); } else { pLocInst->protoState = PROTO_STATE_DET_INVALID; CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n"); pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); } } } void CsrLocation::_CsrLocDetProtoNmeaTimeout(void) { tCsrLocInst *pLocInst = &csrLocInst; CSR_LOG_INFO("Checking NMEA protocol failed\r\n"); if(pLocInst->proto == PROTO_NMEA) { /* Failed to detect NMEA and try to detect OSP */ pLocInst->pSerialLoc->attach(NULL); if(pLocInst->protoState == PROTO_STATE_DET_NMEA) { pLocInst->protoState = PROTO_STATE_DET_OSP; pLocInst->baudRate = BAUDRATE_OSP; CSR_LOG_INFO("Checking NMEA protocol failed, now check OSP protocol...\r\n"); _CsrLocUartInit(); } else { pLocInst->protoState = PROTO_STATE_DET_INVALID; CSR_LOG_INFO("Checking switched NMEA protocol failed.\r\n"); pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); } } else { /* Failed to detect NEMA */ pLocInst->pSerialLoc->attach(NULL); pLocInst->protoState = PROTO_STATE_DET_INVALID; pLocInst->baudRate = BAUDRATE_OSP; CSR_LOG_INFO("Checking NMEA failed.\r\n"); pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); } } void CsrLocation::_CsrLocProcessRawNmeaStream(uint8_t data) { tCsrLocInst *pLocInst = &csrLocInst; switch (pLocInst->protoDetState) { case STATE_START1: if (NMEA_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } break; case STATE_START2: if (NMEA_MSG_HEAD1 == data) { pLocInst->protoDetState = STATE_END1; pLocInst->decodeIndex = 0; pLocInst->serialPkt[pLocInst->decodeIndex++] = data; } else if (NMEA_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { pLocInst->protoDetState = STATE_START1; } break; case STATE_END1: if (NMEA_MSG_TAIL0 == data) { pLocInst->pTimeoutChk->detach(); pLocInst->bTimeoutFlag = false; if(pLocInst->proto == PROTO_NMEA) { pLocInst->protoDetState = STATE_START1; if(pLocInst->protoState == PROTO_STATE_DET_NMEA || pLocInst->protoState == PROTO_STATE_DET_NMEA_FROM_OSP) { CSR_LOG_INFO("Checking NMEA protocol OK.\r\n"); pLocInst->protoState = PROTO_STATE_DET_OK; pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0); } pLocInst->serialPkt[pLocInst->decodeIndex++] = '\0'; _CsrLocProcessRawNmeaPkt(); } else { pLocInst->pSerialLoc->attach(NULL); CSR_LOG_INFO("Checking NMEA protocol OK, switching to OSP...\r\n"); _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ); wait_ms(100); pLocInst->protoState = PROTO_STATE_DET_OSP_FROM_NMEA; pLocInst->baudRate = BAUDRATE_OSP; CSR_LOG_INFO("Checking switched OSP protocol...\r\n"); _CsrLocUartInit(); } } else if (NMEA_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { if(pLocInst->decodeIndex < (MAX_SERIAL_PKT_LEN - 2)) { pLocInst->serialPkt[pLocInst->decodeIndex++] = data; } else { pLocInst->protoDetState = STATE_START1; } } break; default: break; } } void CsrLocation::_CsrLocProcessRawOspStream(uint8_t data) { tCsrLocInst *pLocInst = &csrLocInst; switch (pLocInst->protoDetState) { case STATE_START1: if (OSP_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } break; case STATE_START2: if (OSP_MSG_HEAD1 == data) { pLocInst->protoDetState = STATE_SIZE1; } else if (OSP_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { pLocInst->protoDetState = STATE_START1; } break; case STATE_SIZE1: pLocInst->msgSize = data ; pLocInst->msgSize <<= 8; /* high uint8_t */ pLocInst->protoDetState = STATE_SIZE2; break; case STATE_SIZE2: pLocInst->msgSize += data ; if (MAX_SERIAL_PKT_LEN < pLocInst->msgSize || 0 == pLocInst->msgSize) { if (OSP_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { pLocInst->protoDetState = STATE_START1; } } else { pLocInst->computedCheckSum = 0; pLocInst->decodeIndex = 0; pLocInst->protoDetState = STATE_PAYLOAD; } break; case STATE_PAYLOAD: /* check for a catastrophic error case */ if (MAX_SERIAL_PKT_LEN <= pLocInst->decodeIndex) { /* This is really bad. And should never happen since we * gurantee that msgSize is always less than RECEIVE_PACKET_SIZE * Change the state back to STATE_START1 and start over */ if (OSP_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { pLocInst->protoDetState = STATE_START1; } break; } /* Store the byte */ pLocInst->serialPkt[pLocInst->decodeIndex++] = data; pLocInst->computedCheckSum += data; /* Check to see if we've read the full payload */ if (0 == (--pLocInst->msgSize)) { pLocInst->computedCheckSum &= 0x7FFF; pLocInst->protoDetState = STATE_CHECKSUM1; } break; case STATE_CHECKSUM1: pLocInst->checksum = data ; pLocInst->checksum <<= 8; pLocInst->protoDetState = STATE_CHECKSUM2; break; case STATE_CHECKSUM2: pLocInst->checksum += data; if (pLocInst->computedCheckSum != pLocInst->checksum) { if (OSP_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { pLocInst->protoDetState = STATE_START1; } } else { pLocInst->protoDetState = STATE_END1; } break; case STATE_END1: if (OSP_MSG_TAIL0 == data) { pLocInst->protoDetState = STATE_END2; } else { if (OSP_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { pLocInst->protoDetState = STATE_START1; } } break; case STATE_END2: if (OSP_MSG_TAIL1 == data) { pLocInst->pTimeoutChk->detach(); pLocInst->bTimeoutFlag = false; pLocInst->protoDetState = STATE_START1; if(pLocInst->proto == PROTO_NMEA) { pLocInst->pSerialLoc->attach(NULL); CSR_LOG_INFO("Checking OSP protocol OK, switching to NMEA...\r\n"); _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ); wait_ms(100); pLocInst->protoState = PROTO_STATE_DET_NMEA_FROM_OSP; pLocInst->baudRate = BAUDRATE_NMEA; CSR_LOG_INFO("Checking switched NMEA protocol...\r\n"); _CsrLocUartInit(); } else { if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA) { CSR_LOG_INFO("Checking OSP protocol OK.\r\n"); pLocInst->protoState = PROTO_STATE_DET_OK; pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0); } _CsrLocProcessRawOspPkt(); } } else { if (OSP_MSG_HEAD0 == data) { pLocInst->protoDetState = STATE_START2; } else { pLocInst->protoDetState = STATE_START1; } } break; } /* switch. */ } void CsrLocation::_CsrLocProcessRawNmeaPkt(void) { tCsrLocInst *pLocInst = &csrLocInst; tLocPosResp pos; const char *pNmeaGga = "GPGGA"; float deg, min; char ns, ew; int svUsed; float horDop; int valid; int i, cnt; if(strncmp((char *)pLocInst->serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) { cnt = 0; for(i = 0; i < (int)strlen((char *)pLocInst->serialPkt); i++) { if(pLocInst->serialPkt[i] == ',') { cnt++; if(cnt == 6) { break; } } } if(cnt != 6) { return; } i++; sscanf((char *)(&pLocInst->serialPkt[i]), "%d,", &valid); if(valid == 0) { return; } /* Parse GPGGA and output position information */ memset(&pos, 0, sizeof(tLocPosResp)); if(sscanf((char *)pLocInst->serialPkt, "GPGGA,%f,%lf,%c,%lf,%c,%d,%d,%f,%lf", &pos.u.utcTime, &pos.lat, &ns, &pos.lon, &ew, &valid, &svUsed, &horDop, &pos.alt) >= 1) { if(ns == 'S') { pos.lat *= -1.0; } if(ew == 'W') { pos.lon *= -1.0; } deg = (float)(static_cast<int>(pos.lat * 0.01f)); min = pos.lat - (deg * 100.0f); pos.lat = deg + min / 60.0f; deg = (float)(static_cast<int>(pos.lon * 0.01f)); min = pos.lon - (deg * 100.0f); pos.lon = deg + min / 60.0f; csrLocInst.appOutCb(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp)); } } } void CsrLocation::_CsrLocProcessRawOspPkt(void) { tCsrLocInst *pLocInst = &csrLocInst; tOspMsg *pOspMsg; uint32_t msgSize; CsrResult result; msgSize = _CsrLocCalcMsgSize(); if(msgSize > 0) { msgSize += sizeof(tOspMsg); pOspMsg = (tOspMsg *)malloc(msgSize); if(pOspMsg != NULL) { memset(pOspMsg, 0, msgSize); } else { CSR_LOG_INFO("No memory for received OSP message.\r\n"); return; } } else { /* discard the unprocessed message */ return; } result = _CsrLocDecodeOspPkt(pLocInst->serialPkt,pLocInst->decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length); if ( CSR_RESULT_SUCCESS == result) { _CsrLocProcessOspPkt(pOspMsg); } free(pOspMsg); } uint32_t CsrLocation::_CsrLocCalcMsgSize(void) { tCsrLocInst *pLocInst = &csrLocInst; uint8_t *ptr = pLocInst->serialPkt; uint32_t msgSize = 0; uint32_t msgId; uint8_t mid, sid = 0; mid = BINARY_IMPORT_UINT8(ptr); msgId = OSP_MAKE_MSG_ID(mid,sid); if(OSP_MSG_PWR_MODE_RSP == msgId || OSP_MSG_MULTI_CONSTELLATION == msgId) { /* add the sub-id to the message id */ sid = BINARY_IMPORT_UINT8(ptr); msgId = OSP_MAKE_MSG_ID(mid,sid); } switch(msgId) { case OSP_MSG_OK_TO_SEND: case OSP_MSG_PWR_MODE_FPM_RSP: case OSP_MSG_PWR_MODE_LPM_RSP: case OSP_MSG_HW_CONFIG_REQ : msgSize = sizeof(uint8_t); break; case OSP_MSG_SW_VERSION : msgSize = MAX_VERSION_LENGTH; break; case OSP_MSG_GEODETIC_NAVIGATION: msgSize = sizeof(tLocPosResp); break; case OSP_MSG_GNSS_SAT_DATA: msgSize = sizeof(uint8_t); break; case OSP_MSG_GNSS_NAV_DATA: msgSize = sizeof(uint8_t); break; default : msgSize = 0; break; } return msgSize; } CsrResult CsrLocation::_CsrLocDecodeOspPkt( uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen) { CsrResult tRet = CSR_RESULT_SUCCESS; uint8_t *ptr = pPayload; uint32_t i; uint8_t mid, sid = 0; mid = BINARY_IMPORT_UINT8(ptr); *pMsgId = OSP_MAKE_MSG_ID(mid,sid); *pMsgLen = 0; /* add the sub-id to the message id */ if (OSP_MSG_PWR_MODE_RSP == *pMsgId || OSP_MSG_MULTI_CONSTELLATION == *pMsgId) { sid = BINARY_IMPORT_UINT8(ptr); *pMsgId = OSP_MAKE_MSG_ID(mid,sid); } switch (*pMsgId) { case OSP_MSG_SW_VERSION: /* 0x06 */ *pMsgLen = BINARY_IMPORT_UINT8(ptr); ptr++; if(*pMsgLen >= MAX_VERSION_LENGTH) { tRet = CSR_RESULT_FAILURE; } else { memcpy(pMsgData, ptr, *pMsgLen); } break; case OSP_MSG_OK_TO_SEND: /* 0x12 */ *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr); *pMsgLen = sizeof(uint8_t); break; case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */ { tLocPosResp *pPos = (tLocPosResp*) pMsgData; uint16_t valid; valid = BINARY_IMPORT_UINT16(ptr); if(valid != 0) { tRet = CSR_RESULT_FAILURE; } else { *pMsgLen = sizeof(*pPos); ptr += 2; pPos->u.gpsTime.gps_week = BINARY_IMPORT_UINT16(ptr); pPos->u.gpsTime.tow = BINARY_IMPORT_UINT32(ptr); ptr += 12; pPos->lat = (double)BINARY_IMPORT_SINT32(ptr); pPos->lat *= 1e-7; pPos->lon = (double)BINARY_IMPORT_SINT32(ptr); pPos->lon *= 1e-7; ptr += 4; pPos->alt = (double)BINARY_IMPORT_SINT32(ptr); pPos->alt *= 1e-2; } break; } case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */ { tLocSvStatus *pSvStatus = &csrLocInst.svStatus; *pMsgLen = sizeof(*pSvStatus); ptr += 100; pSvStatus->svUsedInFixMask = BINARY_IMPORT_UINT32(ptr); pSvStatus->sbasSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); pSvStatus->gloSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); pSvStatus->qzssSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); break; } case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */ { tLocSvStatus *pSvStatus = &csrLocInst.svStatus; uint16_t week; uint32_t tow; uint32_t towSubMs; uint8_t info; int32_t nMsg = 0; uint16_t satInfo; uint16_t az; uint16_t el; uint16_t cno; uint8_t gnssType; uint16_t index = 0; *pMsgLen = sizeof(*pSvStatus); week = BINARY_IMPORT_UINT16(ptr); tow = BINARY_IMPORT_UINT32(ptr); towSubMs = BINARY_IMPORT_UINT32(ptr); ptr += 4; info = BINARY_IMPORT_UINT8(ptr); nMsg = info & 0x0F; if(nMsg == 1) { memset(pSvStatus, 0, sizeof(tLocSvStatus)); pSvStatus->gps_week = week; pSvStatus->tow = tow; pSvStatus->tow_sub_ms = towSubMs; } ptr++; for (i = 0; i < GNSS_SAT_DATA_NUM_OF_SATS; i++) { satInfo = BINARY_IMPORT_UINT16(ptr); az = BINARY_IMPORT_UINT16(ptr); el = BINARY_IMPORT_UINT16(ptr); cno = BINARY_IMPORT_UINT16(ptr); ptr += 4; gnssType = (uint8_t)((satInfo>>13)&0x0003); if(0 == gnssType || 1 == gnssType) // GPS, SBAS, QZSS { index = pSvStatus->numOfSVs; if(index < LOC_MAX_GNSS_SVS && cno >0) { pSvStatus->svList[index].prn = (uint8_t)(satInfo & 0xFF); pSvStatus->svList[index].cno = (float)(cno/10.0); // Scale: 10 pSvStatus->svList[index].elevation = (float)(el/10.0); // Scale: 10 pSvStatus->svList[index].azimuth = (float)(az/10.0); // Scale: 10 pSvStatus->numOfSVs++; pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn-1); // prn range: 1-32 } } else if(2 == gnssType) // GLONASS { index = pSvStatus->numOfGloSVs; if(index < CODEC_GLO_MAX_CHANNELS && cno>0) { int16_t freqChan = (satInfo & 0X1F00)>>8; int16_t slotNum = (satInfo & 0X00FF); if(slotNum > 0) { if(freqChan & 0X0010) { freqChan |= 0xFFE0; } pSvStatus->gloSvList[index].prn = (uint8_t)(freqChan + LOC_GLO_FREQ_OFFSET); pSvStatus->gloSvList[index].sno = (uint8_t)slotNum; pSvStatus->gloSvList[index].cno = (float)(cno/10.0); // Scale: 10 pSvStatus->gloSvList[index].elevation = (float)(el/10.0); // Scale: 10 pSvStatus->gloSvList[index].azimuth = (float)(az/10.0); // Scale: 10 pSvStatus->numOfGloSVs++; pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn-LOC_GLO_FREQ_ID_START); } } } } break; } case OSP_MSG_HW_CONFIG_REQ: /* 0x47 */ break; case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */ break; case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */ *((uint8_t *)pMsgData) = *ptr; *pMsgLen = sizeof(uint8_t); break; default: tRet = CSR_RESULT_FAILURE; break; } /* check if length does not match */ if(tRet == CSR_RESULT_FAILURE) { *pMsgId = *pMsgLen = 0; } return tRet; } /* CsrUlocCodecSsbDecode() */ void CsrLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg) { switch(pOspMsg->msgId) { case OSP_MSG_GEODETIC_NAVIGATION: csrLocInst.appOutCb(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp)); break; case OSP_MSG_GNSS_SAT_DATA: break; case OSP_MSG_GNSS_NAV_DATA: csrLocInst.appOutCb(LOC_OUTPUT_SV_STATUS, &csrLocInst.svStatus, sizeof(tLocSvStatus)); break; case OSP_MSG_OK_TO_SEND: csrLocInst.engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND; CSR_LOG_INFO("Ok to send %u\r\n", csrLocInst.engStatus); break; case OSP_MSG_SW_VERSION: csrLocInst.bVerRsp = true; CSR_LOG_INFO("Ver: %s\r\n", pOspMsg->payload); break; case OSP_MSG_HW_CONFIG_REQ: CSR_LOG_INFO("hw config req.\r\n"); if(!csrLocInst.bVerRsp) { _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ); } if(!csrLocInst.bPwrModeRsp) { if(csrLocInst.pwrMode == PWR_PTF) { _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ); } else { _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ); } } break; case OSP_MSG_PWR_MODE_LPM_RSP: csrLocInst.bPwrModeRsp = true; CSR_LOG_INFO("lpm response.\r\n"); break; case OSP_MSG_PWR_MODE_FPM_RSP: csrLocInst.bPwrModeRsp = true; CSR_LOG_INFO("fpm response.\r\n"); break; default: CSR_LOG_INFO("Unknown OSP message 0x%lx.\r\n", pOspMsg->msgId); break; } } void CsrLocation::_CsrLocTimeout(void) { csrLocInst.bTimeoutFlag = true; } void CsrLocation::_CsrLocRxHandler(void) { tCsrLocInst *pLocInst = &csrLocInst; pLocInst->serialBuf[pLocInst->in++] = pLocInst->pSerialLoc->getc(); pLocInst->in &= (MAX_SERIAL_BUF_LEN-1); if(pLocInst->in == pLocInst->out) { CSR_LOG_INFO("rx overwritten %lu %lu.\r\n", pLocInst->in, pLocInst->out); } } void CsrLocation::_CsrLocSendData(eSendDataType type) { tCsrLocInst *pLocInst = &csrLocInst; uint32_t i, size; const uint8_t *pData; switch(type) { case SEND_DATA_TYPE_OSP_STOP_REQ: pData = sOspStopReq; size = sizeof(sOspStopReq); break; case SEND_DATA_TYPE_OSP_VER_REQ: pData = sOspVerReq; size = sizeof(sOspVerReq); break; case SEND_DATA_TYPE_OSP_LPM_REQ: pData = sOspLpmReq; size = sizeof(sOspLpmReq); break; case SEND_DATA_TYPE_OSP_FPM_REQ: pData = sOspFpmReq; size = sizeof(sOspFpmReq); break; case SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ: pData = sOspSwitch2NmeaReq; size = sizeof(sOspSwitch2NmeaReq); break; case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ: pData = (const uint8_t *)sNmeaSwitch2OspReq; size = strlen(sNmeaSwitch2OspReq); break; case SEND_DATA_TYPE_NMEA_STOP_REQ: pData = (const uint8_t *)sNmeaStopReq; size = strlen(sNmeaStopReq); break; default: pData = NULL; } if(pData != NULL) { for (i = 0; i < size; i++) { pLocInst->pSerialLoc->putc(pData[i]); } } } void CsrLocation::_CsrLocHwOnoff(void) { csrLocInst.pPinOnoff->write(1); wait_ms(10); csrLocInst.pPinOnoff->write(0); CSR_LOG_INFO("Onoff pulse given.\r\n"); } void CsrLocation::_CsrLocHwReset(void) { csrLocInst.pPinReset->write(1); wait_ms(10); csrLocInst.pPinReset->write(0); CSR_LOG_INFO("Reset pulse given.\r\n"); }