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@14:bb86a6ca6662, 2014-11-05 (annotated)

Committer:
zhjcpi
Date:
Wed Nov 05 02:26:49 2014 +0000
Revision:
14:bb86a6ca6662
Parent:
12:5fb829ce6b82
Child:
15:ee388b77d730
Child:
17:05033198f8f3
- Fix the sleep issue by processing all the received UART data in one process procedure

; - Extend the static buffer size in _CsrLocProcessRawOspPkt() from 256 to 512 in case overflow when we add more OSP messages in the future

;

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