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@8:fa29f8455134, 2014-08-11 (annotated)

Committer:
zhjcpi
Date:
Mon Aug 11 03:23:23 2014 +0000
Revision:
8:fa29f8455134
Parent:
7:1fde78f27d5b
Child:
12:5fb829ce6b82
Add wakeup pin input process.

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