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

Committer:
rgrover1
Date:
Wed Nov 05 13:23:21 2014 +0000
Revision:
15:ee388b77d730
Parent:
14:bb86a6ca6662
Child:
16:37d3ea06cbb4
mostly white space diffs.

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;
rgrover1 15:ee388b77d730 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 }
rgrover1 15:ee388b77d730 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
rgrover1 15:ee388b77d730 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 if (strncmp((char *)serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) {
zhjcpi 3:71690f7bb480 588 cnt = 0;
zhjcpi 12:5fb829ce6b82 589 for (i = 0; i < (int)strlen((char *)serialPkt); i++) {
zhjcpi 12:5fb829ce6b82 590 if (serialPkt[i] == ',') {
zhjcpi 3:71690f7bb480 591 cnt++;
zhjcpi 12:5fb829ce6b82 592 if (cnt == 6) {
zhjcpi 3:71690f7bb480 593 break;
zhjcpi 3:71690f7bb480 594 }
zhjcpi 3:71690f7bb480 595 }
zhjcpi 3:71690f7bb480 596 }
zhjcpi 12:5fb829ce6b82 597 if (cnt != 6) {
zhjcpi 3:71690f7bb480 598 return;
zhjcpi 3:71690f7bb480 599 }
zhjcpi 3:71690f7bb480 600 i++;
zhjcpi 12:5fb829ce6b82 601 sscanf((char *)(&serialPkt[i]), "%d,", &valid);
zhjcpi 12:5fb829ce6b82 602 if (valid == 0) {
zhjcpi 3:71690f7bb480 603 return;
zhjcpi 3:71690f7bb480 604 }
rgrover1 15:ee388b77d730 605
zhjcpi 3:71690f7bb480 606 /* Parse GPGGA and output position information */
zhjcpi 3:71690f7bb480 607 memset(&pos, 0, sizeof(tLocPosResp));
zhjcpi 12:5fb829ce6b82 608 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 609 &pos.alt) >= 1) {
zhjcpi 12:5fb829ce6b82 610 if (ns == 'S') {
zhjcpi 12:5fb829ce6b82 611 pos.lat *= -1.0;
zhjcpi 12:5fb829ce6b82 612 }
zhjcpi 12:5fb829ce6b82 613 if (ew == 'W') {
zhjcpi 12:5fb829ce6b82 614 pos.lon *= -1.0;
zhjcpi 12:5fb829ce6b82 615 }
zhjcpi 12:5fb829ce6b82 616 deg = (float)(static_cast<int>(pos.lat * 0.01f));
zhjcpi 12:5fb829ce6b82 617 min = pos.lat - (deg * 100.0f);
zhjcpi 12:5fb829ce6b82 618 pos.lat = deg + min / 60.0f;
zhjcpi 12:5fb829ce6b82 619 deg = (float)(static_cast<int>(pos.lon * 0.01f));
zhjcpi 12:5fb829ce6b82 620 min = pos.lon - (deg * 100.0f);
zhjcpi 12:5fb829ce6b82 621 pos.lon = deg + min / 60.0f;
zhjcpi 3:71690f7bb480 622
zhjcpi 12:5fb829ce6b82 623 outputHandler(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp));
zhjcpi 3:71690f7bb480 624 }
zhjcpi 3:71690f7bb480 625 }
zhjcpi 12:5fb829ce6b82 626 #endif
zhjcpi 3:71690f7bb480 627 }
zhjcpi 3:71690f7bb480 628
zhjcpi 12:5fb829ce6b82 629 void CSRLocation::_CsrLocProcessRawOspPkt(void)
zhjcpi 0:aba381fc8158 630 {
zhjcpi 12:5fb829ce6b82 631 tOspMsg *pOspMsg;
zhjcpi 12:5fb829ce6b82 632 uint32_t msgSize;
zhjcpi 12:5fb829ce6b82 633 CsrResult result;
zhjcpi 12:5fb829ce6b82 634
zhjcpi 14:bb86a6ca6662 635 static uint8_t buffer[512];
zhjcpi 12:5fb829ce6b82 636 static unsigned maxMessageSize = 0;
zhjcpi 12:5fb829ce6b82 637
zhjcpi 0:aba381fc8158 638 msgSize = _CsrLocCalcMsgSize();
zhjcpi 12:5fb829ce6b82 639 if (msgSize > 0) {
zhjcpi 0:aba381fc8158 640 msgSize += sizeof(tOspMsg);
zhjcpi 12:5fb829ce6b82 641 if (msgSize > maxMessageSize) {
zhjcpi 12:5fb829ce6b82 642 maxMessageSize = msgSize;
zhjcpi 12:5fb829ce6b82 643 CSR_LOG_INFO("max message size %u\r\n", maxMessageSize);
zhjcpi 0:aba381fc8158 644 }
zhjcpi 12:5fb829ce6b82 645 pOspMsg = (tOspMsg *)buffer;
zhjcpi 12:5fb829ce6b82 646 memset(pOspMsg, 0, msgSize);
zhjcpi 12:5fb829ce6b82 647 } else {
zhjcpi 0:aba381fc8158 648 /* discard the unprocessed message */
zhjcpi 0:aba381fc8158 649 return;
zhjcpi 0:aba381fc8158 650 }
zhjcpi 0:aba381fc8158 651
zhjcpi 12:5fb829ce6b82 652 result = _CsrLocDecodeOspPkt(serialPkt, decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length);
zhjcpi 12:5fb829ce6b82 653 if (CSR_RESULT_SUCCESS == result) {
zhjcpi 12:5fb829ce6b82 654 _CsrLocProcessOspPkt(pOspMsg);
zhjcpi 0:aba381fc8158 655 }
zhjcpi 0:aba381fc8158 656 }
zhjcpi 0:aba381fc8158 657
zhjcpi 12:5fb829ce6b82 658 uint32_t CSRLocation::_CsrLocCalcMsgSize(void)
zhjcpi 0:aba381fc8158 659 {
zhjcpi 12:5fb829ce6b82 660 uint8_t *ptr = serialPkt;
zhjcpi 12:5fb829ce6b82 661 uint32_t msgSize = 0;
zhjcpi 12:5fb829ce6b82 662 uint32_t msgId;
zhjcpi 12:5fb829ce6b82 663 uint8_t mid, sid = 0;
zhjcpi 0:aba381fc8158 664
zhjcpi 0:aba381fc8158 665 mid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 12:5fb829ce6b82 666 msgId = OSP_MAKE_MSG_ID(mid, sid);
zhjcpi 0:aba381fc8158 667
zhjcpi 12:5fb829ce6b82 668 if ((OSP_MSG_PWR_MODE_RSP == msgId) || (OSP_MSG_MULTI_CONSTELLATION == msgId)) {
zhjcpi 12:5fb829ce6b82 669 /* add the sub-id to the message id */
zhjcpi 0:aba381fc8158 670 sid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 12:5fb829ce6b82 671 msgId = OSP_MAKE_MSG_ID(mid, sid);
zhjcpi 0:aba381fc8158 672 }
zhjcpi 0:aba381fc8158 673
zhjcpi 12:5fb829ce6b82 674 switch (msgId) {
zhjcpi 12:5fb829ce6b82 675 case OSP_MSG_OK_TO_SEND:
zhjcpi 12:5fb829ce6b82 676 case OSP_MSG_PWR_MODE_FPM_RSP:
zhjcpi 12:5fb829ce6b82 677 case OSP_MSG_PWR_MODE_LPM_RSP:
zhjcpi 12:5fb829ce6b82 678 case OSP_MSG_HW_CONFIG_REQ:
zhjcpi 12:5fb829ce6b82 679 msgSize = sizeof(uint8_t);
zhjcpi 12:5fb829ce6b82 680 break;
zhjcpi 12:5fb829ce6b82 681 case OSP_MSG_SW_VERSION:
zhjcpi 12:5fb829ce6b82 682 msgSize = MAX_VERSION_LENGTH;
zhjcpi 12:5fb829ce6b82 683 break;
zhjcpi 12:5fb829ce6b82 684 case OSP_MSG_GEODETIC_NAVIGATION:
zhjcpi 12:5fb829ce6b82 685 msgSize = sizeof(tLocPosResp);
zhjcpi 12:5fb829ce6b82 686 break;
zhjcpi 12:5fb829ce6b82 687 case OSP_MSG_GNSS_SAT_DATA:
zhjcpi 12:5fb829ce6b82 688 msgSize = sizeof(uint8_t);
zhjcpi 12:5fb829ce6b82 689 break;
zhjcpi 12:5fb829ce6b82 690 case OSP_MSG_GNSS_NAV_DATA:
zhjcpi 12:5fb829ce6b82 691 msgSize = sizeof(uint8_t);
zhjcpi 12:5fb829ce6b82 692 break;
zhjcpi 0:aba381fc8158 693
zhjcpi 12:5fb829ce6b82 694 default:
zhjcpi 12:5fb829ce6b82 695 msgSize = 0;
zhjcpi 12:5fb829ce6b82 696 break;
zhjcpi 12:5fb829ce6b82 697 }
zhjcpi 0:aba381fc8158 698
zhjcpi 0:aba381fc8158 699 return msgSize;
zhjcpi 0:aba381fc8158 700 }
zhjcpi 0:aba381fc8158 701
zhjcpi 12:5fb829ce6b82 702 CsrResult CSRLocation::_CsrLocDecodeOspPkt(uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen)
zhjcpi 0:aba381fc8158 703 {
zhjcpi 0:aba381fc8158 704 CsrResult tRet = CSR_RESULT_SUCCESS;
zhjcpi 12:5fb829ce6b82 705 uint8_t *ptr = pPayload;
zhjcpi 12:5fb829ce6b82 706 uint32_t i;
zhjcpi 12:5fb829ce6b82 707 uint8_t mid, sid = 0;
zhjcpi 0:aba381fc8158 708
zhjcpi 12:5fb829ce6b82 709 mid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 12:5fb829ce6b82 710 *pMsgId = OSP_MAKE_MSG_ID(mid, sid);
zhjcpi 12:5fb829ce6b82 711 *pMsgLen = 0;
zhjcpi 0:aba381fc8158 712
zhjcpi 0:aba381fc8158 713 /* add the sub-id to the message id */
zhjcpi 12:5fb829ce6b82 714 if ((OSP_MSG_PWR_MODE_RSP == *pMsgId) || (OSP_MSG_MULTI_CONSTELLATION == *pMsgId)) {
zhjcpi 0:aba381fc8158 715 sid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 12:5fb829ce6b82 716 *pMsgId = OSP_MAKE_MSG_ID(mid, sid);
zhjcpi 0:aba381fc8158 717 }
zhjcpi 0:aba381fc8158 718
zhjcpi 12:5fb829ce6b82 719 switch (*pMsgId) {
zhjcpi 12:5fb829ce6b82 720 case OSP_MSG_SW_VERSION: /* 0x06 */
zhjcpi 12:5fb829ce6b82 721 *pMsgLen = BINARY_IMPORT_UINT8(ptr);
zhjcpi 12:5fb829ce6b82 722 ptr++;
zhjcpi 12:5fb829ce6b82 723 if (*pMsgLen >= MAX_VERSION_LENGTH) {
zhjcpi 12:5fb829ce6b82 724 tRet = CSR_RESULT_FAILURE;
zhjcpi 12:5fb829ce6b82 725 } else {
zhjcpi 12:5fb829ce6b82 726 memcpy(pMsgData, ptr, *pMsgLen);
zhjcpi 12:5fb829ce6b82 727 }
zhjcpi 12:5fb829ce6b82 728 break;
zhjcpi 12:5fb829ce6b82 729
zhjcpi 12:5fb829ce6b82 730 case OSP_MSG_OK_TO_SEND: /* 0x12 */
zhjcpi 12:5fb829ce6b82 731 *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr);
zhjcpi 12:5fb829ce6b82 732 *pMsgLen = sizeof(uint8_t);
zhjcpi 12:5fb829ce6b82 733 break;
zhjcpi 0:aba381fc8158 734
zhjcpi 12:5fb829ce6b82 735 case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */
zhjcpi 12:5fb829ce6b82 736 {
zhjcpi 12:5fb829ce6b82 737 tLocPosResp *pPos = (tLocPosResp *) pMsgData;
zhjcpi 12:5fb829ce6b82 738 uint16_t valid;
zhjcpi 12:5fb829ce6b82 739
zhjcpi 12:5fb829ce6b82 740 valid = BINARY_IMPORT_UINT16(ptr);
zhjcpi 12:5fb829ce6b82 741 if (valid != 0) {
zhjcpi 12:5fb829ce6b82 742 tRet = CSR_RESULT_FAILURE;
zhjcpi 12:5fb829ce6b82 743 } else {
zhjcpi 12:5fb829ce6b82 744 *pMsgLen = sizeof(*pPos);
zhjcpi 0:aba381fc8158 745
zhjcpi 12:5fb829ce6b82 746 ptr += 2;
zhjcpi 12:5fb829ce6b82 747 pPos->u.gpsTime.gps_week = BINARY_IMPORT_UINT16(ptr);
zhjcpi 12:5fb829ce6b82 748 pPos->u.gpsTime.tow = BINARY_IMPORT_UINT32(ptr);
zhjcpi 12:5fb829ce6b82 749 ptr += 12;
zhjcpi 12:5fb829ce6b82 750 pPos->lat = (double)BINARY_IMPORT_SINT32(ptr);
zhjcpi 12:5fb829ce6b82 751 pPos->lat *= 1e-7;
zhjcpi 12:5fb829ce6b82 752 pPos->lon = (double)BINARY_IMPORT_SINT32(ptr);
zhjcpi 12:5fb829ce6b82 753 pPos->lon *= 1e-7;
zhjcpi 12:5fb829ce6b82 754 ptr += 4;
zhjcpi 12:5fb829ce6b82 755 pPos->alt = (double)BINARY_IMPORT_SINT32(ptr);
zhjcpi 12:5fb829ce6b82 756 pPos->alt *= 1e-2;
zhjcpi 12:5fb829ce6b82 757 }
zhjcpi 12:5fb829ce6b82 758 break;
zhjcpi 0:aba381fc8158 759 }
zhjcpi 12:5fb829ce6b82 760
zhjcpi 12:5fb829ce6b82 761 case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */
zhjcpi 0:aba381fc8158 762 {
zhjcpi 12:5fb829ce6b82 763 tLocSvStatus *pSvStatus = &svStatus;
zhjcpi 12:5fb829ce6b82 764
zhjcpi 12:5fb829ce6b82 765 *pMsgLen = sizeof(*pSvStatus);
zhjcpi 0:aba381fc8158 766
zhjcpi 12:5fb829ce6b82 767 ptr += 100;
zhjcpi 12:5fb829ce6b82 768 pSvStatus->svUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 12:5fb829ce6b82 769 pSvStatus->sbasSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 12:5fb829ce6b82 770 pSvStatus->gloSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 12:5fb829ce6b82 771 pSvStatus->qzssSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 12:5fb829ce6b82 772 break;
zhjcpi 0:aba381fc8158 773 }
zhjcpi 0:aba381fc8158 774
zhjcpi 12:5fb829ce6b82 775 case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */
zhjcpi 0:aba381fc8158 776 {
zhjcpi 12:5fb829ce6b82 777 tLocSvStatus *pSvStatus = &svStatus;
zhjcpi 12:5fb829ce6b82 778 uint16_t week;
zhjcpi 12:5fb829ce6b82 779 uint32_t tow;
zhjcpi 12:5fb829ce6b82 780 uint32_t towSubMs;
zhjcpi 12:5fb829ce6b82 781 uint8_t info;
zhjcpi 12:5fb829ce6b82 782 int32_t nMsg = 0;
zhjcpi 12:5fb829ce6b82 783 uint16_t satInfo;
zhjcpi 12:5fb829ce6b82 784 uint16_t az;
zhjcpi 12:5fb829ce6b82 785 uint16_t el;
zhjcpi 12:5fb829ce6b82 786 uint16_t cno;
zhjcpi 12:5fb829ce6b82 787 uint8_t gnssType;
zhjcpi 12:5fb829ce6b82 788 uint16_t index = 0;
zhjcpi 12:5fb829ce6b82 789
zhjcpi 12:5fb829ce6b82 790 *pMsgLen = sizeof(*pSvStatus);
zhjcpi 12:5fb829ce6b82 791
zhjcpi 12:5fb829ce6b82 792 week = BINARY_IMPORT_UINT16(ptr);
zhjcpi 12:5fb829ce6b82 793 tow = BINARY_IMPORT_UINT32(ptr);
zhjcpi 12:5fb829ce6b82 794 towSubMs = BINARY_IMPORT_UINT32(ptr);
zhjcpi 12:5fb829ce6b82 795 ptr += 4;
zhjcpi 12:5fb829ce6b82 796 info = BINARY_IMPORT_UINT8(ptr);
zhjcpi 12:5fb829ce6b82 797
zhjcpi 12:5fb829ce6b82 798 nMsg = info & 0x0F;
zhjcpi 12:5fb829ce6b82 799 if (nMsg == 1) {
zhjcpi 12:5fb829ce6b82 800 memset(pSvStatus, 0, sizeof(tLocSvStatus));
zhjcpi 12:5fb829ce6b82 801 pSvStatus->gps_week = week;
zhjcpi 12:5fb829ce6b82 802 pSvStatus->tow = tow;
zhjcpi 12:5fb829ce6b82 803 pSvStatus->tow_sub_ms = towSubMs;
zhjcpi 0:aba381fc8158 804 }
zhjcpi 12:5fb829ce6b82 805
zhjcpi 12:5fb829ce6b82 806 ptr++;
zhjcpi 12:5fb829ce6b82 807 for (i = 0; i < GNSS_SAT_DATA_NUM_OF_SATS; i++) {
zhjcpi 12:5fb829ce6b82 808 satInfo = BINARY_IMPORT_UINT16(ptr);
zhjcpi 12:5fb829ce6b82 809 az = BINARY_IMPORT_UINT16(ptr);
zhjcpi 12:5fb829ce6b82 810 el = BINARY_IMPORT_UINT16(ptr);
zhjcpi 12:5fb829ce6b82 811 cno = BINARY_IMPORT_UINT16(ptr);
zhjcpi 12:5fb829ce6b82 812 ptr += 4;
zhjcpi 12:5fb829ce6b82 813
zhjcpi 12:5fb829ce6b82 814 gnssType = (uint8_t)((satInfo >> 13) & 0x0003);
zhjcpi 12:5fb829ce6b82 815 if ((0 == gnssType) || (1 == gnssType)) { // GPS, SBAS, QZSS
zhjcpi 12:5fb829ce6b82 816 index = pSvStatus->numOfSVs;
zhjcpi 12:5fb829ce6b82 817 if ((index < LOC_MAX_GNSS_SVS) && (cno >0)) {
zhjcpi 12:5fb829ce6b82 818 pSvStatus->svList[index].prn = (uint8_t)(satInfo & 0xFF);
zhjcpi 12:5fb829ce6b82 819 pSvStatus->svList[index].cno = (float)(cno / 10.0); // Scale: 10
zhjcpi 12:5fb829ce6b82 820 pSvStatus->svList[index].elevation = (float)(el / 10.0); // Scale: 10
zhjcpi 12:5fb829ce6b82 821 pSvStatus->svList[index].azimuth = (float)(az / 10.0); // Scale: 10
zhjcpi 12:5fb829ce6b82 822 pSvStatus->numOfSVs++;
zhjcpi 12:5fb829ce6b82 823 pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn - 1); // prn range: 1-32
zhjcpi 12:5fb829ce6b82 824 }
zhjcpi 12:5fb829ce6b82 825 } else if (2 == gnssType) { // GLONASS
zhjcpi 12:5fb829ce6b82 826 index = pSvStatus->numOfGloSVs;
zhjcpi 12:5fb829ce6b82 827 if ((index < CODEC_GLO_MAX_CHANNELS) && (cno>0)) {
zhjcpi 12:5fb829ce6b82 828 int16_t freqChan = (satInfo & 0X1F00) >> 8;
zhjcpi 12:5fb829ce6b82 829 int16_t slotNum = (satInfo & 0X00FF);
zhjcpi 12:5fb829ce6b82 830 if (slotNum > 0) {
zhjcpi 12:5fb829ce6b82 831 if (freqChan & 0X0010) {
zhjcpi 12:5fb829ce6b82 832 freqChan |= 0xFFE0;
zhjcpi 12:5fb829ce6b82 833 }
zhjcpi 12:5fb829ce6b82 834 pSvStatus->gloSvList[index].prn = (uint8_t)(freqChan + LOC_GLO_FREQ_OFFSET);
zhjcpi 12:5fb829ce6b82 835 pSvStatus->gloSvList[index].sno = (uint8_t)slotNum;
zhjcpi 12:5fb829ce6b82 836 pSvStatus->gloSvList[index].cno = (float)(cno / 10.0); // Scale: 10
zhjcpi 12:5fb829ce6b82 837 pSvStatus->gloSvList[index].elevation = (float)(el / 10.0); // Scale: 10
zhjcpi 12:5fb829ce6b82 838 pSvStatus->gloSvList[index].azimuth = (float)(az / 10.0); // Scale: 10
zhjcpi 12:5fb829ce6b82 839 pSvStatus->numOfGloSVs++;
zhjcpi 12:5fb829ce6b82 840 pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn - LOC_GLO_FREQ_ID_START);
zhjcpi 0:aba381fc8158 841 }
zhjcpi 0:aba381fc8158 842 }
zhjcpi 0:aba381fc8158 843 }
zhjcpi 0:aba381fc8158 844 }
zhjcpi 12:5fb829ce6b82 845
zhjcpi 12:5fb829ce6b82 846 break;
zhjcpi 0:aba381fc8158 847 }
zhjcpi 0:aba381fc8158 848
zhjcpi 12:5fb829ce6b82 849 case OSP_MSG_HW_CONFIG_REQ: /* 0x47 */
zhjcpi 12:5fb829ce6b82 850 break;
zhjcpi 0:aba381fc8158 851
zhjcpi 12:5fb829ce6b82 852 case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */
zhjcpi 12:5fb829ce6b82 853 break;
zhjcpi 0:aba381fc8158 854
zhjcpi 12:5fb829ce6b82 855 case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */
zhjcpi 12:5fb829ce6b82 856 *((uint8_t *)pMsgData) = *ptr;
zhjcpi 12:5fb829ce6b82 857 *pMsgLen = sizeof(uint8_t);
zhjcpi 12:5fb829ce6b82 858 break;
zhjcpi 0:aba381fc8158 859
zhjcpi 12:5fb829ce6b82 860 default:
zhjcpi 12:5fb829ce6b82 861 tRet = CSR_RESULT_FAILURE;
zhjcpi 12:5fb829ce6b82 862 break;
zhjcpi 0:aba381fc8158 863 }
zhjcpi 0:aba381fc8158 864
zhjcpi 0:aba381fc8158 865 /* check if length does not match */
zhjcpi 12:5fb829ce6b82 866 if (tRet == CSR_RESULT_FAILURE) {
zhjcpi 0:aba381fc8158 867 *pMsgId = *pMsgLen = 0;
zhjcpi 0:aba381fc8158 868 }
zhjcpi 0:aba381fc8158 869
zhjcpi 12:5fb829ce6b82 870 return tRet;
zhjcpi 0:aba381fc8158 871 } /* CsrUlocCodecSsbDecode() */
zhjcpi 0:aba381fc8158 872
zhjcpi 12:5fb829ce6b82 873 void CSRLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg)
zhjcpi 0:aba381fc8158 874 {
zhjcpi 12:5fb829ce6b82 875 switch (pOspMsg->msgId) {
zhjcpi 12:5fb829ce6b82 876 case OSP_MSG_GEODETIC_NAVIGATION:
zhjcpi 12:5fb829ce6b82 877 outputHandler(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp));
zhjcpi 12:5fb829ce6b82 878 break;
zhjcpi 12:5fb829ce6b82 879 case OSP_MSG_GNSS_SAT_DATA:
zhjcpi 12:5fb829ce6b82 880 break;
zhjcpi 12:5fb829ce6b82 881 case OSP_MSG_GNSS_NAV_DATA:
zhjcpi 12:5fb829ce6b82 882 outputHandler(LOC_OUTPUT_SV_STATUS, &svStatus, sizeof(tLocSvStatus));
zhjcpi 12:5fb829ce6b82 883 break;
zhjcpi 12:5fb829ce6b82 884 case OSP_MSG_OK_TO_SEND:
zhjcpi 12:5fb829ce6b82 885 engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND;
zhjcpi 12:5fb829ce6b82 886 CSR_LOG_INFO("Ok to send %u\r\n", engStatus);
zhjcpi 12:5fb829ce6b82 887 break;
zhjcpi 12:5fb829ce6b82 888 case OSP_MSG_SW_VERSION:
zhjcpi 12:5fb829ce6b82 889 bVerRsp = true;
zhjcpi 12:5fb829ce6b82 890 CSR_LOG_INFO("Ver: %s\r\n", pOspMsg->payload);
zhjcpi 12:5fb829ce6b82 891 break;
zhjcpi 12:5fb829ce6b82 892 case OSP_MSG_HW_CONFIG_REQ:
zhjcpi 12:5fb829ce6b82 893 CSR_LOG_INFO("hw config req.\r\n");
zhjcpi 12:5fb829ce6b82 894 if (!bVerRsp) {
zhjcpi 12:5fb829ce6b82 895 _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
zhjcpi 1:bbaf9b8d646a 896 }
zhjcpi 12:5fb829ce6b82 897
zhjcpi 12:5fb829ce6b82 898 if (!bPwrModeRsp) {
zhjcpi 12:5fb829ce6b82 899 if (pwrMode == PWR_PTF) {
zhjcpi 12:5fb829ce6b82 900 CSR_LOG_INFO("Send PTF command.\r\n");
zhjcpi 12:5fb829ce6b82 901 _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
zhjcpi 12:5fb829ce6b82 902 } else {
zhjcpi 12:5fb829ce6b82 903 CSR_LOG_INFO("Send FPM command.\r\n");
zhjcpi 12:5fb829ce6b82 904 _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
zhjcpi 12:5fb829ce6b82 905 }
zhjcpi 1:bbaf9b8d646a 906 }
zhjcpi 12:5fb829ce6b82 907 break;
zhjcpi 12:5fb829ce6b82 908 case OSP_MSG_PWR_MODE_LPM_RSP:
zhjcpi 12:5fb829ce6b82 909 bPwrModeRsp = true;
zhjcpi 12:5fb829ce6b82 910 CSR_LOG_INFO("lpm response.\r\n");
zhjcpi 12:5fb829ce6b82 911 break;
zhjcpi 12:5fb829ce6b82 912 case OSP_MSG_PWR_MODE_FPM_RSP:
zhjcpi 12:5fb829ce6b82 913 bPwrModeRsp = true;
zhjcpi 12:5fb829ce6b82 914 CSR_LOG_INFO("fpm response.\r\n");
zhjcpi 12:5fb829ce6b82 915 break;
zhjcpi 12:5fb829ce6b82 916 default:
zhjcpi 12:5fb829ce6b82 917 CSR_LOG_INFO("Unknown OSP message 0x%lx.\r\n", pOspMsg->msgId);
zhjcpi 12:5fb829ce6b82 918 break;
zhjcpi 0:aba381fc8158 919 }
zhjcpi 0:aba381fc8158 920 }
zhjcpi 0:aba381fc8158 921
zhjcpi 12:5fb829ce6b82 922 void CSRLocation::_CsrLocTimeout(void)
zhjcpi 0:aba381fc8158 923 {
zhjcpi 12:5fb829ce6b82 924 bTimeoutFlag = true;
zhjcpi 0:aba381fc8158 925 }
zhjcpi 0:aba381fc8158 926
zhjcpi 12:5fb829ce6b82 927 void CSRLocation::_CsrLocRxHandler(void)
zhjcpi 0:aba381fc8158 928 {
zhjcpi 12:5fb829ce6b82 929 serialBuf[in++] = serialLoc.getc();
zhjcpi 12:5fb829ce6b82 930 in &= (MAX_SERIAL_BUF_LEN - 1);
zhjcpi 12:5fb829ce6b82 931 if (in == out) {
zhjcpi 12:5fb829ce6b82 932 CSR_LOG_INFO("rx overwritten %lu %lu.\r\n", in, out);
zhjcpi 0:aba381fc8158 933 }
zhjcpi 0:aba381fc8158 934 }
zhjcpi 0:aba381fc8158 935
zhjcpi 12:5fb829ce6b82 936 void CSRLocation::_CsrLocSendData(eSendDataType type)
zhjcpi 0:aba381fc8158 937 {
zhjcpi 12:5fb829ce6b82 938 uint32_t i, size;
zhjcpi 2:d4fe184925f2 939 const uint8_t *pData;
zhjcpi 0:aba381fc8158 940
zhjcpi 12:5fb829ce6b82 941 switch (type) {
zhjcpi 12:5fb829ce6b82 942 case SEND_DATA_TYPE_OSP_STOP_REQ:
zhjcpi 12:5fb829ce6b82 943 pData = sOspStopReq;
zhjcpi 12:5fb829ce6b82 944 size = sizeof(sOspStopReq);
zhjcpi 12:5fb829ce6b82 945 break;
zhjcpi 12:5fb829ce6b82 946 case SEND_DATA_TYPE_OSP_VER_REQ:
zhjcpi 12:5fb829ce6b82 947 pData = sOspVerReq;
zhjcpi 12:5fb829ce6b82 948 size = sizeof(sOspVerReq);
zhjcpi 12:5fb829ce6b82 949 break;
zhjcpi 12:5fb829ce6b82 950 case SEND_DATA_TYPE_OSP_LPM_REQ:
zhjcpi 12:5fb829ce6b82 951 pData = sOspLpmReq;
zhjcpi 12:5fb829ce6b82 952 size = sizeof(sOspLpmReq);
zhjcpi 12:5fb829ce6b82 953 break;
zhjcpi 12:5fb829ce6b82 954 case SEND_DATA_TYPE_OSP_FPM_REQ:
zhjcpi 12:5fb829ce6b82 955 pData = sOspFpmReq;
zhjcpi 12:5fb829ce6b82 956 size = sizeof(sOspFpmReq);
zhjcpi 12:5fb829ce6b82 957 break;
zhjcpi 12:5fb829ce6b82 958 case SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ:
zhjcpi 12:5fb829ce6b82 959 pData = sOspSwitch2NmeaReq;
zhjcpi 12:5fb829ce6b82 960 size = sizeof(sOspSwitch2NmeaReq);
zhjcpi 12:5fb829ce6b82 961 break;
zhjcpi 12:5fb829ce6b82 962 case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ:
zhjcpi 12:5fb829ce6b82 963 pData = (const uint8_t *)sNmeaSwitch2OspReq;
zhjcpi 12:5fb829ce6b82 964 size = strlen(sNmeaSwitch2OspReq);
zhjcpi 12:5fb829ce6b82 965 break;
zhjcpi 12:5fb829ce6b82 966 case SEND_DATA_TYPE_NMEA_STOP_REQ:
zhjcpi 12:5fb829ce6b82 967 pData = (const uint8_t *)sNmeaStopReq;
zhjcpi 12:5fb829ce6b82 968 size = strlen(sNmeaStopReq);
zhjcpi 12:5fb829ce6b82 969 break;
zhjcpi 12:5fb829ce6b82 970
zhjcpi 12:5fb829ce6b82 971 default:
zhjcpi 12:5fb829ce6b82 972 pData = NULL;
zhjcpi 0:aba381fc8158 973 }
zhjcpi 0:aba381fc8158 974
zhjcpi 12:5fb829ce6b82 975 if (pData != NULL) {
zhjcpi 12:5fb829ce6b82 976 for (i = 0; i < size; i++) {
zhjcpi 12:5fb829ce6b82 977 serialLoc.putc(pData[i]);
zhjcpi 0:aba381fc8158 978 }
zhjcpi 0:aba381fc8158 979 }
zhjcpi 0:aba381fc8158 980 }
zhjcpi 0:aba381fc8158 981
zhjcpi 12:5fb829ce6b82 982 void CSRLocation::_CsrLocHwOnoff(void)
zhjcpi 0:aba381fc8158 983 {
zhjcpi 12:5fb829ce6b82 984 pinOnoff.write(1);
zhjcpi 12:5fb829ce6b82 985 wait_ms(100);
zhjcpi 12:5fb829ce6b82 986 pinOnoff.write(0);
zhjcpi 12:5fb829ce6b82 987 CSR_LOG_INFO("Onoff pulse given.\r\n");
zhjcpi 12:5fb829ce6b82 988 }
zhjcpi 12:5fb829ce6b82 989
zhjcpi 12:5fb829ce6b82 990 void CSRLocation::_CsrLocHwReset(void)
zhjcpi 12:5fb829ce6b82 991 {
zhjcpi 12:5fb829ce6b82 992 pinReset.write(0);
zhjcpi 12:5fb829ce6b82 993 wait_ms(100);
zhjcpi 12:5fb829ce6b82 994 pinReset.write(1);
zhjcpi 12:5fb829ce6b82 995 CSR_LOG_INFO("Reset pulse given.\r\n");
zhjcpi 0:aba381fc8158 996 }
zhjcpi 0:aba381fc8158 997
zhjcpi 12:5fb829ce6b82 998 bool CSRLocation::_CsrLocIsWakeup(void)
zhjcpi 0:aba381fc8158 999 {
zhjcpi 12:5fb829ce6b82 1000 CSR_LOG_INFO("Is wakeup %d.\r\n", wakeup.read());
zhjcpi 12:5fb829ce6b82 1001 return wakeup.read() != 0 ? true : false;
zhjcpi 0:aba381fc8158 1002 }
zhjcpi 8:fa29f8455134 1003
zhjcpi 12:5fb829ce6b82 1004 bool CSRLocation::setPowerMode(GPSProvider::PowerMode_t _pwrMode)
zhjcpi 8:fa29f8455134 1005 {
zhjcpi 12:5fb829ce6b82 1006 switch (_pwrMode) {
zhjcpi 12:5fb829ce6b82 1007 case GPSProvider::POWER_FULL:
zhjcpi 12:5fb829ce6b82 1008 pwrMode = PWR_FULL;
zhjcpi 12:5fb829ce6b82 1009 break;
zhjcpi 12:5fb829ce6b82 1010 case GPSProvider::POWER_LOW:
zhjcpi 12:5fb829ce6b82 1011 default:
zhjcpi 12:5fb829ce6b82 1012 pwrMode = PWR_PTF; /* push to fix */
zhjcpi 12:5fb829ce6b82 1013 break;
zhjcpi 12:5fb829ce6b82 1014 }
zhjcpi 12:5fb829ce6b82 1015
zhjcpi 12:5fb829ce6b82 1016 return (true);
zhjcpi 8:fa29f8455134 1017 }