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@18:5d72465991f5, 2014-11-18 (annotated)

Committer:
zhjcpi
Date:
Tue Nov 18 09:03:17 2014 +0000
Revision:
18:5d72465991f5
Parent:
17:05033198f8f3
Child:
20:88db82cf3f0f
define utcTime as uint_64_t instead of float.

Who changed what in which revision?

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