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@20:88db82cf3f0f, 2014-11-27 (annotated)

Committer:
zhjcpi
Date:
Thu Nov 27 10:28:35 2014 +0000
Revision:
20:88db82cf3f0f
Parent:
18:5d72465991f5 
Support Freescale FDRM platform.

; Location uart cannot use D8/D2, it is changed to D1/D0.

; Trigger on/off before open UART. Otherwise, FDRM will halt when doing the second start(start,then stop, then start again).

Who changed what in which revision?

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