Qualcomm Technologies International, Ltd. / CsrLocation

Csr location class shows location and satellite information, which supports H13467 + ST F103RB/NXP LCP1549 boards now.

Dependents:   CsrLocationDemo CsrLocationDemo

Fork of CsrLocation by jie zhao

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?

UserRevisionLine numberNew 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 }