Csr location class shows location and satellite information, which supports H13467 + ST F103RB/NXP LCP1549 boards now.
Dependents: CsrLocationDemo CsrLocationDemo
Fork of CsrLocation by
Diff: CsrLocation.cpp
- Revision:
- 2:d4fe184925f2
- Parent:
- 1:bbaf9b8d646a
- Child:
- 3:71690f7bb480
diff -r bbaf9b8d646a -r d4fe184925f2 CsrLocation.cpp --- a/CsrLocation.cpp Tue Mar 25 05:26:26 2014 +0000 +++ b/CsrLocation.cpp Wed Mar 26 09:16:55 2014 +0000 @@ -8,15 +8,15 @@ #include "CsrLocation.h" -static CsrUint8 sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3}; -static CsrUint8 sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3}; -static CsrUint8 sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E, +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 CsrUint8 sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3}; -static CsrUint8 sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00, +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 CsrCharString sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n"; +static char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n"; CsrLocation::CsrLocation(tCsrLocConfig *pLocConfig) { @@ -74,8 +74,8 @@ _CsrLocHwOnoff(); csrLocInst.locState = CSR_LOC_STATE_RUN; - csrLocInst.bPwrModeRsp = FALSE; - csrLocInst.bVerRsp = FALSE; + csrLocInst.bPwrModeRsp = false; + csrLocInst.bVerRsp = false; } else @@ -118,7 +118,18 @@ { 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) @@ -153,7 +164,7 @@ void CsrLocation::_CsrLocProcessRawStream(void) { tCsrLocInst *pLocInst = &csrLocInst; - CsrUint8 data; + uint8_t data; if(pLocInst->in != pLocInst->out) { @@ -162,14 +173,12 @@ switch(pLocInst->protoState) { case PROTO_STATE_DET_OSP: + case PROTO_STATE_DET_OSP_FROM_NMEA: _CsrLocDetProtoOsp(data); break; case PROTO_STATE_DET_NMEA: _CsrLocDetProtoNmea(data); break; - case PROTO_STATE_DET_OSP_FROM_NMEA: - _CsrLocDetProtoOsp(data); - break; case PROTO_STATE_DET_OK: _CsrLocProcessRawOspStream(data); break; @@ -181,7 +190,7 @@ } } -void CsrLocation::_CsrLocDetProtoOsp(CsrUint8 data) +void CsrLocation::_CsrLocDetProtoOsp(uint8_t data) { tCsrLocInst *pLocInst = &csrLocInst; @@ -190,7 +199,7 @@ /* Failed to detect OSP and try to detect NMEA */ pLocInst->pSerialLoc->attach(NULL); pLocInst->pTimeoutChk->detach(); - pLocInst->bTimeoutFlag = FALSE; + pLocInst->bTimeoutFlag = false; if(pLocInst->protoState == PROTO_STATE_DET_OSP) { pLocInst->protoState = PROTO_STATE_DET_NMEA; @@ -210,7 +219,7 @@ _CsrLocProcessRawOspStream(data); } -void CsrLocation::_CsrLocDetProtoNmea(CsrUint8 data) +void CsrLocation::_CsrLocDetProtoNmea(uint8_t data) { tCsrLocInst *pLocInst = &csrLocInst; @@ -219,7 +228,7 @@ /* Failed to detect OSP and try to detect NMEA */ pLocInst->pSerialLoc->attach(NULL); pLocInst->pTimeoutChk->detach(); - pLocInst->bTimeoutFlag = FALSE; + pLocInst->bTimeoutFlag = false; pLocInst->protoState = PROTO_STATE_DET_INVALID; pLocInst->baudRate = BAUDRATE_OSP; CSR_LOG_INFO("Checking NMEA failed.\r\n"); @@ -255,7 +264,7 @@ { pLocInst->pSerialLoc->attach(NULL); pLocInst->pTimeoutChk->detach(); - pLocInst->bTimeoutFlag = FALSE; + pLocInst->bTimeoutFlag = false; pLocInst->protoState = PROTO_STATE_SWI_OSP_FROM_NMEA; pLocInst->protoDetState = STATE_START1; CSR_LOG_INFO("Checking OSP protocol OK, switching to OSP...\r\n"); @@ -277,7 +286,7 @@ } } -void CsrLocation::_CsrLocProcessRawOspStream(CsrUint8 data) +void CsrLocation::_CsrLocProcessRawOspStream(uint8_t data) { tCsrLocInst *pLocInst = &csrLocInst; @@ -307,7 +316,7 @@ case STATE_SIZE1: pLocInst->msgSize = data ; - pLocInst->msgSize <<= 8; /* high CsrUint8 */ + pLocInst->msgSize <<= 8; /* high uint8_t */ pLocInst->protoDetState = STATE_SIZE2; break; @@ -408,7 +417,7 @@ if (OSP_MSG_TAIL1 == data) { pLocInst->pTimeoutChk->detach(); - pLocInst->bTimeoutFlag = FALSE; + pLocInst->bTimeoutFlag = false; if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA) { @@ -417,7 +426,6 @@ pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0); } -// pLocInst->bRawOspPktReady = TRUE; _CsrLocProcessRawOspPkt(); pLocInst->protoDetState = STATE_START1; } @@ -441,15 +449,9 @@ { tCsrLocInst *pLocInst = &csrLocInst; tOspMsg *pOspMsg; - CsrUint32 msgSize; + uint32_t msgSize; CsrResult result; -// if(!pLocInst->bRawOspPktReady) -// { -// return; -// } - -// pLocInst->bRawOspPktReady = FALSE; msgSize = _CsrLocCalcMsgSize(); if(msgSize > 0) { @@ -480,13 +482,13 @@ free(pOspMsg); } -CsrUint32 CsrLocation::_CsrLocCalcMsgSize(void) +uint32_t CsrLocation::_CsrLocCalcMsgSize(void) { tCsrLocInst *pLocInst = &csrLocInst; - CsrUint8 *ptr = pLocInst->serialPkt; - CsrUint32 msgSize = 0; - CsrUint32 msgId; - CsrUint8 mid, sid = 0; + 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); @@ -504,7 +506,7 @@ case OSP_MSG_PWR_MODE_FPM_RSP: case OSP_MSG_PWR_MODE_LPM_RSP: case OSP_MSG_HW_CONFIG_REQ : - msgSize = sizeof(CsrUint8); + msgSize = sizeof(uint8_t); break; case OSP_MSG_SW_VERSION : msgSize = MAX_VERSION_LENGTH; @@ -513,10 +515,10 @@ msgSize = sizeof(tLocPosResp); break; case OSP_MSG_GNSS_SAT_DATA: - msgSize = sizeof(CsrUint8); + msgSize = sizeof(uint8_t); break; case OSP_MSG_GNSS_NAV_DATA: - msgSize = sizeof(CsrUint8); + msgSize = sizeof(uint8_t); break; default : @@ -527,12 +529,12 @@ return msgSize; } -CsrResult CsrLocation::_CsrLocDecodeOspPkt( CsrUint8 *pPayload, CsrUint32 payloadLen, CsrUint32 *pMsgId, void *pMsgData, CsrUint32 *pMsgLen) +CsrResult CsrLocation::_CsrLocDecodeOspPkt( uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen) { CsrResult tRet = CSR_RESULT_SUCCESS; - CsrUint8 *ptr = pPayload; - CsrUint32 i; - CsrUint8 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); @@ -561,14 +563,14 @@ break; case OSP_MSG_OK_TO_SEND: /* 0x12 */ - *((CsrUint8 *)pMsgData) = BINARY_IMPORT_UINT8(ptr); - *pMsgLen = sizeof(CsrUint8); + *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr); + *pMsgLen = sizeof(uint8_t); break; case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */ { tLocPosResp *pPos = (tLocPosResp*) pMsgData; - CsrUint16 valid; + uint16_t valid; valid = BINARY_IMPORT_UINT16(ptr); if(valid != 0) @@ -583,12 +585,12 @@ pPos->gps_week = BINARY_IMPORT_UINT16(ptr); pPos->tow = BINARY_IMPORT_UINT32(ptr); ptr += 12; - pPos->lat = (CsrDouble)BINARY_IMPORT_SINT32(ptr); + pPos->lat = (double)BINARY_IMPORT_SINT32(ptr); pPos->lat *= 1e-7; - pPos->lon = (CsrDouble)BINARY_IMPORT_SINT32(ptr); + pPos->lon = (double)BINARY_IMPORT_SINT32(ptr); pPos->lon *= 1e-7; ptr += 4; - pPos->alt = (CsrDouble)BINARY_IMPORT_SINT32(ptr); + pPos->alt = (double)BINARY_IMPORT_SINT32(ptr); pPos->alt *= 1e-2; } break; @@ -611,17 +613,17 @@ case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */ { tLocSvStatus *pSvStatus = &csrLocInst.svStatus; - CsrUint16 week; - CsrUint32 tow; - CsrUint32 towSubMs; - CsrUint8 info; - CsrInt32 nMsg = 0; - CsrUint16 satInfo; - CsrUint16 az; - CsrUint16 el; - CsrUint16 cno; - CsrUint8 gnssType; - CsrUint16 index = 0; + 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); @@ -649,16 +651,16 @@ cno = BINARY_IMPORT_UINT16(ptr); ptr += 4; - gnssType = (CsrUint8)((satInfo>>13)&0x0003); + 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 = (CsrUint8)(satInfo & 0xFF); - pSvStatus->svList[index].cno = (CsrFloat)(cno/10.0); // Scale: 10 - pSvStatus->svList[index].elevation = (CsrFloat)(el/10.0); // Scale: 10 - pSvStatus->svList[index].azimuth = (CsrFloat)(az/10.0); // Scale: 10 + 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 } @@ -668,19 +670,19 @@ index = pSvStatus->numOfGloSVs; if(index < CODEC_GLO_MAX_CHANNELS && cno>0) { - CsrInt16 freqChan = (satInfo & 0X1F00)>>8; - CsrInt16 slotNum = (satInfo & 0X00FF); + int16_t freqChan = (satInfo & 0X1F00)>>8; + int16_t slotNum = (satInfo & 0X00FF); if(slotNum > 0) { if(freqChan & 0X0010) { freqChan |= 0xFFE0; } - pSvStatus->gloSvList[index].prn = (CsrUint8)(freqChan + LOC_GLO_FREQ_OFFSET); - pSvStatus->gloSvList[index].sno = (CsrUint8)slotNum; - pSvStatus->gloSvList[index].cno = (CsrFloat)(cno/10.0); // Scale: 10 - pSvStatus->gloSvList[index].elevation = (CsrFloat)(el/10.0); // Scale: 10 - pSvStatus->gloSvList[index].azimuth = (CsrFloat)(az/10.0); // Scale: 10 + 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); } @@ -698,8 +700,8 @@ break; case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */ - *((CsrUint8 *)pMsgData) = *ptr; - *pMsgLen = sizeof(CsrUint8); + *((uint8_t *)pMsgData) = *ptr; + *pMsgLen = sizeof(uint8_t); break; default: @@ -734,11 +736,12 @@ 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); + if(!csrLocInst.bVerRsp) { _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ); } @@ -756,11 +759,11 @@ } break; case OSP_MSG_PWR_MODE_LPM_RSP: - csrLocInst.bPwrModeRsp = TRUE; + csrLocInst.bPwrModeRsp = true; CSR_LOG_INFO("lpm response.\r\n"); break; case OSP_MSG_PWR_MODE_FPM_RSP: - csrLocInst.bPwrModeRsp = TRUE; + csrLocInst.bPwrModeRsp = true; CSR_LOG_INFO("fpm response.\r\n"); break; default: @@ -771,7 +774,7 @@ void CsrLocation::_CsrLocTimeout(void) { - csrLocInst.bTimeoutFlag = TRUE; + csrLocInst.bTimeoutFlag = true; } void CsrLocation::_CsrLocRxHandler(void) @@ -789,8 +792,8 @@ void CsrLocation::_CsrLocSendData(eSendDataType type) { tCsrLocInst *pLocInst = &csrLocInst; - CsrUint32 i, size; - const CsrUint8 *pData; + uint32_t i, size; + const uint8_t *pData; switch(type) { @@ -815,7 +818,7 @@ size = sizeof(sOspSwitch2NmeaReq); break; case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ: - pData = (const CsrUint8 *)sNmeaSwitch2OspReq; + pData = (const uint8_t *)sNmeaSwitch2OspReq; size = strlen(sNmeaSwitch2OspReq); break;