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@7:1fde78f27d5b, 2014-08-05 (annotated)

Committer:
zhjcpi
Date:
Tue Aug 05 06:22:16 2014 +0000
Revision:
7:1fde78f27d5b
Parent:
6:aed3c66b39d9
Child:
8:fa29f8455134
Add button support to triger position report in PTF mode.

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