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@4:0d9b711fb646, 2014-05-27 (annotated)
- Committer:
- zhjcpi
- Date:
- Tue May 27 10:54:06 2014 +0000
- Revision:
- 4:0d9b711fb646
- Parent:
- 3:71690f7bb480
- Child:
- 6:aed3c66b39d9
add more protection code to control state transfering and commands.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
zhjcpi | 0:aba381fc8158 | 1 | |
zhjcpi | 0:aba381fc8158 | 2 | /* CsrLocation class for mbed Microcontroller |
zhjcpi | 0:aba381fc8158 | 3 | * Copyright 2014 CSR plc |
zhjcpi | 0:aba381fc8158 | 4 | */ |
zhjcpi | 0:aba381fc8158 | 5 | |
zhjcpi | 0:aba381fc8158 | 6 | |
zhjcpi | 0:aba381fc8158 | 7 | #include "mbed.h" |
zhjcpi | 0:aba381fc8158 | 8 | #include "CsrLocation.h" |
zhjcpi | 0:aba381fc8158 | 9 | |
zhjcpi | 0:aba381fc8158 | 10 | |
zhjcpi | 2:d4fe184925f2 | 11 | static uint8_t sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3}; |
zhjcpi | 2:d4fe184925f2 | 12 | static uint8_t sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3}; |
zhjcpi | 2:d4fe184925f2 | 13 | static uint8_t sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E, |
zhjcpi | 1:bbaf9b8d646a | 14 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
zhjcpi | 1:bbaf9b8d646a | 15 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x63, 0xB0, 0xB3}; |
zhjcpi | 2:d4fe184925f2 | 16 | static uint8_t sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3}; |
zhjcpi | 2:d4fe184925f2 | 17 | static uint8_t sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00, |
zhjcpi | 0:aba381fc8158 | 18 | 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x12, 0xC0, 0x01, 0x64, 0xB0, 0xB3}; |
zhjcpi | 2:d4fe184925f2 | 19 | static char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n"; |
zhjcpi | 3:71690f7bb480 | 20 | static char sNmeaStopReq[] = "$PSRF117,16*0B\r\n"; |
zhjcpi | 3:71690f7bb480 | 21 | |
zhjcpi | 0:aba381fc8158 | 22 | |
zhjcpi | 0:aba381fc8158 | 23 | CsrLocation::CsrLocation(tCsrLocConfig *pLocConfig) |
zhjcpi | 0:aba381fc8158 | 24 | { |
zhjcpi | 0:aba381fc8158 | 25 | memset(&csrLocInst, 0, sizeof(tCsrLocInst)); |
zhjcpi | 0:aba381fc8158 | 26 | csrLocInst.pSerialDebug = pLocConfig->pSerialDebug; |
zhjcpi | 0:aba381fc8158 | 27 | csrLocInst.pSerialLoc = pLocConfig->pSerialLoc; |
zhjcpi | 0:aba381fc8158 | 28 | csrLocInst.pPinOnoff = pLocConfig->pPinOnoff; |
zhjcpi | 0:aba381fc8158 | 29 | csrLocInst.pPinReset = pLocConfig->pPinReset; |
zhjcpi | 0:aba381fc8158 | 30 | csrLocInst.pTimeoutChk = new Timeout(); |
zhjcpi | 0:aba381fc8158 | 31 | |
zhjcpi | 0:aba381fc8158 | 32 | csrLocInst.pPinReset->write(1); |
zhjcpi | 0:aba381fc8158 | 33 | csrLocInst.pPinOnoff->write(0); |
zhjcpi | 0:aba381fc8158 | 34 | } |
zhjcpi | 0:aba381fc8158 | 35 | |
zhjcpi | 0:aba381fc8158 | 36 | CsrLocation::~CsrLocation(void) |
zhjcpi | 0:aba381fc8158 | 37 | { |
zhjcpi | 0:aba381fc8158 | 38 | if(csrLocInst.pTimeoutChk != NULL) |
zhjcpi | 0:aba381fc8158 | 39 | { |
zhjcpi | 0:aba381fc8158 | 40 | delete csrLocInst.pTimeoutChk; |
zhjcpi | 0:aba381fc8158 | 41 | csrLocInst.pTimeoutChk = NULL; |
zhjcpi | 0:aba381fc8158 | 42 | } |
zhjcpi | 0:aba381fc8158 | 43 | csrLocInst.pSerialLoc->attach(NULL); |
zhjcpi | 0:aba381fc8158 | 44 | csrLocInst.pPinReset->write(0); |
zhjcpi | 0:aba381fc8158 | 45 | csrLocInst.pPinOnoff->write(0); |
zhjcpi | 0:aba381fc8158 | 46 | memset(&csrLocInst, 0, sizeof(tCsrLocInst)); |
zhjcpi | 0:aba381fc8158 | 47 | |
zhjcpi | 0:aba381fc8158 | 48 | } |
zhjcpi | 0:aba381fc8158 | 49 | |
zhjcpi | 0:aba381fc8158 | 50 | void CsrLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb) |
zhjcpi | 0:aba381fc8158 | 51 | { |
zhjcpi | 0:aba381fc8158 | 52 | csrLocInst.appOutCb = app_output_cb; |
zhjcpi | 0:aba381fc8158 | 53 | csrLocInst.appEventCb = app_event_cb; |
zhjcpi | 0:aba381fc8158 | 54 | } |
zhjcpi | 0:aba381fc8158 | 55 | |
zhjcpi | 0:aba381fc8158 | 56 | void CsrLocation::CsrLocReset(void) |
zhjcpi | 0:aba381fc8158 | 57 | { |
zhjcpi | 0:aba381fc8158 | 58 | _CsrLocHwReset(); |
zhjcpi | 0:aba381fc8158 | 59 | } |
zhjcpi | 0:aba381fc8158 | 60 | |
zhjcpi | 4:0d9b711fb646 | 61 | eCsrLocState CsrLocation::CsrLocGetState(void) |
zhjcpi | 4:0d9b711fb646 | 62 | { |
zhjcpi | 4:0d9b711fb646 | 63 | return csrLocInst.locState; |
zhjcpi | 4:0d9b711fb646 | 64 | } |
zhjcpi | 4:0d9b711fb646 | 65 | |
zhjcpi | 3:71690f7bb480 | 66 | void CsrLocation::CsrLocStart(ePowerMode pwrMode, eProto proto) |
zhjcpi | 0:aba381fc8158 | 67 | { |
zhjcpi | 0:aba381fc8158 | 68 | csrLocInst.pwrMode = pwrMode; |
zhjcpi | 0:aba381fc8158 | 69 | |
zhjcpi | 0:aba381fc8158 | 70 | if(csrLocInst.locState == CSR_LOC_STATE_IDLE) |
zhjcpi | 0:aba381fc8158 | 71 | { |
zhjcpi | 0:aba381fc8158 | 72 | /* Csr Location SDK version */ |
zhjcpi | 0:aba381fc8158 | 73 | CSR_LOG_INFO("==== CSR LOC SDK version: %s Date: %s ====\r\n", CSR_LOC_SDK_VER, __DATE__ " " __TIME__); |
zhjcpi | 3:71690f7bb480 | 74 | |
zhjcpi | 3:71690f7bb480 | 75 | csrLocInst.proto = proto; |
zhjcpi | 3:71690f7bb480 | 76 | |
zhjcpi | 0:aba381fc8158 | 77 | /* open UART */ |
zhjcpi | 3:71690f7bb480 | 78 | if(proto == PROTO_NMEA) |
zhjcpi | 3:71690f7bb480 | 79 | { |
zhjcpi | 3:71690f7bb480 | 80 | CSR_LOG_INFO("Checking NMEA protocol...\r\n"); |
zhjcpi | 3:71690f7bb480 | 81 | csrLocInst.protoState = PROTO_STATE_DET_NMEA; |
zhjcpi | 3:71690f7bb480 | 82 | csrLocInst.baudRate = BAUDRATE_NMEA; |
zhjcpi | 3:71690f7bb480 | 83 | } |
zhjcpi | 3:71690f7bb480 | 84 | else |
zhjcpi | 3:71690f7bb480 | 85 | { |
zhjcpi | 3:71690f7bb480 | 86 | CSR_LOG_INFO("Checking OSP protocol...\r\n"); |
zhjcpi | 3:71690f7bb480 | 87 | csrLocInst.protoState = PROTO_STATE_DET_OSP; |
zhjcpi | 3:71690f7bb480 | 88 | csrLocInst.baudRate = BAUDRATE_OSP; |
zhjcpi | 3:71690f7bb480 | 89 | } |
zhjcpi | 0:aba381fc8158 | 90 | _CsrLocUartInit(); |
zhjcpi | 0:aba381fc8158 | 91 | |
zhjcpi | 0:aba381fc8158 | 92 | /* trigger on_off */ |
zhjcpi | 0:aba381fc8158 | 93 | _CsrLocHwOnoff(); |
zhjcpi | 4:0d9b711fb646 | 94 | if(csrLocInst.pwrMode == PWR_PTF) |
zhjcpi | 4:0d9b711fb646 | 95 | { |
zhjcpi | 4:0d9b711fb646 | 96 | _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ); |
zhjcpi | 4:0d9b711fb646 | 97 | } |
zhjcpi | 0:aba381fc8158 | 98 | |
zhjcpi | 0:aba381fc8158 | 99 | csrLocInst.locState = CSR_LOC_STATE_RUN; |
zhjcpi | 2:d4fe184925f2 | 100 | csrLocInst.bPwrModeRsp = false; |
zhjcpi | 2:d4fe184925f2 | 101 | csrLocInst.bVerRsp = false; |
zhjcpi | 1:bbaf9b8d646a | 102 | |
zhjcpi | 0:aba381fc8158 | 103 | } |
zhjcpi | 0:aba381fc8158 | 104 | else |
zhjcpi | 0:aba381fc8158 | 105 | { |
zhjcpi | 0:aba381fc8158 | 106 | CSR_LOG_INFO("Already started.\r\n"); |
zhjcpi | 0:aba381fc8158 | 107 | } |
zhjcpi | 0:aba381fc8158 | 108 | } |
zhjcpi | 0:aba381fc8158 | 109 | |
zhjcpi | 0:aba381fc8158 | 110 | void CsrLocation::CsrLocUpdate(void) |
zhjcpi | 0:aba381fc8158 | 111 | { |
zhjcpi | 0:aba381fc8158 | 112 | if(csrLocInst.locState == CSR_LOC_STATE_RUN) |
zhjcpi | 0:aba381fc8158 | 113 | { |
zhjcpi | 0:aba381fc8158 | 114 | /* wait and process uart data */ |
zhjcpi | 0:aba381fc8158 | 115 | _CsrLocProcessRawStream(); |
zhjcpi | 0:aba381fc8158 | 116 | } |
zhjcpi | 0:aba381fc8158 | 117 | } |
zhjcpi | 0:aba381fc8158 | 118 | |
zhjcpi | 0:aba381fc8158 | 119 | void CsrLocation::CsrLocStop(void) |
zhjcpi | 0:aba381fc8158 | 120 | { |
zhjcpi | 0:aba381fc8158 | 121 | csrLocInst.pSerialLoc->attach(NULL); |
zhjcpi | 0:aba381fc8158 | 122 | CSR_LOG_INFO("Stop command processed.\r\n"); |
zhjcpi | 0:aba381fc8158 | 123 | |
zhjcpi | 0:aba381fc8158 | 124 | if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND) |
zhjcpi | 0:aba381fc8158 | 125 | { |
zhjcpi | 0:aba381fc8158 | 126 | /* in sleep mode, trigger on_off firstly */ |
zhjcpi | 0:aba381fc8158 | 127 | _CsrLocHwOnoff(); |
zhjcpi | 0:aba381fc8158 | 128 | wait_ms(500); |
zhjcpi | 3:71690f7bb480 | 129 | } |
zhjcpi | 3:71690f7bb480 | 130 | if(csrLocInst.proto == PROTO_NMEA) |
zhjcpi | 3:71690f7bb480 | 131 | { |
zhjcpi | 3:71690f7bb480 | 132 | _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ); |
zhjcpi | 3:71690f7bb480 | 133 | } |
zhjcpi | 3:71690f7bb480 | 134 | else |
zhjcpi | 3:71690f7bb480 | 135 | { |
zhjcpi | 4:0d9b711fb646 | 136 | if(csrLocInst.pwrMode == PWR_PTF) |
zhjcpi | 4:0d9b711fb646 | 137 | { |
zhjcpi | 4:0d9b711fb646 | 138 | CSR_LOG_INFO("Stop with fpm.\r\n"); |
zhjcpi | 4:0d9b711fb646 | 139 | _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ); |
zhjcpi | 4:0d9b711fb646 | 140 | } |
zhjcpi | 4:0d9b711fb646 | 141 | |
zhjcpi | 3:71690f7bb480 | 142 | _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ); |
zhjcpi | 3:71690f7bb480 | 143 | } |
zhjcpi | 0:aba381fc8158 | 144 | wait_ms(10); |
zhjcpi | 0:aba381fc8158 | 145 | _CsrLocHwReset(); |
zhjcpi | 0:aba381fc8158 | 146 | csrLocInst.locState = CSR_LOC_STATE_IDLE; |
zhjcpi | 0:aba381fc8158 | 147 | csrLocInst.appEventCb(CSR_LOC_EVENT_STOP_RESULT, 0); |
zhjcpi | 0:aba381fc8158 | 148 | |
zhjcpi | 0:aba381fc8158 | 149 | } |
zhjcpi | 0:aba381fc8158 | 150 | |
zhjcpi | 0:aba381fc8158 | 151 | void CsrLocation::CsrLocLpmGetPos(void) |
zhjcpi | 0:aba381fc8158 | 152 | { |
zhjcpi | 0:aba381fc8158 | 153 | if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND) |
zhjcpi | 0:aba381fc8158 | 154 | { |
zhjcpi | 0:aba381fc8158 | 155 | CSR_LOG_INFO("LpmGetPos "); |
zhjcpi | 0:aba381fc8158 | 156 | _CsrLocHwOnoff(); |
zhjcpi | 2:d4fe184925f2 | 157 | } |
zhjcpi | 2:d4fe184925f2 | 158 | else |
zhjcpi | 2:d4fe184925f2 | 159 | { |
zhjcpi | 2:d4fe184925f2 | 160 | if(csrLocInst.locState != CSR_LOC_STATE_RUN) |
zhjcpi | 2:d4fe184925f2 | 161 | { |
zhjcpi | 2:d4fe184925f2 | 162 | CSR_LOG_INFO("Not in run state.\r\n"); |
zhjcpi | 2:d4fe184925f2 | 163 | } |
zhjcpi | 2:d4fe184925f2 | 164 | else if(csrLocInst.pwrMode != PWR_PTF) |
zhjcpi | 2:d4fe184925f2 | 165 | { |
zhjcpi | 2:d4fe184925f2 | 166 | CSR_LOG_INFO("Not in low power PTF mode.\r\n"); |
zhjcpi | 2:d4fe184925f2 | 167 | } |
zhjcpi | 2:d4fe184925f2 | 168 | } |
zhjcpi | 0:aba381fc8158 | 169 | } |
zhjcpi | 0:aba381fc8158 | 170 | |
zhjcpi | 0:aba381fc8158 | 171 | void CsrLocation::CsrLocDebugSwitch2Nmea(void) |
zhjcpi | 0:aba381fc8158 | 172 | { |
zhjcpi | 0:aba381fc8158 | 173 | if(csrLocInst.locState == CSR_LOC_STATE_RUN) |
zhjcpi | 0:aba381fc8158 | 174 | { |
zhjcpi | 0:aba381fc8158 | 175 | _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ); |
zhjcpi | 0:aba381fc8158 | 176 | wait_ms(200); |
zhjcpi | 0:aba381fc8158 | 177 | _CsrLocHwReset(); |
zhjcpi | 0:aba381fc8158 | 178 | } |
zhjcpi | 0:aba381fc8158 | 179 | } |
zhjcpi | 0:aba381fc8158 | 180 | |
zhjcpi | 0:aba381fc8158 | 181 | void CsrLocation::_CsrLocUartInit(void) |
zhjcpi | 0:aba381fc8158 | 182 | { |
zhjcpi | 0:aba381fc8158 | 183 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 0:aba381fc8158 | 184 | |
zhjcpi | 0:aba381fc8158 | 185 | pLocInst->in = 0; |
zhjcpi | 0:aba381fc8158 | 186 | pLocInst->out = 0; |
zhjcpi | 0:aba381fc8158 | 187 | memset(pLocInst->serialBuf, 0 , MAX_SERIAL_BUF_LEN); |
zhjcpi | 0:aba381fc8158 | 188 | memset(pLocInst->serialPkt, 0 , MAX_SERIAL_PKT_LEN); |
zhjcpi | 0:aba381fc8158 | 189 | |
zhjcpi | 0:aba381fc8158 | 190 | pLocInst->checksum = 0; |
zhjcpi | 0:aba381fc8158 | 191 | pLocInst->msgSize = 0; |
zhjcpi | 0:aba381fc8158 | 192 | pLocInst->decodeIndex = 0; |
zhjcpi | 0:aba381fc8158 | 193 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 194 | pLocInst->pTimeoutChk->attach(this, &CsrLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT); |
zhjcpi | 0:aba381fc8158 | 195 | |
zhjcpi | 0:aba381fc8158 | 196 | pLocInst->pSerialLoc->baud(pLocInst->baudRate); |
zhjcpi | 0:aba381fc8158 | 197 | pLocInst->pSerialLoc->attach(this, &CsrLocation::_CsrLocRxHandler ); |
zhjcpi | 0:aba381fc8158 | 198 | } |
zhjcpi | 0:aba381fc8158 | 199 | |
zhjcpi | 0:aba381fc8158 | 200 | void CsrLocation::_CsrLocProcessRawStream(void) |
zhjcpi | 0:aba381fc8158 | 201 | { |
zhjcpi | 0:aba381fc8158 | 202 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 2:d4fe184925f2 | 203 | uint8_t data; |
zhjcpi | 0:aba381fc8158 | 204 | |
zhjcpi | 3:71690f7bb480 | 205 | if(pLocInst->bTimeoutFlag) |
zhjcpi | 3:71690f7bb480 | 206 | { |
zhjcpi | 3:71690f7bb480 | 207 | pLocInst->pTimeoutChk->detach(); |
zhjcpi | 3:71690f7bb480 | 208 | pLocInst->bTimeoutFlag = false; |
zhjcpi | 3:71690f7bb480 | 209 | if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA) |
zhjcpi | 3:71690f7bb480 | 210 | { |
zhjcpi | 3:71690f7bb480 | 211 | _CsrLocDetProtoOspTimeout(); |
zhjcpi | 3:71690f7bb480 | 212 | } |
zhjcpi | 3:71690f7bb480 | 213 | else if(pLocInst->protoState == PROTO_STATE_DET_NMEA || pLocInst->protoState == PROTO_STATE_DET_NMEA_FROM_OSP) |
zhjcpi | 3:71690f7bb480 | 214 | { |
zhjcpi | 3:71690f7bb480 | 215 | _CsrLocDetProtoNmeaTimeout(); |
zhjcpi | 3:71690f7bb480 | 216 | } |
zhjcpi | 3:71690f7bb480 | 217 | else |
zhjcpi | 3:71690f7bb480 | 218 | { |
zhjcpi | 3:71690f7bb480 | 219 | CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", pLocInst->protoState); |
zhjcpi | 3:71690f7bb480 | 220 | } |
zhjcpi | 3:71690f7bb480 | 221 | return; |
zhjcpi | 3:71690f7bb480 | 222 | } |
zhjcpi | 3:71690f7bb480 | 223 | |
zhjcpi | 0:aba381fc8158 | 224 | if(pLocInst->in != pLocInst->out) |
zhjcpi | 0:aba381fc8158 | 225 | { |
zhjcpi | 0:aba381fc8158 | 226 | data = pLocInst->serialBuf[pLocInst->out++]; |
zhjcpi | 0:aba381fc8158 | 227 | pLocInst->out &= (MAX_SERIAL_BUF_LEN-1); |
zhjcpi | 0:aba381fc8158 | 228 | switch(pLocInst->protoState) |
zhjcpi | 0:aba381fc8158 | 229 | { |
zhjcpi | 0:aba381fc8158 | 230 | case PROTO_STATE_DET_OSP: |
zhjcpi | 2:d4fe184925f2 | 231 | case PROTO_STATE_DET_OSP_FROM_NMEA: |
zhjcpi | 3:71690f7bb480 | 232 | _CsrLocProcessRawOspStream(data); |
zhjcpi | 0:aba381fc8158 | 233 | break; |
zhjcpi | 0:aba381fc8158 | 234 | case PROTO_STATE_DET_NMEA: |
zhjcpi | 3:71690f7bb480 | 235 | case PROTO_STATE_DET_NMEA_FROM_OSP: |
zhjcpi | 3:71690f7bb480 | 236 | _CsrLocProcessRawNmeaStream(data); |
zhjcpi | 0:aba381fc8158 | 237 | break; |
zhjcpi | 0:aba381fc8158 | 238 | case PROTO_STATE_DET_OK: |
zhjcpi | 3:71690f7bb480 | 239 | if(pLocInst->proto == PROTO_NMEA) |
zhjcpi | 3:71690f7bb480 | 240 | { |
zhjcpi | 3:71690f7bb480 | 241 | _CsrLocProcessRawNmeaStream(data); |
zhjcpi | 3:71690f7bb480 | 242 | } |
zhjcpi | 3:71690f7bb480 | 243 | else |
zhjcpi | 3:71690f7bb480 | 244 | { |
zhjcpi | 3:71690f7bb480 | 245 | _CsrLocProcessRawOspStream(data); |
zhjcpi | 3:71690f7bb480 | 246 | } |
zhjcpi | 0:aba381fc8158 | 247 | break; |
zhjcpi | 0:aba381fc8158 | 248 | default: |
zhjcpi | 0:aba381fc8158 | 249 | /* Discard received data */ |
zhjcpi | 1:bbaf9b8d646a | 250 | //pLocInst->out = pLocInst->in; |
zhjcpi | 0:aba381fc8158 | 251 | break; |
zhjcpi | 0:aba381fc8158 | 252 | } |
zhjcpi | 0:aba381fc8158 | 253 | } |
zhjcpi | 0:aba381fc8158 | 254 | } |
zhjcpi | 0:aba381fc8158 | 255 | |
zhjcpi | 3:71690f7bb480 | 256 | void CsrLocation::_CsrLocDetProtoOspTimeout(void) |
zhjcpi | 0:aba381fc8158 | 257 | { |
zhjcpi | 0:aba381fc8158 | 258 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 0:aba381fc8158 | 259 | |
zhjcpi | 3:71690f7bb480 | 260 | if(pLocInst->proto == PROTO_NMEA) |
zhjcpi | 3:71690f7bb480 | 261 | { |
zhjcpi | 3:71690f7bb480 | 262 | /* Failed to detect OSP */ |
zhjcpi | 3:71690f7bb480 | 263 | pLocInst->pSerialLoc->attach(NULL); |
zhjcpi | 3:71690f7bb480 | 264 | pLocInst->protoState = PROTO_STATE_DET_INVALID; |
zhjcpi | 3:71690f7bb480 | 265 | pLocInst->baudRate = BAUDRATE_NMEA; |
zhjcpi | 3:71690f7bb480 | 266 | CSR_LOG_INFO("Checking OSP failed.\r\n"); |
zhjcpi | 3:71690f7bb480 | 267 | pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); |
zhjcpi | 3:71690f7bb480 | 268 | } |
zhjcpi | 3:71690f7bb480 | 269 | else |
zhjcpi | 0:aba381fc8158 | 270 | { |
zhjcpi | 0:aba381fc8158 | 271 | /* Failed to detect OSP and try to detect NMEA */ |
zhjcpi | 0:aba381fc8158 | 272 | pLocInst->pSerialLoc->attach(NULL); |
zhjcpi | 0:aba381fc8158 | 273 | if(pLocInst->protoState == PROTO_STATE_DET_OSP) |
zhjcpi | 0:aba381fc8158 | 274 | { |
zhjcpi | 0:aba381fc8158 | 275 | pLocInst->protoState = PROTO_STATE_DET_NMEA; |
zhjcpi | 0:aba381fc8158 | 276 | pLocInst->baudRate = BAUDRATE_NMEA; |
zhjcpi | 0:aba381fc8158 | 277 | CSR_LOG_INFO("Checking OSP protocol failed, now check NMEA protocol...\r\n"); |
zhjcpi | 0:aba381fc8158 | 278 | _CsrLocUartInit(); |
zhjcpi | 0:aba381fc8158 | 279 | } |
zhjcpi | 0:aba381fc8158 | 280 | else |
zhjcpi | 0:aba381fc8158 | 281 | { |
zhjcpi | 0:aba381fc8158 | 282 | pLocInst->protoState = PROTO_STATE_DET_INVALID; |
zhjcpi | 0:aba381fc8158 | 283 | CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n"); |
zhjcpi | 0:aba381fc8158 | 284 | pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); |
zhjcpi | 0:aba381fc8158 | 285 | } |
zhjcpi | 0:aba381fc8158 | 286 | } |
zhjcpi | 0:aba381fc8158 | 287 | } |
zhjcpi | 0:aba381fc8158 | 288 | |
zhjcpi | 3:71690f7bb480 | 289 | void CsrLocation::_CsrLocDetProtoNmeaTimeout(void) |
zhjcpi | 0:aba381fc8158 | 290 | { |
zhjcpi | 0:aba381fc8158 | 291 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 0:aba381fc8158 | 292 | |
zhjcpi | 3:71690f7bb480 | 293 | CSR_LOG_INFO("Checking NMEA protocol failed\r\n"); |
zhjcpi | 3:71690f7bb480 | 294 | |
zhjcpi | 3:71690f7bb480 | 295 | if(pLocInst->proto == PROTO_NMEA) |
zhjcpi | 0:aba381fc8158 | 296 | { |
zhjcpi | 3:71690f7bb480 | 297 | /* Failed to detect NMEA and try to detect OSP */ |
zhjcpi | 0:aba381fc8158 | 298 | pLocInst->pSerialLoc->attach(NULL); |
zhjcpi | 3:71690f7bb480 | 299 | if(pLocInst->protoState == PROTO_STATE_DET_NMEA) |
zhjcpi | 3:71690f7bb480 | 300 | { |
zhjcpi | 3:71690f7bb480 | 301 | pLocInst->protoState = PROTO_STATE_DET_OSP; |
zhjcpi | 3:71690f7bb480 | 302 | pLocInst->baudRate = BAUDRATE_OSP; |
zhjcpi | 3:71690f7bb480 | 303 | CSR_LOG_INFO("Checking NMEA protocol failed, now check OSP protocol...\r\n"); |
zhjcpi | 3:71690f7bb480 | 304 | _CsrLocUartInit(); |
zhjcpi | 3:71690f7bb480 | 305 | } |
zhjcpi | 3:71690f7bb480 | 306 | else |
zhjcpi | 3:71690f7bb480 | 307 | { |
zhjcpi | 3:71690f7bb480 | 308 | pLocInst->protoState = PROTO_STATE_DET_INVALID; |
zhjcpi | 3:71690f7bb480 | 309 | CSR_LOG_INFO("Checking switched NMEA protocol failed.\r\n"); |
zhjcpi | 3:71690f7bb480 | 310 | pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); |
zhjcpi | 3:71690f7bb480 | 311 | } |
zhjcpi | 3:71690f7bb480 | 312 | } |
zhjcpi | 3:71690f7bb480 | 313 | else |
zhjcpi | 3:71690f7bb480 | 314 | { |
zhjcpi | 3:71690f7bb480 | 315 | /* Failed to detect NEMA */ |
zhjcpi | 3:71690f7bb480 | 316 | pLocInst->pSerialLoc->attach(NULL); |
zhjcpi | 0:aba381fc8158 | 317 | pLocInst->protoState = PROTO_STATE_DET_INVALID; |
zhjcpi | 0:aba381fc8158 | 318 | pLocInst->baudRate = BAUDRATE_OSP; |
zhjcpi | 0:aba381fc8158 | 319 | CSR_LOG_INFO("Checking NMEA failed.\r\n"); |
zhjcpi | 0:aba381fc8158 | 320 | pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1); |
zhjcpi | 0:aba381fc8158 | 321 | } |
zhjcpi | 3:71690f7bb480 | 322 | } |
zhjcpi | 0:aba381fc8158 | 323 | |
zhjcpi | 3:71690f7bb480 | 324 | void CsrLocation::_CsrLocProcessRawNmeaStream(uint8_t data) |
zhjcpi | 3:71690f7bb480 | 325 | { |
zhjcpi | 3:71690f7bb480 | 326 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 3:71690f7bb480 | 327 | |
zhjcpi | 0:aba381fc8158 | 328 | switch (pLocInst->protoDetState) |
zhjcpi | 0:aba381fc8158 | 329 | { |
zhjcpi | 0:aba381fc8158 | 330 | case STATE_START1: |
zhjcpi | 0:aba381fc8158 | 331 | if (NMEA_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 332 | { |
zhjcpi | 0:aba381fc8158 | 333 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 334 | } |
zhjcpi | 0:aba381fc8158 | 335 | break; |
zhjcpi | 0:aba381fc8158 | 336 | |
zhjcpi | 0:aba381fc8158 | 337 | case STATE_START2: |
zhjcpi | 0:aba381fc8158 | 338 | if (NMEA_MSG_HEAD1 == data) |
zhjcpi | 0:aba381fc8158 | 339 | { |
zhjcpi | 0:aba381fc8158 | 340 | pLocInst->protoDetState = STATE_END1; |
zhjcpi | 3:71690f7bb480 | 341 | pLocInst->decodeIndex = 0; |
zhjcpi | 3:71690f7bb480 | 342 | pLocInst->serialPkt[pLocInst->decodeIndex++] = data; |
zhjcpi | 0:aba381fc8158 | 343 | } |
zhjcpi | 0:aba381fc8158 | 344 | else if (NMEA_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 345 | { |
zhjcpi | 0:aba381fc8158 | 346 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 347 | } |
zhjcpi | 0:aba381fc8158 | 348 | else |
zhjcpi | 0:aba381fc8158 | 349 | { |
zhjcpi | 0:aba381fc8158 | 350 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 351 | } |
zhjcpi | 0:aba381fc8158 | 352 | break; |
zhjcpi | 0:aba381fc8158 | 353 | case STATE_END1: |
zhjcpi | 0:aba381fc8158 | 354 | if (NMEA_MSG_TAIL0 == data) |
zhjcpi | 0:aba381fc8158 | 355 | { |
zhjcpi | 0:aba381fc8158 | 356 | pLocInst->pTimeoutChk->detach(); |
zhjcpi | 2:d4fe184925f2 | 357 | pLocInst->bTimeoutFlag = false; |
zhjcpi | 3:71690f7bb480 | 358 | if(pLocInst->proto == PROTO_NMEA) |
zhjcpi | 3:71690f7bb480 | 359 | { |
zhjcpi | 3:71690f7bb480 | 360 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 3:71690f7bb480 | 361 | if(pLocInst->protoState == PROTO_STATE_DET_NMEA || pLocInst->protoState == PROTO_STATE_DET_NMEA_FROM_OSP) |
zhjcpi | 3:71690f7bb480 | 362 | { |
zhjcpi | 3:71690f7bb480 | 363 | CSR_LOG_INFO("Checking NMEA protocol OK.\r\n"); |
zhjcpi | 3:71690f7bb480 | 364 | pLocInst->protoState = PROTO_STATE_DET_OK; |
zhjcpi | 3:71690f7bb480 | 365 | pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0); |
zhjcpi | 3:71690f7bb480 | 366 | } |
zhjcpi | 0:aba381fc8158 | 367 | |
zhjcpi | 3:71690f7bb480 | 368 | pLocInst->serialPkt[pLocInst->decodeIndex++] = '\0'; |
zhjcpi | 3:71690f7bb480 | 369 | _CsrLocProcessRawNmeaPkt(); |
zhjcpi | 3:71690f7bb480 | 370 | } |
zhjcpi | 3:71690f7bb480 | 371 | else |
zhjcpi | 3:71690f7bb480 | 372 | { |
zhjcpi | 3:71690f7bb480 | 373 | pLocInst->pSerialLoc->attach(NULL); |
zhjcpi | 3:71690f7bb480 | 374 | |
zhjcpi | 3:71690f7bb480 | 375 | CSR_LOG_INFO("Checking NMEA protocol OK, switching to OSP...\r\n"); |
zhjcpi | 3:71690f7bb480 | 376 | _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ); |
zhjcpi | 3:71690f7bb480 | 377 | wait_ms(100); |
zhjcpi | 3:71690f7bb480 | 378 | pLocInst->protoState = PROTO_STATE_DET_OSP_FROM_NMEA; |
zhjcpi | 3:71690f7bb480 | 379 | pLocInst->baudRate = BAUDRATE_OSP; |
zhjcpi | 3:71690f7bb480 | 380 | CSR_LOG_INFO("Checking switched OSP protocol...\r\n"); |
zhjcpi | 3:71690f7bb480 | 381 | _CsrLocUartInit(); |
zhjcpi | 3:71690f7bb480 | 382 | } |
zhjcpi | 0:aba381fc8158 | 383 | } |
zhjcpi | 0:aba381fc8158 | 384 | else if (NMEA_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 385 | { |
zhjcpi | 0:aba381fc8158 | 386 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 387 | } |
zhjcpi | 3:71690f7bb480 | 388 | else |
zhjcpi | 3:71690f7bb480 | 389 | { |
zhjcpi | 3:71690f7bb480 | 390 | if(pLocInst->decodeIndex < (MAX_SERIAL_PKT_LEN - 2)) |
zhjcpi | 3:71690f7bb480 | 391 | { |
zhjcpi | 3:71690f7bb480 | 392 | pLocInst->serialPkt[pLocInst->decodeIndex++] = data; |
zhjcpi | 3:71690f7bb480 | 393 | } |
zhjcpi | 3:71690f7bb480 | 394 | else |
zhjcpi | 3:71690f7bb480 | 395 | { |
zhjcpi | 3:71690f7bb480 | 396 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 3:71690f7bb480 | 397 | } |
zhjcpi | 3:71690f7bb480 | 398 | } |
zhjcpi | 0:aba381fc8158 | 399 | break; |
zhjcpi | 0:aba381fc8158 | 400 | default: |
zhjcpi | 0:aba381fc8158 | 401 | break; |
zhjcpi | 0:aba381fc8158 | 402 | } |
zhjcpi | 0:aba381fc8158 | 403 | } |
zhjcpi | 0:aba381fc8158 | 404 | |
zhjcpi | 2:d4fe184925f2 | 405 | void CsrLocation::_CsrLocProcessRawOspStream(uint8_t data) |
zhjcpi | 0:aba381fc8158 | 406 | { |
zhjcpi | 0:aba381fc8158 | 407 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 0:aba381fc8158 | 408 | |
zhjcpi | 0:aba381fc8158 | 409 | switch (pLocInst->protoDetState) |
zhjcpi | 0:aba381fc8158 | 410 | { |
zhjcpi | 0:aba381fc8158 | 411 | case STATE_START1: |
zhjcpi | 0:aba381fc8158 | 412 | if (OSP_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 413 | { |
zhjcpi | 0:aba381fc8158 | 414 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 415 | } |
zhjcpi | 0:aba381fc8158 | 416 | break; |
zhjcpi | 0:aba381fc8158 | 417 | |
zhjcpi | 0:aba381fc8158 | 418 | case STATE_START2: |
zhjcpi | 0:aba381fc8158 | 419 | if (OSP_MSG_HEAD1 == data) |
zhjcpi | 0:aba381fc8158 | 420 | { |
zhjcpi | 0:aba381fc8158 | 421 | pLocInst->protoDetState = STATE_SIZE1; |
zhjcpi | 0:aba381fc8158 | 422 | } |
zhjcpi | 0:aba381fc8158 | 423 | else if (OSP_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 424 | { |
zhjcpi | 0:aba381fc8158 | 425 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 426 | } |
zhjcpi | 0:aba381fc8158 | 427 | else |
zhjcpi | 0:aba381fc8158 | 428 | { |
zhjcpi | 0:aba381fc8158 | 429 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 430 | } |
zhjcpi | 0:aba381fc8158 | 431 | break; |
zhjcpi | 0:aba381fc8158 | 432 | |
zhjcpi | 0:aba381fc8158 | 433 | case STATE_SIZE1: |
zhjcpi | 0:aba381fc8158 | 434 | pLocInst->msgSize = data ; |
zhjcpi | 2:d4fe184925f2 | 435 | pLocInst->msgSize <<= 8; /* high uint8_t */ |
zhjcpi | 0:aba381fc8158 | 436 | pLocInst->protoDetState = STATE_SIZE2; |
zhjcpi | 0:aba381fc8158 | 437 | break; |
zhjcpi | 0:aba381fc8158 | 438 | |
zhjcpi | 0:aba381fc8158 | 439 | case STATE_SIZE2: |
zhjcpi | 0:aba381fc8158 | 440 | pLocInst->msgSize += data ; |
zhjcpi | 0:aba381fc8158 | 441 | if (MAX_SERIAL_PKT_LEN < pLocInst->msgSize || 0 == pLocInst->msgSize) |
zhjcpi | 0:aba381fc8158 | 442 | { |
zhjcpi | 0:aba381fc8158 | 443 | if (OSP_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 444 | { |
zhjcpi | 0:aba381fc8158 | 445 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 446 | } |
zhjcpi | 0:aba381fc8158 | 447 | else |
zhjcpi | 0:aba381fc8158 | 448 | { |
zhjcpi | 0:aba381fc8158 | 449 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 450 | } |
zhjcpi | 0:aba381fc8158 | 451 | } |
zhjcpi | 0:aba381fc8158 | 452 | else |
zhjcpi | 0:aba381fc8158 | 453 | { |
zhjcpi | 0:aba381fc8158 | 454 | pLocInst->computedCheckSum = 0; |
zhjcpi | 0:aba381fc8158 | 455 | pLocInst->decodeIndex = 0; |
zhjcpi | 0:aba381fc8158 | 456 | pLocInst->protoDetState = STATE_PAYLOAD; |
zhjcpi | 0:aba381fc8158 | 457 | } |
zhjcpi | 0:aba381fc8158 | 458 | break; |
zhjcpi | 0:aba381fc8158 | 459 | |
zhjcpi | 0:aba381fc8158 | 460 | case STATE_PAYLOAD: |
zhjcpi | 0:aba381fc8158 | 461 | /* check for a catastrophic error case */ |
zhjcpi | 0:aba381fc8158 | 462 | if (MAX_SERIAL_PKT_LEN <= pLocInst->decodeIndex) |
zhjcpi | 0:aba381fc8158 | 463 | { |
zhjcpi | 0:aba381fc8158 | 464 | /* This is really bad. And should never happen since we |
zhjcpi | 0:aba381fc8158 | 465 | * gurantee that msgSize is always less than RECEIVE_PACKET_SIZE |
zhjcpi | 0:aba381fc8158 | 466 | * Change the state back to STATE_START1 and start over */ |
zhjcpi | 0:aba381fc8158 | 467 | if (OSP_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 468 | { |
zhjcpi | 0:aba381fc8158 | 469 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 470 | } |
zhjcpi | 0:aba381fc8158 | 471 | else |
zhjcpi | 0:aba381fc8158 | 472 | { |
zhjcpi | 0:aba381fc8158 | 473 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 474 | } |
zhjcpi | 0:aba381fc8158 | 475 | break; |
zhjcpi | 0:aba381fc8158 | 476 | } |
zhjcpi | 0:aba381fc8158 | 477 | /* Store the byte */ |
zhjcpi | 0:aba381fc8158 | 478 | pLocInst->serialPkt[pLocInst->decodeIndex++] = data; |
zhjcpi | 0:aba381fc8158 | 479 | pLocInst->computedCheckSum += data; |
zhjcpi | 0:aba381fc8158 | 480 | |
zhjcpi | 0:aba381fc8158 | 481 | /* Check to see if we've read the full payload */ |
zhjcpi | 0:aba381fc8158 | 482 | if (0 == (--pLocInst->msgSize)) |
zhjcpi | 0:aba381fc8158 | 483 | { |
zhjcpi | 0:aba381fc8158 | 484 | pLocInst->computedCheckSum &= 0x7FFF; |
zhjcpi | 0:aba381fc8158 | 485 | pLocInst->protoDetState = STATE_CHECKSUM1; |
zhjcpi | 0:aba381fc8158 | 486 | } |
zhjcpi | 0:aba381fc8158 | 487 | break; |
zhjcpi | 0:aba381fc8158 | 488 | |
zhjcpi | 0:aba381fc8158 | 489 | case STATE_CHECKSUM1: |
zhjcpi | 0:aba381fc8158 | 490 | pLocInst->checksum = data ; |
zhjcpi | 0:aba381fc8158 | 491 | pLocInst->checksum <<= 8; |
zhjcpi | 0:aba381fc8158 | 492 | pLocInst->protoDetState = STATE_CHECKSUM2; |
zhjcpi | 0:aba381fc8158 | 493 | break; |
zhjcpi | 0:aba381fc8158 | 494 | |
zhjcpi | 0:aba381fc8158 | 495 | case STATE_CHECKSUM2: |
zhjcpi | 0:aba381fc8158 | 496 | pLocInst->checksum += data; |
zhjcpi | 0:aba381fc8158 | 497 | if (pLocInst->computedCheckSum != pLocInst->checksum) |
zhjcpi | 0:aba381fc8158 | 498 | { |
zhjcpi | 0:aba381fc8158 | 499 | if (OSP_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 500 | { |
zhjcpi | 0:aba381fc8158 | 501 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 502 | } |
zhjcpi | 0:aba381fc8158 | 503 | else |
zhjcpi | 0:aba381fc8158 | 504 | { |
zhjcpi | 0:aba381fc8158 | 505 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 506 | } |
zhjcpi | 0:aba381fc8158 | 507 | } |
zhjcpi | 0:aba381fc8158 | 508 | else |
zhjcpi | 0:aba381fc8158 | 509 | { |
zhjcpi | 0:aba381fc8158 | 510 | pLocInst->protoDetState = STATE_END1; |
zhjcpi | 0:aba381fc8158 | 511 | } |
zhjcpi | 0:aba381fc8158 | 512 | break; |
zhjcpi | 0:aba381fc8158 | 513 | |
zhjcpi | 0:aba381fc8158 | 514 | case STATE_END1: |
zhjcpi | 0:aba381fc8158 | 515 | if (OSP_MSG_TAIL0 == data) |
zhjcpi | 0:aba381fc8158 | 516 | { |
zhjcpi | 0:aba381fc8158 | 517 | pLocInst->protoDetState = STATE_END2; |
zhjcpi | 0:aba381fc8158 | 518 | } |
zhjcpi | 0:aba381fc8158 | 519 | else |
zhjcpi | 0:aba381fc8158 | 520 | { |
zhjcpi | 0:aba381fc8158 | 521 | if (OSP_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 522 | { |
zhjcpi | 0:aba381fc8158 | 523 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 524 | } |
zhjcpi | 0:aba381fc8158 | 525 | else |
zhjcpi | 0:aba381fc8158 | 526 | { |
zhjcpi | 0:aba381fc8158 | 527 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 528 | } |
zhjcpi | 0:aba381fc8158 | 529 | } |
zhjcpi | 0:aba381fc8158 | 530 | break; |
zhjcpi | 0:aba381fc8158 | 531 | |
zhjcpi | 0:aba381fc8158 | 532 | case STATE_END2: |
zhjcpi | 0:aba381fc8158 | 533 | if (OSP_MSG_TAIL1 == data) |
zhjcpi | 0:aba381fc8158 | 534 | { |
zhjcpi | 0:aba381fc8158 | 535 | pLocInst->pTimeoutChk->detach(); |
zhjcpi | 2:d4fe184925f2 | 536 | pLocInst->bTimeoutFlag = false; |
zhjcpi | 3:71690f7bb480 | 537 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 538 | |
zhjcpi | 3:71690f7bb480 | 539 | if(pLocInst->proto == PROTO_NMEA) |
zhjcpi | 0:aba381fc8158 | 540 | { |
zhjcpi | 3:71690f7bb480 | 541 | pLocInst->pSerialLoc->attach(NULL); |
zhjcpi | 3:71690f7bb480 | 542 | |
zhjcpi | 3:71690f7bb480 | 543 | CSR_LOG_INFO("Checking OSP protocol OK, switching to NMEA...\r\n"); |
zhjcpi | 3:71690f7bb480 | 544 | _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ); |
zhjcpi | 3:71690f7bb480 | 545 | wait_ms(100); |
zhjcpi | 3:71690f7bb480 | 546 | pLocInst->protoState = PROTO_STATE_DET_NMEA_FROM_OSP; |
zhjcpi | 3:71690f7bb480 | 547 | pLocInst->baudRate = BAUDRATE_NMEA; |
zhjcpi | 3:71690f7bb480 | 548 | CSR_LOG_INFO("Checking switched NMEA protocol...\r\n"); |
zhjcpi | 3:71690f7bb480 | 549 | _CsrLocUartInit(); |
zhjcpi | 3:71690f7bb480 | 550 | |
zhjcpi | 0:aba381fc8158 | 551 | } |
zhjcpi | 3:71690f7bb480 | 552 | else |
zhjcpi | 3:71690f7bb480 | 553 | { |
zhjcpi | 3:71690f7bb480 | 554 | if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA) |
zhjcpi | 3:71690f7bb480 | 555 | { |
zhjcpi | 4:0d9b711fb646 | 556 | if(pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA) |
zhjcpi | 4:0d9b711fb646 | 557 | { |
zhjcpi | 4:0d9b711fb646 | 558 | if(!csrLocInst.bPwrModeRsp) |
zhjcpi | 4:0d9b711fb646 | 559 | { |
zhjcpi | 4:0d9b711fb646 | 560 | if(csrLocInst.pwrMode == PWR_PTF) |
zhjcpi | 4:0d9b711fb646 | 561 | { |
zhjcpi | 4:0d9b711fb646 | 562 | _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ); |
zhjcpi | 4:0d9b711fb646 | 563 | } |
zhjcpi | 4:0d9b711fb646 | 564 | else |
zhjcpi | 4:0d9b711fb646 | 565 | { |
zhjcpi | 4:0d9b711fb646 | 566 | _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ); |
zhjcpi | 4:0d9b711fb646 | 567 | } |
zhjcpi | 4:0d9b711fb646 | 568 | } |
zhjcpi | 4:0d9b711fb646 | 569 | } |
zhjcpi | 3:71690f7bb480 | 570 | CSR_LOG_INFO("Checking OSP protocol OK.\r\n"); |
zhjcpi | 3:71690f7bb480 | 571 | pLocInst->protoState = PROTO_STATE_DET_OK; |
zhjcpi | 3:71690f7bb480 | 572 | pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0); |
zhjcpi | 3:71690f7bb480 | 573 | } |
zhjcpi | 3:71690f7bb480 | 574 | |
zhjcpi | 3:71690f7bb480 | 575 | _CsrLocProcessRawOspPkt(); |
zhjcpi | 3:71690f7bb480 | 576 | } |
zhjcpi | 0:aba381fc8158 | 577 | } |
zhjcpi | 0:aba381fc8158 | 578 | else |
zhjcpi | 0:aba381fc8158 | 579 | { |
zhjcpi | 0:aba381fc8158 | 580 | if (OSP_MSG_HEAD0 == data) |
zhjcpi | 0:aba381fc8158 | 581 | { |
zhjcpi | 0:aba381fc8158 | 582 | pLocInst->protoDetState = STATE_START2; |
zhjcpi | 0:aba381fc8158 | 583 | } |
zhjcpi | 0:aba381fc8158 | 584 | else |
zhjcpi | 0:aba381fc8158 | 585 | { |
zhjcpi | 0:aba381fc8158 | 586 | pLocInst->protoDetState = STATE_START1; |
zhjcpi | 0:aba381fc8158 | 587 | } |
zhjcpi | 0:aba381fc8158 | 588 | } |
zhjcpi | 0:aba381fc8158 | 589 | break; |
zhjcpi | 0:aba381fc8158 | 590 | } /* switch. */ |
zhjcpi | 0:aba381fc8158 | 591 | |
zhjcpi | 0:aba381fc8158 | 592 | } |
zhjcpi | 0:aba381fc8158 | 593 | |
zhjcpi | 3:71690f7bb480 | 594 | void CsrLocation::_CsrLocProcessRawNmeaPkt(void) |
zhjcpi | 3:71690f7bb480 | 595 | { |
zhjcpi | 3:71690f7bb480 | 596 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 3:71690f7bb480 | 597 | tLocPosResp pos; |
zhjcpi | 3:71690f7bb480 | 598 | const char *pNmeaGga = "GPGGA"; |
zhjcpi | 3:71690f7bb480 | 599 | float deg, min; |
zhjcpi | 3:71690f7bb480 | 600 | char ns, ew; |
zhjcpi | 3:71690f7bb480 | 601 | int svUsed; |
zhjcpi | 3:71690f7bb480 | 602 | float horDop; |
zhjcpi | 3:71690f7bb480 | 603 | int valid; |
zhjcpi | 3:71690f7bb480 | 604 | int i, cnt; |
zhjcpi | 3:71690f7bb480 | 605 | |
zhjcpi | 3:71690f7bb480 | 606 | if(strncmp((char *)pLocInst->serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) |
zhjcpi | 3:71690f7bb480 | 607 | { |
zhjcpi | 3:71690f7bb480 | 608 | cnt = 0; |
zhjcpi | 3:71690f7bb480 | 609 | for(i = 0; i < (int)strlen((char *)pLocInst->serialPkt); i++) |
zhjcpi | 3:71690f7bb480 | 610 | { |
zhjcpi | 3:71690f7bb480 | 611 | if(pLocInst->serialPkt[i] == ',') |
zhjcpi | 3:71690f7bb480 | 612 | { |
zhjcpi | 3:71690f7bb480 | 613 | cnt++; |
zhjcpi | 3:71690f7bb480 | 614 | if(cnt == 6) |
zhjcpi | 3:71690f7bb480 | 615 | { |
zhjcpi | 3:71690f7bb480 | 616 | break; |
zhjcpi | 3:71690f7bb480 | 617 | } |
zhjcpi | 3:71690f7bb480 | 618 | } |
zhjcpi | 3:71690f7bb480 | 619 | } |
zhjcpi | 3:71690f7bb480 | 620 | if(cnt != 6) |
zhjcpi | 3:71690f7bb480 | 621 | { |
zhjcpi | 3:71690f7bb480 | 622 | return; |
zhjcpi | 3:71690f7bb480 | 623 | } |
zhjcpi | 3:71690f7bb480 | 624 | i++; |
zhjcpi | 3:71690f7bb480 | 625 | sscanf((char *)(&pLocInst->serialPkt[i]), "%d,", &valid); |
zhjcpi | 3:71690f7bb480 | 626 | if(valid == 0) |
zhjcpi | 3:71690f7bb480 | 627 | { |
zhjcpi | 3:71690f7bb480 | 628 | return; |
zhjcpi | 3:71690f7bb480 | 629 | } |
zhjcpi | 3:71690f7bb480 | 630 | |
zhjcpi | 3:71690f7bb480 | 631 | /* Parse GPGGA and output position information */ |
zhjcpi | 3:71690f7bb480 | 632 | memset(&pos, 0, sizeof(tLocPosResp)); |
zhjcpi | 3:71690f7bb480 | 633 | 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) |
zhjcpi | 3:71690f7bb480 | 634 | { |
zhjcpi | 3:71690f7bb480 | 635 | if(ns == 'S') { pos.lat *= -1.0; } |
zhjcpi | 3:71690f7bb480 | 636 | if(ew == 'W') { pos.lon *= -1.0; } |
zhjcpi | 3:71690f7bb480 | 637 | deg = (float)(static_cast<int>(pos.lat * 0.01f)); |
zhjcpi | 3:71690f7bb480 | 638 | min = pos.lat - (deg * 100.0f); |
zhjcpi | 3:71690f7bb480 | 639 | pos.lat = deg + min / 60.0f; |
zhjcpi | 3:71690f7bb480 | 640 | deg = (float)(static_cast<int>(pos.lon * 0.01f)); |
zhjcpi | 3:71690f7bb480 | 641 | min = pos.lon - (deg * 100.0f); |
zhjcpi | 3:71690f7bb480 | 642 | pos.lon = deg + min / 60.0f; |
zhjcpi | 3:71690f7bb480 | 643 | |
zhjcpi | 3:71690f7bb480 | 644 | csrLocInst.appOutCb(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp)); |
zhjcpi | 3:71690f7bb480 | 645 | } |
zhjcpi | 3:71690f7bb480 | 646 | } |
zhjcpi | 3:71690f7bb480 | 647 | } |
zhjcpi | 3:71690f7bb480 | 648 | |
zhjcpi | 0:aba381fc8158 | 649 | void CsrLocation::_CsrLocProcessRawOspPkt(void) |
zhjcpi | 0:aba381fc8158 | 650 | { |
zhjcpi | 0:aba381fc8158 | 651 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 0:aba381fc8158 | 652 | tOspMsg *pOspMsg; |
zhjcpi | 2:d4fe184925f2 | 653 | uint32_t msgSize; |
zhjcpi | 0:aba381fc8158 | 654 | CsrResult result; |
zhjcpi | 0:aba381fc8158 | 655 | |
zhjcpi | 0:aba381fc8158 | 656 | msgSize = _CsrLocCalcMsgSize(); |
zhjcpi | 0:aba381fc8158 | 657 | if(msgSize > 0) |
zhjcpi | 0:aba381fc8158 | 658 | { |
zhjcpi | 0:aba381fc8158 | 659 | msgSize += sizeof(tOspMsg); |
zhjcpi | 0:aba381fc8158 | 660 | pOspMsg = (tOspMsg *)malloc(msgSize); |
zhjcpi | 0:aba381fc8158 | 661 | if(pOspMsg != NULL) |
zhjcpi | 0:aba381fc8158 | 662 | { |
zhjcpi | 0:aba381fc8158 | 663 | memset(pOspMsg, 0, msgSize); |
zhjcpi | 0:aba381fc8158 | 664 | } |
zhjcpi | 0:aba381fc8158 | 665 | else |
zhjcpi | 0:aba381fc8158 | 666 | { |
zhjcpi | 0:aba381fc8158 | 667 | CSR_LOG_INFO("No memory for received OSP message.\r\n"); |
zhjcpi | 0:aba381fc8158 | 668 | return; |
zhjcpi | 0:aba381fc8158 | 669 | } |
zhjcpi | 0:aba381fc8158 | 670 | } |
zhjcpi | 0:aba381fc8158 | 671 | else |
zhjcpi | 0:aba381fc8158 | 672 | { |
zhjcpi | 0:aba381fc8158 | 673 | /* discard the unprocessed message */ |
zhjcpi | 0:aba381fc8158 | 674 | return; |
zhjcpi | 0:aba381fc8158 | 675 | } |
zhjcpi | 0:aba381fc8158 | 676 | |
zhjcpi | 0:aba381fc8158 | 677 | result = _CsrLocDecodeOspPkt(pLocInst->serialPkt,pLocInst->decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length); |
zhjcpi | 0:aba381fc8158 | 678 | if ( CSR_RESULT_SUCCESS == result) |
zhjcpi | 0:aba381fc8158 | 679 | { |
zhjcpi | 0:aba381fc8158 | 680 | _CsrLocProcessOspPkt(pOspMsg); |
zhjcpi | 0:aba381fc8158 | 681 | } |
zhjcpi | 0:aba381fc8158 | 682 | |
zhjcpi | 0:aba381fc8158 | 683 | free(pOspMsg); |
zhjcpi | 0:aba381fc8158 | 684 | } |
zhjcpi | 0:aba381fc8158 | 685 | |
zhjcpi | 2:d4fe184925f2 | 686 | uint32_t CsrLocation::_CsrLocCalcMsgSize(void) |
zhjcpi | 0:aba381fc8158 | 687 | { |
zhjcpi | 0:aba381fc8158 | 688 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 2:d4fe184925f2 | 689 | uint8_t *ptr = pLocInst->serialPkt; |
zhjcpi | 2:d4fe184925f2 | 690 | uint32_t msgSize = 0; |
zhjcpi | 2:d4fe184925f2 | 691 | uint32_t msgId; |
zhjcpi | 2:d4fe184925f2 | 692 | uint8_t mid, sid = 0; |
zhjcpi | 0:aba381fc8158 | 693 | |
zhjcpi | 0:aba381fc8158 | 694 | mid = BINARY_IMPORT_UINT8(ptr); |
zhjcpi | 0:aba381fc8158 | 695 | msgId = OSP_MAKE_MSG_ID(mid,sid); |
zhjcpi | 0:aba381fc8158 | 696 | |
zhjcpi | 0:aba381fc8158 | 697 | if(OSP_MSG_PWR_MODE_RSP == msgId || OSP_MSG_MULTI_CONSTELLATION == msgId) |
zhjcpi | 0:aba381fc8158 | 698 | { |
zhjcpi | 0:aba381fc8158 | 699 | /* add the sub-id to the message id */ |
zhjcpi | 0:aba381fc8158 | 700 | sid = BINARY_IMPORT_UINT8(ptr); |
zhjcpi | 0:aba381fc8158 | 701 | msgId = OSP_MAKE_MSG_ID(mid,sid); |
zhjcpi | 0:aba381fc8158 | 702 | } |
zhjcpi | 0:aba381fc8158 | 703 | |
zhjcpi | 0:aba381fc8158 | 704 | switch(msgId) |
zhjcpi | 0:aba381fc8158 | 705 | { |
zhjcpi | 0:aba381fc8158 | 706 | case OSP_MSG_OK_TO_SEND: |
zhjcpi | 0:aba381fc8158 | 707 | case OSP_MSG_PWR_MODE_FPM_RSP: |
zhjcpi | 0:aba381fc8158 | 708 | case OSP_MSG_PWR_MODE_LPM_RSP: |
zhjcpi | 0:aba381fc8158 | 709 | case OSP_MSG_HW_CONFIG_REQ : |
zhjcpi | 2:d4fe184925f2 | 710 | msgSize = sizeof(uint8_t); |
zhjcpi | 0:aba381fc8158 | 711 | break; |
zhjcpi | 0:aba381fc8158 | 712 | case OSP_MSG_SW_VERSION : |
zhjcpi | 0:aba381fc8158 | 713 | msgSize = MAX_VERSION_LENGTH; |
zhjcpi | 0:aba381fc8158 | 714 | break; |
zhjcpi | 0:aba381fc8158 | 715 | case OSP_MSG_GEODETIC_NAVIGATION: |
zhjcpi | 0:aba381fc8158 | 716 | msgSize = sizeof(tLocPosResp); |
zhjcpi | 0:aba381fc8158 | 717 | break; |
zhjcpi | 0:aba381fc8158 | 718 | case OSP_MSG_GNSS_SAT_DATA: |
zhjcpi | 2:d4fe184925f2 | 719 | msgSize = sizeof(uint8_t); |
zhjcpi | 0:aba381fc8158 | 720 | break; |
zhjcpi | 0:aba381fc8158 | 721 | case OSP_MSG_GNSS_NAV_DATA: |
zhjcpi | 2:d4fe184925f2 | 722 | msgSize = sizeof(uint8_t); |
zhjcpi | 0:aba381fc8158 | 723 | break; |
zhjcpi | 0:aba381fc8158 | 724 | |
zhjcpi | 0:aba381fc8158 | 725 | default : |
zhjcpi | 0:aba381fc8158 | 726 | msgSize = 0; |
zhjcpi | 0:aba381fc8158 | 727 | break; |
zhjcpi | 0:aba381fc8158 | 728 | } |
zhjcpi | 0:aba381fc8158 | 729 | |
zhjcpi | 0:aba381fc8158 | 730 | return msgSize; |
zhjcpi | 0:aba381fc8158 | 731 | } |
zhjcpi | 0:aba381fc8158 | 732 | |
zhjcpi | 2:d4fe184925f2 | 733 | CsrResult CsrLocation::_CsrLocDecodeOspPkt( uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen) |
zhjcpi | 0:aba381fc8158 | 734 | { |
zhjcpi | 0:aba381fc8158 | 735 | CsrResult tRet = CSR_RESULT_SUCCESS; |
zhjcpi | 2:d4fe184925f2 | 736 | uint8_t *ptr = pPayload; |
zhjcpi | 2:d4fe184925f2 | 737 | uint32_t i; |
zhjcpi | 2:d4fe184925f2 | 738 | uint8_t mid, sid = 0; |
zhjcpi | 0:aba381fc8158 | 739 | |
zhjcpi | 0:aba381fc8158 | 740 | mid = BINARY_IMPORT_UINT8(ptr); |
zhjcpi | 0:aba381fc8158 | 741 | *pMsgId = OSP_MAKE_MSG_ID(mid,sid); |
zhjcpi | 0:aba381fc8158 | 742 | *pMsgLen = 0; |
zhjcpi | 0:aba381fc8158 | 743 | |
zhjcpi | 0:aba381fc8158 | 744 | /* add the sub-id to the message id */ |
zhjcpi | 0:aba381fc8158 | 745 | if (OSP_MSG_PWR_MODE_RSP == *pMsgId || OSP_MSG_MULTI_CONSTELLATION == *pMsgId) |
zhjcpi | 0:aba381fc8158 | 746 | { |
zhjcpi | 0:aba381fc8158 | 747 | sid = BINARY_IMPORT_UINT8(ptr); |
zhjcpi | 0:aba381fc8158 | 748 | *pMsgId = OSP_MAKE_MSG_ID(mid,sid); |
zhjcpi | 0:aba381fc8158 | 749 | } |
zhjcpi | 0:aba381fc8158 | 750 | |
zhjcpi | 0:aba381fc8158 | 751 | switch (*pMsgId) |
zhjcpi | 0:aba381fc8158 | 752 | { |
zhjcpi | 0:aba381fc8158 | 753 | case OSP_MSG_SW_VERSION: /* 0x06 */ |
zhjcpi | 0:aba381fc8158 | 754 | *pMsgLen = BINARY_IMPORT_UINT8(ptr); |
zhjcpi | 0:aba381fc8158 | 755 | ptr++; |
zhjcpi | 0:aba381fc8158 | 756 | if(*pMsgLen >= MAX_VERSION_LENGTH) |
zhjcpi | 0:aba381fc8158 | 757 | { |
zhjcpi | 0:aba381fc8158 | 758 | tRet = CSR_RESULT_FAILURE; |
zhjcpi | 0:aba381fc8158 | 759 | } |
zhjcpi | 0:aba381fc8158 | 760 | else |
zhjcpi | 0:aba381fc8158 | 761 | { |
zhjcpi | 0:aba381fc8158 | 762 | memcpy(pMsgData, ptr, *pMsgLen); |
zhjcpi | 0:aba381fc8158 | 763 | } |
zhjcpi | 0:aba381fc8158 | 764 | break; |
zhjcpi | 0:aba381fc8158 | 765 | |
zhjcpi | 0:aba381fc8158 | 766 | case OSP_MSG_OK_TO_SEND: /* 0x12 */ |
zhjcpi | 2:d4fe184925f2 | 767 | *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr); |
zhjcpi | 2:d4fe184925f2 | 768 | *pMsgLen = sizeof(uint8_t); |
zhjcpi | 0:aba381fc8158 | 769 | break; |
zhjcpi | 0:aba381fc8158 | 770 | |
zhjcpi | 0:aba381fc8158 | 771 | case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */ |
zhjcpi | 0:aba381fc8158 | 772 | { |
zhjcpi | 0:aba381fc8158 | 773 | tLocPosResp *pPos = (tLocPosResp*) pMsgData; |
zhjcpi | 2:d4fe184925f2 | 774 | uint16_t valid; |
zhjcpi | 0:aba381fc8158 | 775 | |
zhjcpi | 0:aba381fc8158 | 776 | valid = BINARY_IMPORT_UINT16(ptr); |
zhjcpi | 0:aba381fc8158 | 777 | if(valid != 0) |
zhjcpi | 0:aba381fc8158 | 778 | { |
zhjcpi | 0:aba381fc8158 | 779 | tRet = CSR_RESULT_FAILURE; |
zhjcpi | 0:aba381fc8158 | 780 | } |
zhjcpi | 0:aba381fc8158 | 781 | else |
zhjcpi | 0:aba381fc8158 | 782 | { |
zhjcpi | 0:aba381fc8158 | 783 | *pMsgLen = sizeof(*pPos); |
zhjcpi | 0:aba381fc8158 | 784 | |
zhjcpi | 0:aba381fc8158 | 785 | ptr += 2; |
zhjcpi | 3:71690f7bb480 | 786 | pPos->u.gpsTime.gps_week = BINARY_IMPORT_UINT16(ptr); |
zhjcpi | 3:71690f7bb480 | 787 | pPos->u.gpsTime.tow = BINARY_IMPORT_UINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 788 | ptr += 12; |
zhjcpi | 2:d4fe184925f2 | 789 | pPos->lat = (double)BINARY_IMPORT_SINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 790 | pPos->lat *= 1e-7; |
zhjcpi | 2:d4fe184925f2 | 791 | pPos->lon = (double)BINARY_IMPORT_SINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 792 | pPos->lon *= 1e-7; |
zhjcpi | 0:aba381fc8158 | 793 | ptr += 4; |
zhjcpi | 2:d4fe184925f2 | 794 | pPos->alt = (double)BINARY_IMPORT_SINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 795 | pPos->alt *= 1e-2; |
zhjcpi | 0:aba381fc8158 | 796 | } |
zhjcpi | 0:aba381fc8158 | 797 | break; |
zhjcpi | 0:aba381fc8158 | 798 | } |
zhjcpi | 0:aba381fc8158 | 799 | |
zhjcpi | 0:aba381fc8158 | 800 | case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */ |
zhjcpi | 0:aba381fc8158 | 801 | { |
zhjcpi | 0:aba381fc8158 | 802 | tLocSvStatus *pSvStatus = &csrLocInst.svStatus; |
zhjcpi | 0:aba381fc8158 | 803 | |
zhjcpi | 0:aba381fc8158 | 804 | *pMsgLen = sizeof(*pSvStatus); |
zhjcpi | 0:aba381fc8158 | 805 | |
zhjcpi | 0:aba381fc8158 | 806 | ptr += 100; |
zhjcpi | 0:aba381fc8158 | 807 | pSvStatus->svUsedInFixMask = BINARY_IMPORT_UINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 808 | pSvStatus->sbasSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 809 | pSvStatus->gloSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 810 | pSvStatus->qzssSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 811 | break; |
zhjcpi | 0:aba381fc8158 | 812 | } |
zhjcpi | 0:aba381fc8158 | 813 | |
zhjcpi | 0:aba381fc8158 | 814 | case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */ |
zhjcpi | 0:aba381fc8158 | 815 | { |
zhjcpi | 0:aba381fc8158 | 816 | tLocSvStatus *pSvStatus = &csrLocInst.svStatus; |
zhjcpi | 2:d4fe184925f2 | 817 | uint16_t week; |
zhjcpi | 2:d4fe184925f2 | 818 | uint32_t tow; |
zhjcpi | 2:d4fe184925f2 | 819 | uint32_t towSubMs; |
zhjcpi | 2:d4fe184925f2 | 820 | uint8_t info; |
zhjcpi | 2:d4fe184925f2 | 821 | int32_t nMsg = 0; |
zhjcpi | 2:d4fe184925f2 | 822 | uint16_t satInfo; |
zhjcpi | 2:d4fe184925f2 | 823 | uint16_t az; |
zhjcpi | 2:d4fe184925f2 | 824 | uint16_t el; |
zhjcpi | 2:d4fe184925f2 | 825 | uint16_t cno; |
zhjcpi | 2:d4fe184925f2 | 826 | uint8_t gnssType; |
zhjcpi | 2:d4fe184925f2 | 827 | uint16_t index = 0; |
zhjcpi | 0:aba381fc8158 | 828 | |
zhjcpi | 0:aba381fc8158 | 829 | *pMsgLen = sizeof(*pSvStatus); |
zhjcpi | 0:aba381fc8158 | 830 | |
zhjcpi | 0:aba381fc8158 | 831 | week = BINARY_IMPORT_UINT16(ptr); |
zhjcpi | 0:aba381fc8158 | 832 | tow = BINARY_IMPORT_UINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 833 | towSubMs = BINARY_IMPORT_UINT32(ptr); |
zhjcpi | 0:aba381fc8158 | 834 | ptr += 4; |
zhjcpi | 0:aba381fc8158 | 835 | info = BINARY_IMPORT_UINT8(ptr); |
zhjcpi | 0:aba381fc8158 | 836 | |
zhjcpi | 0:aba381fc8158 | 837 | nMsg = info & 0x0F; |
zhjcpi | 0:aba381fc8158 | 838 | if(nMsg == 1) |
zhjcpi | 0:aba381fc8158 | 839 | { |
zhjcpi | 0:aba381fc8158 | 840 | memset(pSvStatus, 0, sizeof(tLocSvStatus)); |
zhjcpi | 0:aba381fc8158 | 841 | pSvStatus->gps_week = week; |
zhjcpi | 0:aba381fc8158 | 842 | pSvStatus->tow = tow; |
zhjcpi | 0:aba381fc8158 | 843 | pSvStatus->tow_sub_ms = towSubMs; |
zhjcpi | 0:aba381fc8158 | 844 | } |
zhjcpi | 0:aba381fc8158 | 845 | |
zhjcpi | 0:aba381fc8158 | 846 | ptr++; |
zhjcpi | 0:aba381fc8158 | 847 | for (i = 0; i < GNSS_SAT_DATA_NUM_OF_SATS; i++) |
zhjcpi | 0:aba381fc8158 | 848 | { |
zhjcpi | 0:aba381fc8158 | 849 | satInfo = BINARY_IMPORT_UINT16(ptr); |
zhjcpi | 0:aba381fc8158 | 850 | az = BINARY_IMPORT_UINT16(ptr); |
zhjcpi | 0:aba381fc8158 | 851 | el = BINARY_IMPORT_UINT16(ptr); |
zhjcpi | 0:aba381fc8158 | 852 | cno = BINARY_IMPORT_UINT16(ptr); |
zhjcpi | 0:aba381fc8158 | 853 | ptr += 4; |
zhjcpi | 0:aba381fc8158 | 854 | |
zhjcpi | 2:d4fe184925f2 | 855 | gnssType = (uint8_t)((satInfo>>13)&0x0003); |
zhjcpi | 0:aba381fc8158 | 856 | if(0 == gnssType || 1 == gnssType) // GPS, SBAS, QZSS |
zhjcpi | 0:aba381fc8158 | 857 | { |
zhjcpi | 0:aba381fc8158 | 858 | index = pSvStatus->numOfSVs; |
zhjcpi | 0:aba381fc8158 | 859 | if(index < LOC_MAX_GNSS_SVS && cno >0) |
zhjcpi | 0:aba381fc8158 | 860 | { |
zhjcpi | 2:d4fe184925f2 | 861 | pSvStatus->svList[index].prn = (uint8_t)(satInfo & 0xFF); |
zhjcpi | 2:d4fe184925f2 | 862 | pSvStatus->svList[index].cno = (float)(cno/10.0); // Scale: 10 |
zhjcpi | 2:d4fe184925f2 | 863 | pSvStatus->svList[index].elevation = (float)(el/10.0); // Scale: 10 |
zhjcpi | 2:d4fe184925f2 | 864 | pSvStatus->svList[index].azimuth = (float)(az/10.0); // Scale: 10 |
zhjcpi | 0:aba381fc8158 | 865 | pSvStatus->numOfSVs++; |
zhjcpi | 0:aba381fc8158 | 866 | pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn-1); // prn range: 1-32 |
zhjcpi | 0:aba381fc8158 | 867 | } |
zhjcpi | 0:aba381fc8158 | 868 | } |
zhjcpi | 0:aba381fc8158 | 869 | else if(2 == gnssType) // GLONASS |
zhjcpi | 0:aba381fc8158 | 870 | { |
zhjcpi | 0:aba381fc8158 | 871 | index = pSvStatus->numOfGloSVs; |
zhjcpi | 0:aba381fc8158 | 872 | if(index < CODEC_GLO_MAX_CHANNELS && cno>0) |
zhjcpi | 0:aba381fc8158 | 873 | { |
zhjcpi | 2:d4fe184925f2 | 874 | int16_t freqChan = (satInfo & 0X1F00)>>8; |
zhjcpi | 2:d4fe184925f2 | 875 | int16_t slotNum = (satInfo & 0X00FF); |
zhjcpi | 0:aba381fc8158 | 876 | if(slotNum > 0) |
zhjcpi | 0:aba381fc8158 | 877 | { |
zhjcpi | 0:aba381fc8158 | 878 | if(freqChan & 0X0010) |
zhjcpi | 0:aba381fc8158 | 879 | { |
zhjcpi | 0:aba381fc8158 | 880 | freqChan |= 0xFFE0; |
zhjcpi | 0:aba381fc8158 | 881 | } |
zhjcpi | 2:d4fe184925f2 | 882 | pSvStatus->gloSvList[index].prn = (uint8_t)(freqChan + LOC_GLO_FREQ_OFFSET); |
zhjcpi | 2:d4fe184925f2 | 883 | pSvStatus->gloSvList[index].sno = (uint8_t)slotNum; |
zhjcpi | 2:d4fe184925f2 | 884 | pSvStatus->gloSvList[index].cno = (float)(cno/10.0); // Scale: 10 |
zhjcpi | 2:d4fe184925f2 | 885 | pSvStatus->gloSvList[index].elevation = (float)(el/10.0); // Scale: 10 |
zhjcpi | 2:d4fe184925f2 | 886 | pSvStatus->gloSvList[index].azimuth = (float)(az/10.0); // Scale: 10 |
zhjcpi | 0:aba381fc8158 | 887 | pSvStatus->numOfGloSVs++; |
zhjcpi | 0:aba381fc8158 | 888 | pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn-LOC_GLO_FREQ_ID_START); |
zhjcpi | 0:aba381fc8158 | 889 | } |
zhjcpi | 0:aba381fc8158 | 890 | } |
zhjcpi | 0:aba381fc8158 | 891 | } |
zhjcpi | 0:aba381fc8158 | 892 | } |
zhjcpi | 0:aba381fc8158 | 893 | |
zhjcpi | 0:aba381fc8158 | 894 | break; |
zhjcpi | 0:aba381fc8158 | 895 | } |
zhjcpi | 0:aba381fc8158 | 896 | |
zhjcpi | 0:aba381fc8158 | 897 | case OSP_MSG_HW_CONFIG_REQ: /* 0x47 */ |
zhjcpi | 0:aba381fc8158 | 898 | break; |
zhjcpi | 0:aba381fc8158 | 899 | |
zhjcpi | 0:aba381fc8158 | 900 | case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */ |
zhjcpi | 0:aba381fc8158 | 901 | break; |
zhjcpi | 0:aba381fc8158 | 902 | |
zhjcpi | 0:aba381fc8158 | 903 | case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */ |
zhjcpi | 2:d4fe184925f2 | 904 | *((uint8_t *)pMsgData) = *ptr; |
zhjcpi | 2:d4fe184925f2 | 905 | *pMsgLen = sizeof(uint8_t); |
zhjcpi | 0:aba381fc8158 | 906 | break; |
zhjcpi | 0:aba381fc8158 | 907 | |
zhjcpi | 0:aba381fc8158 | 908 | default: |
zhjcpi | 0:aba381fc8158 | 909 | tRet = CSR_RESULT_FAILURE; |
zhjcpi | 0:aba381fc8158 | 910 | break; |
zhjcpi | 0:aba381fc8158 | 911 | } |
zhjcpi | 0:aba381fc8158 | 912 | |
zhjcpi | 0:aba381fc8158 | 913 | /* check if length does not match */ |
zhjcpi | 0:aba381fc8158 | 914 | if(tRet == CSR_RESULT_FAILURE) |
zhjcpi | 0:aba381fc8158 | 915 | { |
zhjcpi | 0:aba381fc8158 | 916 | *pMsgId = *pMsgLen = 0; |
zhjcpi | 0:aba381fc8158 | 917 | } |
zhjcpi | 0:aba381fc8158 | 918 | |
zhjcpi | 0:aba381fc8158 | 919 | return tRet; |
zhjcpi | 0:aba381fc8158 | 920 | |
zhjcpi | 0:aba381fc8158 | 921 | } /* CsrUlocCodecSsbDecode() */ |
zhjcpi | 0:aba381fc8158 | 922 | |
zhjcpi | 0:aba381fc8158 | 923 | void CsrLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg) |
zhjcpi | 0:aba381fc8158 | 924 | { |
zhjcpi | 0:aba381fc8158 | 925 | switch(pOspMsg->msgId) |
zhjcpi | 0:aba381fc8158 | 926 | { |
zhjcpi | 0:aba381fc8158 | 927 | case OSP_MSG_GEODETIC_NAVIGATION: |
zhjcpi | 0:aba381fc8158 | 928 | csrLocInst.appOutCb(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp)); |
zhjcpi | 0:aba381fc8158 | 929 | break; |
zhjcpi | 0:aba381fc8158 | 930 | case OSP_MSG_GNSS_SAT_DATA: |
zhjcpi | 0:aba381fc8158 | 931 | break; |
zhjcpi | 0:aba381fc8158 | 932 | case OSP_MSG_GNSS_NAV_DATA: |
zhjcpi | 0:aba381fc8158 | 933 | csrLocInst.appOutCb(LOC_OUTPUT_SV_STATUS, &csrLocInst.svStatus, sizeof(tLocSvStatus)); |
zhjcpi | 0:aba381fc8158 | 934 | break; |
zhjcpi | 0:aba381fc8158 | 935 | case OSP_MSG_OK_TO_SEND: |
zhjcpi | 0:aba381fc8158 | 936 | csrLocInst.engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND; |
zhjcpi | 0:aba381fc8158 | 937 | CSR_LOG_INFO("Ok to send %u\r\n", csrLocInst.engStatus); |
zhjcpi | 0:aba381fc8158 | 938 | break; |
zhjcpi | 0:aba381fc8158 | 939 | case OSP_MSG_SW_VERSION: |
zhjcpi | 2:d4fe184925f2 | 940 | csrLocInst.bVerRsp = true; |
zhjcpi | 0:aba381fc8158 | 941 | CSR_LOG_INFO("Ver: %s\r\n", pOspMsg->payload); |
zhjcpi | 0:aba381fc8158 | 942 | break; |
zhjcpi | 0:aba381fc8158 | 943 | case OSP_MSG_HW_CONFIG_REQ: |
zhjcpi | 0:aba381fc8158 | 944 | CSR_LOG_INFO("hw config req.\r\n"); |
zhjcpi | 2:d4fe184925f2 | 945 | if(!csrLocInst.bVerRsp) |
zhjcpi | 1:bbaf9b8d646a | 946 | { |
zhjcpi | 0:aba381fc8158 | 947 | _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ); |
zhjcpi | 0:aba381fc8158 | 948 | } |
zhjcpi | 1:bbaf9b8d646a | 949 | |
zhjcpi | 1:bbaf9b8d646a | 950 | if(!csrLocInst.bPwrModeRsp) |
zhjcpi | 0:aba381fc8158 | 951 | { |
zhjcpi | 1:bbaf9b8d646a | 952 | if(csrLocInst.pwrMode == PWR_PTF) |
zhjcpi | 1:bbaf9b8d646a | 953 | { |
zhjcpi | 1:bbaf9b8d646a | 954 | _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ); |
zhjcpi | 1:bbaf9b8d646a | 955 | } |
zhjcpi | 1:bbaf9b8d646a | 956 | else |
zhjcpi | 1:bbaf9b8d646a | 957 | { |
zhjcpi | 1:bbaf9b8d646a | 958 | _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ); |
zhjcpi | 1:bbaf9b8d646a | 959 | } |
zhjcpi | 0:aba381fc8158 | 960 | } |
zhjcpi | 0:aba381fc8158 | 961 | break; |
zhjcpi | 0:aba381fc8158 | 962 | case OSP_MSG_PWR_MODE_LPM_RSP: |
zhjcpi | 2:d4fe184925f2 | 963 | csrLocInst.bPwrModeRsp = true; |
zhjcpi | 0:aba381fc8158 | 964 | CSR_LOG_INFO("lpm response.\r\n"); |
zhjcpi | 0:aba381fc8158 | 965 | break; |
zhjcpi | 0:aba381fc8158 | 966 | case OSP_MSG_PWR_MODE_FPM_RSP: |
zhjcpi | 2:d4fe184925f2 | 967 | csrLocInst.bPwrModeRsp = true; |
zhjcpi | 0:aba381fc8158 | 968 | CSR_LOG_INFO("fpm response.\r\n"); |
zhjcpi | 0:aba381fc8158 | 969 | break; |
zhjcpi | 0:aba381fc8158 | 970 | default: |
zhjcpi | 0:aba381fc8158 | 971 | CSR_LOG_INFO("Unknown OSP message 0x%lx.\r\n", pOspMsg->msgId); |
zhjcpi | 0:aba381fc8158 | 972 | break; |
zhjcpi | 0:aba381fc8158 | 973 | } |
zhjcpi | 0:aba381fc8158 | 974 | } |
zhjcpi | 0:aba381fc8158 | 975 | |
zhjcpi | 0:aba381fc8158 | 976 | void CsrLocation::_CsrLocTimeout(void) |
zhjcpi | 0:aba381fc8158 | 977 | { |
zhjcpi | 2:d4fe184925f2 | 978 | csrLocInst.bTimeoutFlag = true; |
zhjcpi | 0:aba381fc8158 | 979 | } |
zhjcpi | 0:aba381fc8158 | 980 | |
zhjcpi | 0:aba381fc8158 | 981 | void CsrLocation::_CsrLocRxHandler(void) |
zhjcpi | 0:aba381fc8158 | 982 | { |
zhjcpi | 0:aba381fc8158 | 983 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 0:aba381fc8158 | 984 | |
zhjcpi | 0:aba381fc8158 | 985 | pLocInst->serialBuf[pLocInst->in++] = pLocInst->pSerialLoc->getc(); |
zhjcpi | 0:aba381fc8158 | 986 | pLocInst->in &= (MAX_SERIAL_BUF_LEN-1); |
zhjcpi | 0:aba381fc8158 | 987 | if(pLocInst->in == pLocInst->out) |
zhjcpi | 0:aba381fc8158 | 988 | { |
zhjcpi | 3:71690f7bb480 | 989 | CSR_LOG_INFO("rx overwritten %lu %lu.\r\n", pLocInst->in, pLocInst->out); |
zhjcpi | 0:aba381fc8158 | 990 | } |
zhjcpi | 0:aba381fc8158 | 991 | } |
zhjcpi | 0:aba381fc8158 | 992 | |
zhjcpi | 0:aba381fc8158 | 993 | void CsrLocation::_CsrLocSendData(eSendDataType type) |
zhjcpi | 0:aba381fc8158 | 994 | { |
zhjcpi | 0:aba381fc8158 | 995 | tCsrLocInst *pLocInst = &csrLocInst; |
zhjcpi | 2:d4fe184925f2 | 996 | uint32_t i, size; |
zhjcpi | 2:d4fe184925f2 | 997 | const uint8_t *pData; |
zhjcpi | 0:aba381fc8158 | 998 | |
zhjcpi | 0:aba381fc8158 | 999 | switch(type) |
zhjcpi | 0:aba381fc8158 | 1000 | { |
zhjcpi | 0:aba381fc8158 | 1001 | case SEND_DATA_TYPE_OSP_STOP_REQ: |
zhjcpi | 0:aba381fc8158 | 1002 | pData = sOspStopReq; |
zhjcpi | 0:aba381fc8158 | 1003 | size = sizeof(sOspStopReq); |
zhjcpi | 0:aba381fc8158 | 1004 | break; |
zhjcpi | 0:aba381fc8158 | 1005 | case SEND_DATA_TYPE_OSP_VER_REQ: |
zhjcpi | 0:aba381fc8158 | 1006 | pData = sOspVerReq; |
zhjcpi | 0:aba381fc8158 | 1007 | size = sizeof(sOspVerReq); |
zhjcpi | 0:aba381fc8158 | 1008 | break; |
zhjcpi | 0:aba381fc8158 | 1009 | case SEND_DATA_TYPE_OSP_LPM_REQ: |
zhjcpi | 0:aba381fc8158 | 1010 | pData = sOspLpmReq; |
zhjcpi | 0:aba381fc8158 | 1011 | size = sizeof(sOspLpmReq); |
zhjcpi | 0:aba381fc8158 | 1012 | break; |
zhjcpi | 0:aba381fc8158 | 1013 | case SEND_DATA_TYPE_OSP_FPM_REQ: |
zhjcpi | 0:aba381fc8158 | 1014 | pData = sOspFpmReq; |
zhjcpi | 0:aba381fc8158 | 1015 | size = sizeof(sOspFpmReq); |
zhjcpi | 0:aba381fc8158 | 1016 | break; |
zhjcpi | 0:aba381fc8158 | 1017 | case SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ: |
zhjcpi | 0:aba381fc8158 | 1018 | pData = sOspSwitch2NmeaReq; |
zhjcpi | 0:aba381fc8158 | 1019 | size = sizeof(sOspSwitch2NmeaReq); |
zhjcpi | 0:aba381fc8158 | 1020 | break; |
zhjcpi | 0:aba381fc8158 | 1021 | case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ: |
zhjcpi | 2:d4fe184925f2 | 1022 | pData = (const uint8_t *)sNmeaSwitch2OspReq; |
zhjcpi | 0:aba381fc8158 | 1023 | size = strlen(sNmeaSwitch2OspReq); |
zhjcpi | 0:aba381fc8158 | 1024 | break; |
zhjcpi | 3:71690f7bb480 | 1025 | case SEND_DATA_TYPE_NMEA_STOP_REQ: |
zhjcpi | 3:71690f7bb480 | 1026 | pData = (const uint8_t *)sNmeaStopReq; |
zhjcpi | 3:71690f7bb480 | 1027 | size = strlen(sNmeaStopReq); |
zhjcpi | 3:71690f7bb480 | 1028 | break; |
zhjcpi | 3:71690f7bb480 | 1029 | |
zhjcpi | 0:aba381fc8158 | 1030 | default: |
zhjcpi | 0:aba381fc8158 | 1031 | pData = NULL; |
zhjcpi | 0:aba381fc8158 | 1032 | } |
zhjcpi | 0:aba381fc8158 | 1033 | |
zhjcpi | 0:aba381fc8158 | 1034 | if(pData != NULL) |
zhjcpi | 0:aba381fc8158 | 1035 | { |
zhjcpi | 0:aba381fc8158 | 1036 | for (i = 0; i < size; i++) |
zhjcpi | 0:aba381fc8158 | 1037 | { |
zhjcpi | 0:aba381fc8158 | 1038 | pLocInst->pSerialLoc->putc(pData[i]); |
zhjcpi | 0:aba381fc8158 | 1039 | } |
zhjcpi | 0:aba381fc8158 | 1040 | } |
zhjcpi | 0:aba381fc8158 | 1041 | } |
zhjcpi | 0:aba381fc8158 | 1042 | |
zhjcpi | 0:aba381fc8158 | 1043 | void CsrLocation::_CsrLocHwOnoff(void) |
zhjcpi | 0:aba381fc8158 | 1044 | { |
zhjcpi | 0:aba381fc8158 | 1045 | csrLocInst.pPinOnoff->write(1); |
zhjcpi | 0:aba381fc8158 | 1046 | wait_ms(10); |
zhjcpi | 0:aba381fc8158 | 1047 | csrLocInst.pPinOnoff->write(0); |
zhjcpi | 0:aba381fc8158 | 1048 | CSR_LOG_INFO("Onoff pulse given.\r\n"); |
zhjcpi | 0:aba381fc8158 | 1049 | } |
zhjcpi | 0:aba381fc8158 | 1050 | |
zhjcpi | 0:aba381fc8158 | 1051 | void CsrLocation::_CsrLocHwReset(void) |
zhjcpi | 0:aba381fc8158 | 1052 | { |
zhjcpi | 0:aba381fc8158 | 1053 | csrLocInst.pPinReset->write(1); |
zhjcpi | 0:aba381fc8158 | 1054 | wait_ms(10); |
zhjcpi | 0:aba381fc8158 | 1055 | csrLocInst.pPinReset->write(0); |
zhjcpi | 0:aba381fc8158 | 1056 | CSR_LOG_INFO("Reset pulse given.\r\n"); |
zhjcpi | 0:aba381fc8158 | 1057 | } |