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@2:d4fe184925f2, 2014-03-26 (annotated)

Committer:
zhjcpi
Date:
Wed Mar 26 09:16:55 2014 +0000
Revision:
2:d4fe184925f2
Parent:
1:bbaf9b8d646a
Child:
3:71690f7bb480
optimization

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zhjcpi 0:aba381fc8158 1
zhjcpi 0:aba381fc8158 2 /* CsrLocation class for mbed Microcontroller
zhjcpi 0:aba381fc8158 3 * Copyright 2014 CSR plc
zhjcpi 0:aba381fc8158 4 */
zhjcpi 0:aba381fc8158 5
zhjcpi 0:aba381fc8158 6
zhjcpi 0:aba381fc8158 7 #include "mbed.h"
zhjcpi 0:aba381fc8158 8 #include "CsrLocation.h"
zhjcpi 0:aba381fc8158 9
zhjcpi 0:aba381fc8158 10
zhjcpi 2:d4fe184925f2 11 static uint8_t sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3};
zhjcpi 2:d4fe184925f2 12 static uint8_t sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3};
zhjcpi 2:d4fe184925f2 13 static uint8_t sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E,
zhjcpi 1:bbaf9b8d646a 14 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
zhjcpi 1:bbaf9b8d646a 15 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x63, 0xB0, 0xB3};
zhjcpi 2:d4fe184925f2 16 static uint8_t sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3};
zhjcpi 2:d4fe184925f2 17 static uint8_t sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00,
zhjcpi 0:aba381fc8158 18 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x12, 0xC0, 0x01, 0x64, 0xB0, 0xB3};
zhjcpi 2:d4fe184925f2 19 static char sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n";
zhjcpi 0:aba381fc8158 20
zhjcpi 0:aba381fc8158 21 CsrLocation::CsrLocation(tCsrLocConfig *pLocConfig)
zhjcpi 0:aba381fc8158 22 {
zhjcpi 0:aba381fc8158 23 memset(&csrLocInst, 0, sizeof(tCsrLocInst));
zhjcpi 0:aba381fc8158 24 csrLocInst.pSerialDebug = pLocConfig->pSerialDebug;
zhjcpi 0:aba381fc8158 25 csrLocInst.pSerialLoc = pLocConfig->pSerialLoc;
zhjcpi 0:aba381fc8158 26 csrLocInst.pPinOnoff = pLocConfig->pPinOnoff;
zhjcpi 0:aba381fc8158 27 csrLocInst.pPinReset = pLocConfig->pPinReset;
zhjcpi 0:aba381fc8158 28 csrLocInst.pTimeoutChk = new Timeout();
zhjcpi 0:aba381fc8158 29
zhjcpi 0:aba381fc8158 30 csrLocInst.pPinReset->write(1);
zhjcpi 0:aba381fc8158 31 csrLocInst.pPinOnoff->write(0);
zhjcpi 0:aba381fc8158 32 }
zhjcpi 0:aba381fc8158 33
zhjcpi 0:aba381fc8158 34 CsrLocation::~CsrLocation(void)
zhjcpi 0:aba381fc8158 35 {
zhjcpi 0:aba381fc8158 36 if(csrLocInst.pTimeoutChk != NULL)
zhjcpi 0:aba381fc8158 37 {
zhjcpi 0:aba381fc8158 38 delete csrLocInst.pTimeoutChk;
zhjcpi 0:aba381fc8158 39 csrLocInst.pTimeoutChk = NULL;
zhjcpi 0:aba381fc8158 40 }
zhjcpi 0:aba381fc8158 41 csrLocInst.pSerialLoc->attach(NULL);
zhjcpi 0:aba381fc8158 42 csrLocInst.pPinReset->write(0);
zhjcpi 0:aba381fc8158 43 csrLocInst.pPinOnoff->write(0);
zhjcpi 0:aba381fc8158 44 memset(&csrLocInst, 0, sizeof(tCsrLocInst));
zhjcpi 0:aba381fc8158 45
zhjcpi 0:aba381fc8158 46 }
zhjcpi 0:aba381fc8158 47
zhjcpi 0:aba381fc8158 48 void CsrLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb)
zhjcpi 0:aba381fc8158 49 {
zhjcpi 0:aba381fc8158 50 csrLocInst.appOutCb = app_output_cb;
zhjcpi 0:aba381fc8158 51 csrLocInst.appEventCb = app_event_cb;
zhjcpi 0:aba381fc8158 52 }
zhjcpi 0:aba381fc8158 53
zhjcpi 0:aba381fc8158 54 void CsrLocation::CsrLocReset(void)
zhjcpi 0:aba381fc8158 55 {
zhjcpi 0:aba381fc8158 56 _CsrLocHwReset();
zhjcpi 0:aba381fc8158 57 }
zhjcpi 0:aba381fc8158 58
zhjcpi 0:aba381fc8158 59 void CsrLocation::CsrLocStart(ePowerMode pwrMode)
zhjcpi 0:aba381fc8158 60 {
zhjcpi 0:aba381fc8158 61 csrLocInst.pwrMode = pwrMode;
zhjcpi 0:aba381fc8158 62
zhjcpi 0:aba381fc8158 63 if(csrLocInst.locState == CSR_LOC_STATE_IDLE)
zhjcpi 0:aba381fc8158 64 {
zhjcpi 0:aba381fc8158 65 /* Csr Location SDK version */
zhjcpi 0:aba381fc8158 66 CSR_LOG_INFO("==== CSR LOC SDK version: %s Date: %s ====\r\n", CSR_LOC_SDK_VER, __DATE__ " " __TIME__);
zhjcpi 0:aba381fc8158 67 /* open UART */
zhjcpi 0:aba381fc8158 68 CSR_LOG_INFO("Checking OSP protocol...\r\n");
zhjcpi 0:aba381fc8158 69 csrLocInst.protoState = PROTO_STATE_DET_OSP;
zhjcpi 0:aba381fc8158 70 csrLocInst.baudRate = BAUDRATE_OSP;
zhjcpi 0:aba381fc8158 71 _CsrLocUartInit();
zhjcpi 0:aba381fc8158 72
zhjcpi 0:aba381fc8158 73 /* trigger on_off */
zhjcpi 0:aba381fc8158 74 _CsrLocHwOnoff();
zhjcpi 0:aba381fc8158 75
zhjcpi 0:aba381fc8158 76 csrLocInst.locState = CSR_LOC_STATE_RUN;
zhjcpi 2:d4fe184925f2 77 csrLocInst.bPwrModeRsp = false;
zhjcpi 2:d4fe184925f2 78 csrLocInst.bVerRsp = false;
zhjcpi 1:bbaf9b8d646a 79
zhjcpi 0:aba381fc8158 80 }
zhjcpi 0:aba381fc8158 81 else
zhjcpi 0:aba381fc8158 82 {
zhjcpi 0:aba381fc8158 83 CSR_LOG_INFO("Already started.\r\n");
zhjcpi 0:aba381fc8158 84 }
zhjcpi 0:aba381fc8158 85 }
zhjcpi 0:aba381fc8158 86
zhjcpi 0:aba381fc8158 87 void CsrLocation::CsrLocUpdate(void)
zhjcpi 0:aba381fc8158 88 {
zhjcpi 0:aba381fc8158 89 if(csrLocInst.locState == CSR_LOC_STATE_RUN)
zhjcpi 0:aba381fc8158 90 {
zhjcpi 0:aba381fc8158 91 /* wait and process uart data */
zhjcpi 0:aba381fc8158 92 _CsrLocProcessRawStream();
zhjcpi 0:aba381fc8158 93 }
zhjcpi 0:aba381fc8158 94 }
zhjcpi 0:aba381fc8158 95
zhjcpi 0:aba381fc8158 96 void CsrLocation::CsrLocStop(void)
zhjcpi 0:aba381fc8158 97 {
zhjcpi 0:aba381fc8158 98 csrLocInst.pSerialLoc->attach(NULL);
zhjcpi 0:aba381fc8158 99 CSR_LOG_INFO("Stop command processed.\r\n");
zhjcpi 0:aba381fc8158 100
zhjcpi 0:aba381fc8158 101 if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND)
zhjcpi 0:aba381fc8158 102 {
zhjcpi 0:aba381fc8158 103 /* in sleep mode, trigger on_off firstly */
zhjcpi 0:aba381fc8158 104 _CsrLocHwOnoff();
zhjcpi 0:aba381fc8158 105 wait_ms(500);
zhjcpi 0:aba381fc8158 106 }
zhjcpi 0:aba381fc8158 107 _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ);
zhjcpi 0:aba381fc8158 108 wait_ms(10);
zhjcpi 0:aba381fc8158 109 _CsrLocHwReset();
zhjcpi 0:aba381fc8158 110 csrLocInst.locState = CSR_LOC_STATE_IDLE;
zhjcpi 0:aba381fc8158 111 csrLocInst.appEventCb(CSR_LOC_EVENT_STOP_RESULT, 0);
zhjcpi 0:aba381fc8158 112
zhjcpi 0:aba381fc8158 113 }
zhjcpi 0:aba381fc8158 114
zhjcpi 0:aba381fc8158 115 void CsrLocation::CsrLocLpmGetPos(void)
zhjcpi 0:aba381fc8158 116 {
zhjcpi 0:aba381fc8158 117 if(csrLocInst.locState == CSR_LOC_STATE_RUN && csrLocInst.pwrMode == PWR_PTF && csrLocInst.engStatus == ENGINE_STATUS_NOTOK2SEND)
zhjcpi 0:aba381fc8158 118 {
zhjcpi 0:aba381fc8158 119 CSR_LOG_INFO("LpmGetPos ");
zhjcpi 0:aba381fc8158 120 _CsrLocHwOnoff();
zhjcpi 2:d4fe184925f2 121 }
zhjcpi 2:d4fe184925f2 122 else
zhjcpi 2:d4fe184925f2 123 {
zhjcpi 2:d4fe184925f2 124 if(csrLocInst.locState != CSR_LOC_STATE_RUN)
zhjcpi 2:d4fe184925f2 125 {
zhjcpi 2:d4fe184925f2 126 CSR_LOG_INFO("Not in run state.\r\n");
zhjcpi 2:d4fe184925f2 127 }
zhjcpi 2:d4fe184925f2 128 else if(csrLocInst.pwrMode != PWR_PTF)
zhjcpi 2:d4fe184925f2 129 {
zhjcpi 2:d4fe184925f2 130 CSR_LOG_INFO("Not in low power PTF mode.\r\n");
zhjcpi 2:d4fe184925f2 131 }
zhjcpi 2:d4fe184925f2 132 }
zhjcpi 0:aba381fc8158 133 }
zhjcpi 0:aba381fc8158 134
zhjcpi 0:aba381fc8158 135 void CsrLocation::CsrLocDebugSwitch2Nmea(void)
zhjcpi 0:aba381fc8158 136 {
zhjcpi 0:aba381fc8158 137 if(csrLocInst.locState == CSR_LOC_STATE_RUN)
zhjcpi 0:aba381fc8158 138 {
zhjcpi 0:aba381fc8158 139 _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ);
zhjcpi 0:aba381fc8158 140 wait_ms(200);
zhjcpi 0:aba381fc8158 141 _CsrLocHwReset();
zhjcpi 0:aba381fc8158 142 }
zhjcpi 0:aba381fc8158 143 }
zhjcpi 0:aba381fc8158 144
zhjcpi 0:aba381fc8158 145 void CsrLocation::_CsrLocUartInit(void)
zhjcpi 0:aba381fc8158 146 {
zhjcpi 0:aba381fc8158 147 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 0:aba381fc8158 148
zhjcpi 0:aba381fc8158 149 pLocInst->in = 0;
zhjcpi 0:aba381fc8158 150 pLocInst->out = 0;
zhjcpi 0:aba381fc8158 151 memset(pLocInst->serialBuf, 0 , MAX_SERIAL_BUF_LEN);
zhjcpi 0:aba381fc8158 152 memset(pLocInst->serialPkt, 0 , MAX_SERIAL_PKT_LEN);
zhjcpi 0:aba381fc8158 153
zhjcpi 0:aba381fc8158 154 pLocInst->checksum = 0;
zhjcpi 0:aba381fc8158 155 pLocInst->msgSize = 0;
zhjcpi 0:aba381fc8158 156 pLocInst->decodeIndex = 0;
zhjcpi 0:aba381fc8158 157 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 158 pLocInst->pTimeoutChk->attach(this, &CsrLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT);
zhjcpi 0:aba381fc8158 159
zhjcpi 0:aba381fc8158 160 pLocInst->pSerialLoc->baud(pLocInst->baudRate);
zhjcpi 0:aba381fc8158 161 pLocInst->pSerialLoc->attach(this, &CsrLocation::_CsrLocRxHandler );
zhjcpi 0:aba381fc8158 162 }
zhjcpi 0:aba381fc8158 163
zhjcpi 0:aba381fc8158 164 void CsrLocation::_CsrLocProcessRawStream(void)
zhjcpi 0:aba381fc8158 165 {
zhjcpi 0:aba381fc8158 166 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 2:d4fe184925f2 167 uint8_t data;
zhjcpi 0:aba381fc8158 168
zhjcpi 0:aba381fc8158 169 if(pLocInst->in != pLocInst->out)
zhjcpi 0:aba381fc8158 170 {
zhjcpi 0:aba381fc8158 171 data = pLocInst->serialBuf[pLocInst->out++];
zhjcpi 0:aba381fc8158 172 pLocInst->out &= (MAX_SERIAL_BUF_LEN-1);
zhjcpi 0:aba381fc8158 173 switch(pLocInst->protoState)
zhjcpi 0:aba381fc8158 174 {
zhjcpi 0:aba381fc8158 175 case PROTO_STATE_DET_OSP:
zhjcpi 2:d4fe184925f2 176 case PROTO_STATE_DET_OSP_FROM_NMEA:
zhjcpi 0:aba381fc8158 177 _CsrLocDetProtoOsp(data);
zhjcpi 0:aba381fc8158 178 break;
zhjcpi 0:aba381fc8158 179 case PROTO_STATE_DET_NMEA:
zhjcpi 0:aba381fc8158 180 _CsrLocDetProtoNmea(data);
zhjcpi 0:aba381fc8158 181 break;
zhjcpi 0:aba381fc8158 182 case PROTO_STATE_DET_OK:
zhjcpi 0:aba381fc8158 183 _CsrLocProcessRawOspStream(data);
zhjcpi 0:aba381fc8158 184 break;
zhjcpi 0:aba381fc8158 185 default:
zhjcpi 0:aba381fc8158 186 /* Discard received data */
zhjcpi 1:bbaf9b8d646a 187 //pLocInst->out = pLocInst->in;
zhjcpi 0:aba381fc8158 188 break;
zhjcpi 0:aba381fc8158 189 }
zhjcpi 0:aba381fc8158 190 }
zhjcpi 0:aba381fc8158 191 }
zhjcpi 0:aba381fc8158 192
zhjcpi 2:d4fe184925f2 193 void CsrLocation::_CsrLocDetProtoOsp(uint8_t data)
zhjcpi 0:aba381fc8158 194 {
zhjcpi 0:aba381fc8158 195 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 0:aba381fc8158 196
zhjcpi 0:aba381fc8158 197 if(pLocInst->bTimeoutFlag)
zhjcpi 0:aba381fc8158 198 {
zhjcpi 0:aba381fc8158 199 /* Failed to detect OSP and try to detect NMEA */
zhjcpi 0:aba381fc8158 200 pLocInst->pSerialLoc->attach(NULL);
zhjcpi 0:aba381fc8158 201 pLocInst->pTimeoutChk->detach();
zhjcpi 2:d4fe184925f2 202 pLocInst->bTimeoutFlag = false;
zhjcpi 0:aba381fc8158 203 if(pLocInst->protoState == PROTO_STATE_DET_OSP)
zhjcpi 0:aba381fc8158 204 {
zhjcpi 0:aba381fc8158 205 pLocInst->protoState = PROTO_STATE_DET_NMEA;
zhjcpi 0:aba381fc8158 206 pLocInst->baudRate = BAUDRATE_NMEA;
zhjcpi 0:aba381fc8158 207 CSR_LOG_INFO("Checking OSP protocol failed, now check NMEA protocol...\r\n");
zhjcpi 0:aba381fc8158 208 _CsrLocUartInit();
zhjcpi 0:aba381fc8158 209 }
zhjcpi 0:aba381fc8158 210 else
zhjcpi 0:aba381fc8158 211 {
zhjcpi 0:aba381fc8158 212 pLocInst->protoState = PROTO_STATE_DET_INVALID;
zhjcpi 0:aba381fc8158 213 CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n");
zhjcpi 0:aba381fc8158 214 pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1);
zhjcpi 0:aba381fc8158 215 }
zhjcpi 0:aba381fc8158 216 return;
zhjcpi 0:aba381fc8158 217 }
zhjcpi 0:aba381fc8158 218
zhjcpi 0:aba381fc8158 219 _CsrLocProcessRawOspStream(data);
zhjcpi 0:aba381fc8158 220 }
zhjcpi 0:aba381fc8158 221
zhjcpi 2:d4fe184925f2 222 void CsrLocation::_CsrLocDetProtoNmea(uint8_t data)
zhjcpi 0:aba381fc8158 223 {
zhjcpi 0:aba381fc8158 224 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 0:aba381fc8158 225
zhjcpi 0:aba381fc8158 226 if(pLocInst->bTimeoutFlag)
zhjcpi 0:aba381fc8158 227 {
zhjcpi 0:aba381fc8158 228 /* Failed to detect OSP and try to detect NMEA */
zhjcpi 0:aba381fc8158 229 pLocInst->pSerialLoc->attach(NULL);
zhjcpi 0:aba381fc8158 230 pLocInst->pTimeoutChk->detach();
zhjcpi 2:d4fe184925f2 231 pLocInst->bTimeoutFlag = false;
zhjcpi 0:aba381fc8158 232 pLocInst->protoState = PROTO_STATE_DET_INVALID;
zhjcpi 0:aba381fc8158 233 pLocInst->baudRate = BAUDRATE_OSP;
zhjcpi 0:aba381fc8158 234 CSR_LOG_INFO("Checking NMEA failed.\r\n");
zhjcpi 0:aba381fc8158 235 pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 1);
zhjcpi 0:aba381fc8158 236 return;
zhjcpi 0:aba381fc8158 237 }
zhjcpi 0:aba381fc8158 238
zhjcpi 0:aba381fc8158 239 switch (pLocInst->protoDetState)
zhjcpi 0:aba381fc8158 240 {
zhjcpi 0:aba381fc8158 241 case STATE_START1:
zhjcpi 0:aba381fc8158 242 if (NMEA_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 243 {
zhjcpi 0:aba381fc8158 244 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 245 }
zhjcpi 0:aba381fc8158 246 break;
zhjcpi 0:aba381fc8158 247
zhjcpi 0:aba381fc8158 248 case STATE_START2:
zhjcpi 0:aba381fc8158 249 if (NMEA_MSG_HEAD1 == data)
zhjcpi 0:aba381fc8158 250 {
zhjcpi 0:aba381fc8158 251 pLocInst->protoDetState = STATE_END1;
zhjcpi 0:aba381fc8158 252 }
zhjcpi 0:aba381fc8158 253 else if (NMEA_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 254 {
zhjcpi 0:aba381fc8158 255 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 256 }
zhjcpi 0:aba381fc8158 257 else
zhjcpi 0:aba381fc8158 258 {
zhjcpi 0:aba381fc8158 259 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 260 }
zhjcpi 0:aba381fc8158 261 break;
zhjcpi 0:aba381fc8158 262 case STATE_END1:
zhjcpi 0:aba381fc8158 263 if (NMEA_MSG_TAIL0 == data)
zhjcpi 0:aba381fc8158 264 {
zhjcpi 0:aba381fc8158 265 pLocInst->pSerialLoc->attach(NULL);
zhjcpi 0:aba381fc8158 266 pLocInst->pTimeoutChk->detach();
zhjcpi 2:d4fe184925f2 267 pLocInst->bTimeoutFlag = false;
zhjcpi 0:aba381fc8158 268 pLocInst->protoState = PROTO_STATE_SWI_OSP_FROM_NMEA;
zhjcpi 0:aba381fc8158 269 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 270 CSR_LOG_INFO("Checking OSP protocol OK, switching to OSP...\r\n");
zhjcpi 0:aba381fc8158 271 _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ);
zhjcpi 0:aba381fc8158 272 wait_ms(100);
zhjcpi 0:aba381fc8158 273
zhjcpi 0:aba381fc8158 274 pLocInst->protoState = PROTO_STATE_DET_OSP_FROM_NMEA;
zhjcpi 0:aba381fc8158 275 pLocInst->baudRate = BAUDRATE_OSP;
zhjcpi 0:aba381fc8158 276 CSR_LOG_INFO("Checking switched OSP protocol...\r\n");
zhjcpi 0:aba381fc8158 277 _CsrLocUartInit();
zhjcpi 0:aba381fc8158 278 }
zhjcpi 0:aba381fc8158 279 else if (NMEA_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 280 {
zhjcpi 0:aba381fc8158 281 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 282 }
zhjcpi 0:aba381fc8158 283 break;
zhjcpi 0:aba381fc8158 284 default:
zhjcpi 0:aba381fc8158 285 break;
zhjcpi 0:aba381fc8158 286 }
zhjcpi 0:aba381fc8158 287 }
zhjcpi 0:aba381fc8158 288
zhjcpi 2:d4fe184925f2 289 void CsrLocation::_CsrLocProcessRawOspStream(uint8_t data)
zhjcpi 0:aba381fc8158 290 {
zhjcpi 0:aba381fc8158 291 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 0:aba381fc8158 292
zhjcpi 0:aba381fc8158 293 switch (pLocInst->protoDetState)
zhjcpi 0:aba381fc8158 294 {
zhjcpi 0:aba381fc8158 295 case STATE_START1:
zhjcpi 0:aba381fc8158 296 if (OSP_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 297 {
zhjcpi 0:aba381fc8158 298 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 299 }
zhjcpi 0:aba381fc8158 300 break;
zhjcpi 0:aba381fc8158 301
zhjcpi 0:aba381fc8158 302 case STATE_START2:
zhjcpi 0:aba381fc8158 303 if (OSP_MSG_HEAD1 == data)
zhjcpi 0:aba381fc8158 304 {
zhjcpi 0:aba381fc8158 305 pLocInst->protoDetState = STATE_SIZE1;
zhjcpi 0:aba381fc8158 306 }
zhjcpi 0:aba381fc8158 307 else if (OSP_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 308 {
zhjcpi 0:aba381fc8158 309 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 310 }
zhjcpi 0:aba381fc8158 311 else
zhjcpi 0:aba381fc8158 312 {
zhjcpi 0:aba381fc8158 313 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 314 }
zhjcpi 0:aba381fc8158 315 break;
zhjcpi 0:aba381fc8158 316
zhjcpi 0:aba381fc8158 317 case STATE_SIZE1:
zhjcpi 0:aba381fc8158 318 pLocInst->msgSize = data ;
zhjcpi 2:d4fe184925f2 319 pLocInst->msgSize <<= 8; /* high uint8_t */
zhjcpi 0:aba381fc8158 320 pLocInst->protoDetState = STATE_SIZE2;
zhjcpi 0:aba381fc8158 321 break;
zhjcpi 0:aba381fc8158 322
zhjcpi 0:aba381fc8158 323 case STATE_SIZE2:
zhjcpi 0:aba381fc8158 324 pLocInst->msgSize += data ;
zhjcpi 0:aba381fc8158 325 if (MAX_SERIAL_PKT_LEN < pLocInst->msgSize || 0 == pLocInst->msgSize)
zhjcpi 0:aba381fc8158 326 {
zhjcpi 0:aba381fc8158 327 if (OSP_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 328 {
zhjcpi 0:aba381fc8158 329 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 330 }
zhjcpi 0:aba381fc8158 331 else
zhjcpi 0:aba381fc8158 332 {
zhjcpi 0:aba381fc8158 333 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 334 }
zhjcpi 0:aba381fc8158 335 }
zhjcpi 0:aba381fc8158 336 else
zhjcpi 0:aba381fc8158 337 {
zhjcpi 0:aba381fc8158 338 pLocInst->computedCheckSum = 0;
zhjcpi 0:aba381fc8158 339 pLocInst->decodeIndex = 0;
zhjcpi 0:aba381fc8158 340 pLocInst->protoDetState = STATE_PAYLOAD;
zhjcpi 0:aba381fc8158 341 }
zhjcpi 0:aba381fc8158 342 break;
zhjcpi 0:aba381fc8158 343
zhjcpi 0:aba381fc8158 344 case STATE_PAYLOAD:
zhjcpi 0:aba381fc8158 345 /* check for a catastrophic error case */
zhjcpi 0:aba381fc8158 346 if (MAX_SERIAL_PKT_LEN <= pLocInst->decodeIndex)
zhjcpi 0:aba381fc8158 347 {
zhjcpi 0:aba381fc8158 348 /* This is really bad. And should never happen since we
zhjcpi 0:aba381fc8158 349 * gurantee that msgSize is always less than RECEIVE_PACKET_SIZE
zhjcpi 0:aba381fc8158 350 * Change the state back to STATE_START1 and start over */
zhjcpi 0:aba381fc8158 351 if (OSP_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 352 {
zhjcpi 0:aba381fc8158 353 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 354 }
zhjcpi 0:aba381fc8158 355 else
zhjcpi 0:aba381fc8158 356 {
zhjcpi 0:aba381fc8158 357 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 358 }
zhjcpi 0:aba381fc8158 359 break;
zhjcpi 0:aba381fc8158 360 }
zhjcpi 0:aba381fc8158 361 /* Store the byte */
zhjcpi 0:aba381fc8158 362 pLocInst->serialPkt[pLocInst->decodeIndex++] = data;
zhjcpi 0:aba381fc8158 363 pLocInst->computedCheckSum += data;
zhjcpi 0:aba381fc8158 364
zhjcpi 0:aba381fc8158 365 /* Check to see if we've read the full payload */
zhjcpi 0:aba381fc8158 366 if (0 == (--pLocInst->msgSize))
zhjcpi 0:aba381fc8158 367 {
zhjcpi 0:aba381fc8158 368 pLocInst->computedCheckSum &= 0x7FFF;
zhjcpi 0:aba381fc8158 369 pLocInst->protoDetState = STATE_CHECKSUM1;
zhjcpi 0:aba381fc8158 370 }
zhjcpi 0:aba381fc8158 371 break;
zhjcpi 0:aba381fc8158 372
zhjcpi 0:aba381fc8158 373 case STATE_CHECKSUM1:
zhjcpi 0:aba381fc8158 374 pLocInst->checksum = data ;
zhjcpi 0:aba381fc8158 375 pLocInst->checksum <<= 8;
zhjcpi 0:aba381fc8158 376 pLocInst->protoDetState = STATE_CHECKSUM2;
zhjcpi 0:aba381fc8158 377 break;
zhjcpi 0:aba381fc8158 378
zhjcpi 0:aba381fc8158 379 case STATE_CHECKSUM2:
zhjcpi 0:aba381fc8158 380 pLocInst->checksum += data;
zhjcpi 0:aba381fc8158 381 if (pLocInst->computedCheckSum != pLocInst->checksum)
zhjcpi 0:aba381fc8158 382 {
zhjcpi 0:aba381fc8158 383 if (OSP_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 384 {
zhjcpi 0:aba381fc8158 385 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 386 }
zhjcpi 0:aba381fc8158 387 else
zhjcpi 0:aba381fc8158 388 {
zhjcpi 0:aba381fc8158 389 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 390 }
zhjcpi 0:aba381fc8158 391 }
zhjcpi 0:aba381fc8158 392 else
zhjcpi 0:aba381fc8158 393 {
zhjcpi 0:aba381fc8158 394 pLocInst->protoDetState = STATE_END1;
zhjcpi 0:aba381fc8158 395 }
zhjcpi 0:aba381fc8158 396 break;
zhjcpi 0:aba381fc8158 397
zhjcpi 0:aba381fc8158 398 case STATE_END1:
zhjcpi 0:aba381fc8158 399 if (OSP_MSG_TAIL0 == data)
zhjcpi 0:aba381fc8158 400 {
zhjcpi 0:aba381fc8158 401 pLocInst->protoDetState = STATE_END2;
zhjcpi 0:aba381fc8158 402 }
zhjcpi 0:aba381fc8158 403 else
zhjcpi 0:aba381fc8158 404 {
zhjcpi 0:aba381fc8158 405 if (OSP_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 406 {
zhjcpi 0:aba381fc8158 407 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 408 }
zhjcpi 0:aba381fc8158 409 else
zhjcpi 0:aba381fc8158 410 {
zhjcpi 0:aba381fc8158 411 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 412 }
zhjcpi 0:aba381fc8158 413 }
zhjcpi 0:aba381fc8158 414 break;
zhjcpi 0:aba381fc8158 415
zhjcpi 0:aba381fc8158 416 case STATE_END2:
zhjcpi 0:aba381fc8158 417 if (OSP_MSG_TAIL1 == data)
zhjcpi 0:aba381fc8158 418 {
zhjcpi 0:aba381fc8158 419 pLocInst->pTimeoutChk->detach();
zhjcpi 2:d4fe184925f2 420 pLocInst->bTimeoutFlag = false;
zhjcpi 0:aba381fc8158 421
zhjcpi 0:aba381fc8158 422 if(pLocInst->protoState == PROTO_STATE_DET_OSP || pLocInst->protoState == PROTO_STATE_DET_OSP_FROM_NMEA)
zhjcpi 0:aba381fc8158 423 {
zhjcpi 0:aba381fc8158 424 CSR_LOG_INFO("Checking OSP protocol OK.\r\n");
zhjcpi 0:aba381fc8158 425 pLocInst->protoState = PROTO_STATE_DET_OK;
zhjcpi 0:aba381fc8158 426 pLocInst->appEventCb(CSR_LOC_EVENT_START_RESULT, 0);
zhjcpi 0:aba381fc8158 427 }
zhjcpi 1:bbaf9b8d646a 428
zhjcpi 0:aba381fc8158 429 _CsrLocProcessRawOspPkt();
zhjcpi 0:aba381fc8158 430 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 431 }
zhjcpi 0:aba381fc8158 432 else
zhjcpi 0:aba381fc8158 433 {
zhjcpi 0:aba381fc8158 434 if (OSP_MSG_HEAD0 == data)
zhjcpi 0:aba381fc8158 435 {
zhjcpi 0:aba381fc8158 436 pLocInst->protoDetState = STATE_START2;
zhjcpi 0:aba381fc8158 437 }
zhjcpi 0:aba381fc8158 438 else
zhjcpi 0:aba381fc8158 439 {
zhjcpi 0:aba381fc8158 440 pLocInst->protoDetState = STATE_START1;
zhjcpi 0:aba381fc8158 441 }
zhjcpi 0:aba381fc8158 442 }
zhjcpi 0:aba381fc8158 443 break;
zhjcpi 0:aba381fc8158 444 } /* switch. */
zhjcpi 0:aba381fc8158 445
zhjcpi 0:aba381fc8158 446 }
zhjcpi 0:aba381fc8158 447
zhjcpi 0:aba381fc8158 448 void CsrLocation::_CsrLocProcessRawOspPkt(void)
zhjcpi 0:aba381fc8158 449 {
zhjcpi 0:aba381fc8158 450 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 0:aba381fc8158 451 tOspMsg *pOspMsg;
zhjcpi 2:d4fe184925f2 452 uint32_t msgSize;
zhjcpi 0:aba381fc8158 453 CsrResult result;
zhjcpi 0:aba381fc8158 454
zhjcpi 0:aba381fc8158 455 msgSize = _CsrLocCalcMsgSize();
zhjcpi 0:aba381fc8158 456 if(msgSize > 0)
zhjcpi 0:aba381fc8158 457 {
zhjcpi 0:aba381fc8158 458 msgSize += sizeof(tOspMsg);
zhjcpi 0:aba381fc8158 459 pOspMsg = (tOspMsg *)malloc(msgSize);
zhjcpi 0:aba381fc8158 460 if(pOspMsg != NULL)
zhjcpi 0:aba381fc8158 461 {
zhjcpi 0:aba381fc8158 462 memset(pOspMsg, 0, msgSize);
zhjcpi 0:aba381fc8158 463 }
zhjcpi 0:aba381fc8158 464 else
zhjcpi 0:aba381fc8158 465 {
zhjcpi 0:aba381fc8158 466 CSR_LOG_INFO("No memory for received OSP message.\r\n");
zhjcpi 0:aba381fc8158 467 return;
zhjcpi 0:aba381fc8158 468 }
zhjcpi 0:aba381fc8158 469 }
zhjcpi 0:aba381fc8158 470 else
zhjcpi 0:aba381fc8158 471 {
zhjcpi 0:aba381fc8158 472 /* discard the unprocessed message */
zhjcpi 0:aba381fc8158 473 return;
zhjcpi 0:aba381fc8158 474 }
zhjcpi 0:aba381fc8158 475
zhjcpi 0:aba381fc8158 476 result = _CsrLocDecodeOspPkt(pLocInst->serialPkt,pLocInst->decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length);
zhjcpi 0:aba381fc8158 477 if ( CSR_RESULT_SUCCESS == result)
zhjcpi 0:aba381fc8158 478 {
zhjcpi 0:aba381fc8158 479 _CsrLocProcessOspPkt(pOspMsg);
zhjcpi 0:aba381fc8158 480 }
zhjcpi 0:aba381fc8158 481
zhjcpi 0:aba381fc8158 482 free(pOspMsg);
zhjcpi 0:aba381fc8158 483 }
zhjcpi 0:aba381fc8158 484
zhjcpi 2:d4fe184925f2 485 uint32_t CsrLocation::_CsrLocCalcMsgSize(void)
zhjcpi 0:aba381fc8158 486 {
zhjcpi 0:aba381fc8158 487 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 2:d4fe184925f2 488 uint8_t *ptr = pLocInst->serialPkt;
zhjcpi 2:d4fe184925f2 489 uint32_t msgSize = 0;
zhjcpi 2:d4fe184925f2 490 uint32_t msgId;
zhjcpi 2:d4fe184925f2 491 uint8_t mid, sid = 0;
zhjcpi 0:aba381fc8158 492
zhjcpi 0:aba381fc8158 493 mid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 0:aba381fc8158 494 msgId = OSP_MAKE_MSG_ID(mid,sid);
zhjcpi 0:aba381fc8158 495
zhjcpi 0:aba381fc8158 496 if(OSP_MSG_PWR_MODE_RSP == msgId || OSP_MSG_MULTI_CONSTELLATION == msgId)
zhjcpi 0:aba381fc8158 497 {
zhjcpi 0:aba381fc8158 498 /* add the sub-id to the message id */
zhjcpi 0:aba381fc8158 499 sid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 0:aba381fc8158 500 msgId = OSP_MAKE_MSG_ID(mid,sid);
zhjcpi 0:aba381fc8158 501 }
zhjcpi 0:aba381fc8158 502
zhjcpi 0:aba381fc8158 503 switch(msgId)
zhjcpi 0:aba381fc8158 504 {
zhjcpi 0:aba381fc8158 505 case OSP_MSG_OK_TO_SEND:
zhjcpi 0:aba381fc8158 506 case OSP_MSG_PWR_MODE_FPM_RSP:
zhjcpi 0:aba381fc8158 507 case OSP_MSG_PWR_MODE_LPM_RSP:
zhjcpi 0:aba381fc8158 508 case OSP_MSG_HW_CONFIG_REQ :
zhjcpi 2:d4fe184925f2 509 msgSize = sizeof(uint8_t);
zhjcpi 0:aba381fc8158 510 break;
zhjcpi 0:aba381fc8158 511 case OSP_MSG_SW_VERSION :
zhjcpi 0:aba381fc8158 512 msgSize = MAX_VERSION_LENGTH;
zhjcpi 0:aba381fc8158 513 break;
zhjcpi 0:aba381fc8158 514 case OSP_MSG_GEODETIC_NAVIGATION:
zhjcpi 0:aba381fc8158 515 msgSize = sizeof(tLocPosResp);
zhjcpi 0:aba381fc8158 516 break;
zhjcpi 0:aba381fc8158 517 case OSP_MSG_GNSS_SAT_DATA:
zhjcpi 2:d4fe184925f2 518 msgSize = sizeof(uint8_t);
zhjcpi 0:aba381fc8158 519 break;
zhjcpi 0:aba381fc8158 520 case OSP_MSG_GNSS_NAV_DATA:
zhjcpi 2:d4fe184925f2 521 msgSize = sizeof(uint8_t);
zhjcpi 0:aba381fc8158 522 break;
zhjcpi 0:aba381fc8158 523
zhjcpi 0:aba381fc8158 524 default :
zhjcpi 0:aba381fc8158 525 msgSize = 0;
zhjcpi 0:aba381fc8158 526 break;
zhjcpi 0:aba381fc8158 527 }
zhjcpi 0:aba381fc8158 528
zhjcpi 0:aba381fc8158 529 return msgSize;
zhjcpi 0:aba381fc8158 530 }
zhjcpi 0:aba381fc8158 531
zhjcpi 2:d4fe184925f2 532 CsrResult CsrLocation::_CsrLocDecodeOspPkt( uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen)
zhjcpi 0:aba381fc8158 533 {
zhjcpi 0:aba381fc8158 534 CsrResult tRet = CSR_RESULT_SUCCESS;
zhjcpi 2:d4fe184925f2 535 uint8_t *ptr = pPayload;
zhjcpi 2:d4fe184925f2 536 uint32_t i;
zhjcpi 2:d4fe184925f2 537 uint8_t mid, sid = 0;
zhjcpi 0:aba381fc8158 538
zhjcpi 0:aba381fc8158 539 mid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 0:aba381fc8158 540 *pMsgId = OSP_MAKE_MSG_ID(mid,sid);
zhjcpi 0:aba381fc8158 541 *pMsgLen = 0;
zhjcpi 0:aba381fc8158 542
zhjcpi 0:aba381fc8158 543 /* add the sub-id to the message id */
zhjcpi 0:aba381fc8158 544 if (OSP_MSG_PWR_MODE_RSP == *pMsgId || OSP_MSG_MULTI_CONSTELLATION == *pMsgId)
zhjcpi 0:aba381fc8158 545 {
zhjcpi 0:aba381fc8158 546 sid = BINARY_IMPORT_UINT8(ptr);
zhjcpi 0:aba381fc8158 547 *pMsgId = OSP_MAKE_MSG_ID(mid,sid);
zhjcpi 0:aba381fc8158 548 }
zhjcpi 0:aba381fc8158 549
zhjcpi 0:aba381fc8158 550 switch (*pMsgId)
zhjcpi 0:aba381fc8158 551 {
zhjcpi 0:aba381fc8158 552 case OSP_MSG_SW_VERSION: /* 0x06 */
zhjcpi 0:aba381fc8158 553 *pMsgLen = BINARY_IMPORT_UINT8(ptr);
zhjcpi 0:aba381fc8158 554 ptr++;
zhjcpi 0:aba381fc8158 555 if(*pMsgLen >= MAX_VERSION_LENGTH)
zhjcpi 0:aba381fc8158 556 {
zhjcpi 0:aba381fc8158 557 tRet = CSR_RESULT_FAILURE;
zhjcpi 0:aba381fc8158 558 }
zhjcpi 0:aba381fc8158 559 else
zhjcpi 0:aba381fc8158 560 {
zhjcpi 0:aba381fc8158 561 memcpy(pMsgData, ptr, *pMsgLen);
zhjcpi 0:aba381fc8158 562 }
zhjcpi 0:aba381fc8158 563 break;
zhjcpi 0:aba381fc8158 564
zhjcpi 0:aba381fc8158 565 case OSP_MSG_OK_TO_SEND: /* 0x12 */
zhjcpi 2:d4fe184925f2 566 *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr);
zhjcpi 2:d4fe184925f2 567 *pMsgLen = sizeof(uint8_t);
zhjcpi 0:aba381fc8158 568 break;
zhjcpi 0:aba381fc8158 569
zhjcpi 0:aba381fc8158 570 case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */
zhjcpi 0:aba381fc8158 571 {
zhjcpi 0:aba381fc8158 572 tLocPosResp *pPos = (tLocPosResp*) pMsgData;
zhjcpi 2:d4fe184925f2 573 uint16_t valid;
zhjcpi 0:aba381fc8158 574
zhjcpi 0:aba381fc8158 575 valid = BINARY_IMPORT_UINT16(ptr);
zhjcpi 0:aba381fc8158 576 if(valid != 0)
zhjcpi 0:aba381fc8158 577 {
zhjcpi 0:aba381fc8158 578 tRet = CSR_RESULT_FAILURE;
zhjcpi 0:aba381fc8158 579 }
zhjcpi 0:aba381fc8158 580 else
zhjcpi 0:aba381fc8158 581 {
zhjcpi 0:aba381fc8158 582 *pMsgLen = sizeof(*pPos);
zhjcpi 0:aba381fc8158 583
zhjcpi 0:aba381fc8158 584 ptr += 2;
zhjcpi 0:aba381fc8158 585 pPos->gps_week = BINARY_IMPORT_UINT16(ptr);
zhjcpi 0:aba381fc8158 586 pPos->tow = BINARY_IMPORT_UINT32(ptr);
zhjcpi 0:aba381fc8158 587 ptr += 12;
zhjcpi 2:d4fe184925f2 588 pPos->lat = (double)BINARY_IMPORT_SINT32(ptr);
zhjcpi 0:aba381fc8158 589 pPos->lat *= 1e-7;
zhjcpi 2:d4fe184925f2 590 pPos->lon = (double)BINARY_IMPORT_SINT32(ptr);
zhjcpi 0:aba381fc8158 591 pPos->lon *= 1e-7;
zhjcpi 0:aba381fc8158 592 ptr += 4;
zhjcpi 2:d4fe184925f2 593 pPos->alt = (double)BINARY_IMPORT_SINT32(ptr);
zhjcpi 0:aba381fc8158 594 pPos->alt *= 1e-2;
zhjcpi 0:aba381fc8158 595 }
zhjcpi 0:aba381fc8158 596 break;
zhjcpi 0:aba381fc8158 597 }
zhjcpi 0:aba381fc8158 598
zhjcpi 0:aba381fc8158 599 case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */
zhjcpi 0:aba381fc8158 600 {
zhjcpi 0:aba381fc8158 601 tLocSvStatus *pSvStatus = &csrLocInst.svStatus;
zhjcpi 0:aba381fc8158 602
zhjcpi 0:aba381fc8158 603 *pMsgLen = sizeof(*pSvStatus);
zhjcpi 0:aba381fc8158 604
zhjcpi 0:aba381fc8158 605 ptr += 100;
zhjcpi 0:aba381fc8158 606 pSvStatus->svUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 0:aba381fc8158 607 pSvStatus->sbasSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 0:aba381fc8158 608 pSvStatus->gloSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 0:aba381fc8158 609 pSvStatus->qzssSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
zhjcpi 0:aba381fc8158 610 break;
zhjcpi 0:aba381fc8158 611 }
zhjcpi 0:aba381fc8158 612
zhjcpi 0:aba381fc8158 613 case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */
zhjcpi 0:aba381fc8158 614 {
zhjcpi 0:aba381fc8158 615 tLocSvStatus *pSvStatus = &csrLocInst.svStatus;
zhjcpi 2:d4fe184925f2 616 uint16_t week;
zhjcpi 2:d4fe184925f2 617 uint32_t tow;
zhjcpi 2:d4fe184925f2 618 uint32_t towSubMs;
zhjcpi 2:d4fe184925f2 619 uint8_t info;
zhjcpi 2:d4fe184925f2 620 int32_t nMsg = 0;
zhjcpi 2:d4fe184925f2 621 uint16_t satInfo;
zhjcpi 2:d4fe184925f2 622 uint16_t az;
zhjcpi 2:d4fe184925f2 623 uint16_t el;
zhjcpi 2:d4fe184925f2 624 uint16_t cno;
zhjcpi 2:d4fe184925f2 625 uint8_t gnssType;
zhjcpi 2:d4fe184925f2 626 uint16_t index = 0;
zhjcpi 0:aba381fc8158 627
zhjcpi 0:aba381fc8158 628 *pMsgLen = sizeof(*pSvStatus);
zhjcpi 0:aba381fc8158 629
zhjcpi 0:aba381fc8158 630 week = BINARY_IMPORT_UINT16(ptr);
zhjcpi 0:aba381fc8158 631 tow = BINARY_IMPORT_UINT32(ptr);
zhjcpi 0:aba381fc8158 632 towSubMs = BINARY_IMPORT_UINT32(ptr);
zhjcpi 0:aba381fc8158 633 ptr += 4;
zhjcpi 0:aba381fc8158 634 info = BINARY_IMPORT_UINT8(ptr);
zhjcpi 0:aba381fc8158 635
zhjcpi 0:aba381fc8158 636 nMsg = info & 0x0F;
zhjcpi 0:aba381fc8158 637 if(nMsg == 1)
zhjcpi 0:aba381fc8158 638 {
zhjcpi 0:aba381fc8158 639 memset(pSvStatus, 0, sizeof(tLocSvStatus));
zhjcpi 0:aba381fc8158 640 pSvStatus->gps_week = week;
zhjcpi 0:aba381fc8158 641 pSvStatus->tow = tow;
zhjcpi 0:aba381fc8158 642 pSvStatus->tow_sub_ms = towSubMs;
zhjcpi 0:aba381fc8158 643 }
zhjcpi 0:aba381fc8158 644
zhjcpi 0:aba381fc8158 645 ptr++;
zhjcpi 0:aba381fc8158 646 for (i = 0; i < GNSS_SAT_DATA_NUM_OF_SATS; i++)
zhjcpi 0:aba381fc8158 647 {
zhjcpi 0:aba381fc8158 648 satInfo = BINARY_IMPORT_UINT16(ptr);
zhjcpi 0:aba381fc8158 649 az = BINARY_IMPORT_UINT16(ptr);
zhjcpi 0:aba381fc8158 650 el = BINARY_IMPORT_UINT16(ptr);
zhjcpi 0:aba381fc8158 651 cno = BINARY_IMPORT_UINT16(ptr);
zhjcpi 0:aba381fc8158 652 ptr += 4;
zhjcpi 0:aba381fc8158 653
zhjcpi 2:d4fe184925f2 654 gnssType = (uint8_t)((satInfo>>13)&0x0003);
zhjcpi 0:aba381fc8158 655 if(0 == gnssType || 1 == gnssType) // GPS, SBAS, QZSS
zhjcpi 0:aba381fc8158 656 {
zhjcpi 0:aba381fc8158 657 index = pSvStatus->numOfSVs;
zhjcpi 0:aba381fc8158 658 if(index < LOC_MAX_GNSS_SVS && cno >0)
zhjcpi 0:aba381fc8158 659 {
zhjcpi 2:d4fe184925f2 660 pSvStatus->svList[index].prn = (uint8_t)(satInfo & 0xFF);
zhjcpi 2:d4fe184925f2 661 pSvStatus->svList[index].cno = (float)(cno/10.0); // Scale: 10
zhjcpi 2:d4fe184925f2 662 pSvStatus->svList[index].elevation = (float)(el/10.0); // Scale: 10
zhjcpi 2:d4fe184925f2 663 pSvStatus->svList[index].azimuth = (float)(az/10.0); // Scale: 10
zhjcpi 0:aba381fc8158 664 pSvStatus->numOfSVs++;
zhjcpi 0:aba381fc8158 665 pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn-1); // prn range: 1-32
zhjcpi 0:aba381fc8158 666 }
zhjcpi 0:aba381fc8158 667 }
zhjcpi 0:aba381fc8158 668 else if(2 == gnssType) // GLONASS
zhjcpi 0:aba381fc8158 669 {
zhjcpi 0:aba381fc8158 670 index = pSvStatus->numOfGloSVs;
zhjcpi 0:aba381fc8158 671 if(index < CODEC_GLO_MAX_CHANNELS && cno>0)
zhjcpi 0:aba381fc8158 672 {
zhjcpi 2:d4fe184925f2 673 int16_t freqChan = (satInfo & 0X1F00)>>8;
zhjcpi 2:d4fe184925f2 674 int16_t slotNum = (satInfo & 0X00FF);
zhjcpi 0:aba381fc8158 675 if(slotNum > 0)
zhjcpi 0:aba381fc8158 676 {
zhjcpi 0:aba381fc8158 677 if(freqChan & 0X0010)
zhjcpi 0:aba381fc8158 678 {
zhjcpi 0:aba381fc8158 679 freqChan |= 0xFFE0;
zhjcpi 0:aba381fc8158 680 }
zhjcpi 2:d4fe184925f2 681 pSvStatus->gloSvList[index].prn = (uint8_t)(freqChan + LOC_GLO_FREQ_OFFSET);
zhjcpi 2:d4fe184925f2 682 pSvStatus->gloSvList[index].sno = (uint8_t)slotNum;
zhjcpi 2:d4fe184925f2 683 pSvStatus->gloSvList[index].cno = (float)(cno/10.0); // Scale: 10
zhjcpi 2:d4fe184925f2 684 pSvStatus->gloSvList[index].elevation = (float)(el/10.0); // Scale: 10
zhjcpi 2:d4fe184925f2 685 pSvStatus->gloSvList[index].azimuth = (float)(az/10.0); // Scale: 10
zhjcpi 0:aba381fc8158 686 pSvStatus->numOfGloSVs++;
zhjcpi 0:aba381fc8158 687 pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn-LOC_GLO_FREQ_ID_START);
zhjcpi 0:aba381fc8158 688 }
zhjcpi 0:aba381fc8158 689 }
zhjcpi 0:aba381fc8158 690 }
zhjcpi 0:aba381fc8158 691 }
zhjcpi 0:aba381fc8158 692
zhjcpi 0:aba381fc8158 693 break;
zhjcpi 0:aba381fc8158 694 }
zhjcpi 0:aba381fc8158 695
zhjcpi 0:aba381fc8158 696 case OSP_MSG_HW_CONFIG_REQ: /* 0x47 */
zhjcpi 0:aba381fc8158 697 break;
zhjcpi 0:aba381fc8158 698
zhjcpi 0:aba381fc8158 699 case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */
zhjcpi 0:aba381fc8158 700 break;
zhjcpi 0:aba381fc8158 701
zhjcpi 0:aba381fc8158 702 case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */
zhjcpi 2:d4fe184925f2 703 *((uint8_t *)pMsgData) = *ptr;
zhjcpi 2:d4fe184925f2 704 *pMsgLen = sizeof(uint8_t);
zhjcpi 0:aba381fc8158 705 break;
zhjcpi 0:aba381fc8158 706
zhjcpi 0:aba381fc8158 707 default:
zhjcpi 0:aba381fc8158 708 tRet = CSR_RESULT_FAILURE;
zhjcpi 0:aba381fc8158 709 break;
zhjcpi 0:aba381fc8158 710 }
zhjcpi 0:aba381fc8158 711
zhjcpi 0:aba381fc8158 712 /* check if length does not match */
zhjcpi 0:aba381fc8158 713 if(tRet == CSR_RESULT_FAILURE)
zhjcpi 0:aba381fc8158 714 {
zhjcpi 0:aba381fc8158 715 *pMsgId = *pMsgLen = 0;
zhjcpi 0:aba381fc8158 716 }
zhjcpi 0:aba381fc8158 717
zhjcpi 0:aba381fc8158 718 return tRet;
zhjcpi 0:aba381fc8158 719
zhjcpi 0:aba381fc8158 720 } /* CsrUlocCodecSsbDecode() */
zhjcpi 0:aba381fc8158 721
zhjcpi 0:aba381fc8158 722 void CsrLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg)
zhjcpi 0:aba381fc8158 723 {
zhjcpi 0:aba381fc8158 724 switch(pOspMsg->msgId)
zhjcpi 0:aba381fc8158 725 {
zhjcpi 0:aba381fc8158 726 case OSP_MSG_GEODETIC_NAVIGATION:
zhjcpi 0:aba381fc8158 727 csrLocInst.appOutCb(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp));
zhjcpi 0:aba381fc8158 728 break;
zhjcpi 0:aba381fc8158 729 case OSP_MSG_GNSS_SAT_DATA:
zhjcpi 0:aba381fc8158 730 break;
zhjcpi 0:aba381fc8158 731 case OSP_MSG_GNSS_NAV_DATA:
zhjcpi 0:aba381fc8158 732 csrLocInst.appOutCb(LOC_OUTPUT_SV_STATUS, &csrLocInst.svStatus, sizeof(tLocSvStatus));
zhjcpi 0:aba381fc8158 733 break;
zhjcpi 0:aba381fc8158 734 case OSP_MSG_OK_TO_SEND:
zhjcpi 0:aba381fc8158 735 csrLocInst.engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND;
zhjcpi 0:aba381fc8158 736 CSR_LOG_INFO("Ok to send %u\r\n", csrLocInst.engStatus);
zhjcpi 0:aba381fc8158 737 break;
zhjcpi 0:aba381fc8158 738 case OSP_MSG_SW_VERSION:
zhjcpi 2:d4fe184925f2 739 csrLocInst.bVerRsp = true;
zhjcpi 0:aba381fc8158 740 CSR_LOG_INFO("Ver: %s\r\n", pOspMsg->payload);
zhjcpi 0:aba381fc8158 741 break;
zhjcpi 0:aba381fc8158 742 case OSP_MSG_HW_CONFIG_REQ:
zhjcpi 0:aba381fc8158 743 CSR_LOG_INFO("hw config req.\r\n");
zhjcpi 2:d4fe184925f2 744 if(!csrLocInst.bVerRsp)
zhjcpi 1:bbaf9b8d646a 745 {
zhjcpi 0:aba381fc8158 746 _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
zhjcpi 0:aba381fc8158 747 }
zhjcpi 1:bbaf9b8d646a 748
zhjcpi 1:bbaf9b8d646a 749 if(!csrLocInst.bPwrModeRsp)
zhjcpi 0:aba381fc8158 750 {
zhjcpi 1:bbaf9b8d646a 751 if(csrLocInst.pwrMode == PWR_PTF)
zhjcpi 1:bbaf9b8d646a 752 {
zhjcpi 1:bbaf9b8d646a 753 _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
zhjcpi 1:bbaf9b8d646a 754 }
zhjcpi 1:bbaf9b8d646a 755 else
zhjcpi 1:bbaf9b8d646a 756 {
zhjcpi 1:bbaf9b8d646a 757 _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
zhjcpi 1:bbaf9b8d646a 758 }
zhjcpi 0:aba381fc8158 759 }
zhjcpi 0:aba381fc8158 760 break;
zhjcpi 0:aba381fc8158 761 case OSP_MSG_PWR_MODE_LPM_RSP:
zhjcpi 2:d4fe184925f2 762 csrLocInst.bPwrModeRsp = true;
zhjcpi 0:aba381fc8158 763 CSR_LOG_INFO("lpm response.\r\n");
zhjcpi 0:aba381fc8158 764 break;
zhjcpi 0:aba381fc8158 765 case OSP_MSG_PWR_MODE_FPM_RSP:
zhjcpi 2:d4fe184925f2 766 csrLocInst.bPwrModeRsp = true;
zhjcpi 0:aba381fc8158 767 CSR_LOG_INFO("fpm response.\r\n");
zhjcpi 0:aba381fc8158 768 break;
zhjcpi 0:aba381fc8158 769 default:
zhjcpi 0:aba381fc8158 770 CSR_LOG_INFO("Unknown OSP message 0x%lx.\r\n", pOspMsg->msgId);
zhjcpi 0:aba381fc8158 771 break;
zhjcpi 0:aba381fc8158 772 }
zhjcpi 0:aba381fc8158 773 }
zhjcpi 0:aba381fc8158 774
zhjcpi 0:aba381fc8158 775 void CsrLocation::_CsrLocTimeout(void)
zhjcpi 0:aba381fc8158 776 {
zhjcpi 2:d4fe184925f2 777 csrLocInst.bTimeoutFlag = true;
zhjcpi 0:aba381fc8158 778 }
zhjcpi 0:aba381fc8158 779
zhjcpi 0:aba381fc8158 780 void CsrLocation::_CsrLocRxHandler(void)
zhjcpi 0:aba381fc8158 781 {
zhjcpi 0:aba381fc8158 782 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 0:aba381fc8158 783
zhjcpi 0:aba381fc8158 784 pLocInst->serialBuf[pLocInst->in++] = pLocInst->pSerialLoc->getc();
zhjcpi 0:aba381fc8158 785 pLocInst->in &= (MAX_SERIAL_BUF_LEN-1);
zhjcpi 0:aba381fc8158 786 if(pLocInst->in == pLocInst->out)
zhjcpi 0:aba381fc8158 787 {
zhjcpi 1:bbaf9b8d646a 788 CSR_LOG_INFO("rx overwritten %d %d.\r\n", pLocInst->in, pLocInst->out);
zhjcpi 0:aba381fc8158 789 }
zhjcpi 0:aba381fc8158 790 }
zhjcpi 0:aba381fc8158 791
zhjcpi 0:aba381fc8158 792 void CsrLocation::_CsrLocSendData(eSendDataType type)
zhjcpi 0:aba381fc8158 793 {
zhjcpi 0:aba381fc8158 794 tCsrLocInst *pLocInst = &csrLocInst;
zhjcpi 2:d4fe184925f2 795 uint32_t i, size;
zhjcpi 2:d4fe184925f2 796 const uint8_t *pData;
zhjcpi 0:aba381fc8158 797
zhjcpi 0:aba381fc8158 798 switch(type)
zhjcpi 0:aba381fc8158 799 {
zhjcpi 0:aba381fc8158 800 case SEND_DATA_TYPE_OSP_STOP_REQ:
zhjcpi 0:aba381fc8158 801 pData = sOspStopReq;
zhjcpi 0:aba381fc8158 802 size = sizeof(sOspStopReq);
zhjcpi 0:aba381fc8158 803 break;
zhjcpi 0:aba381fc8158 804 case SEND_DATA_TYPE_OSP_VER_REQ:
zhjcpi 0:aba381fc8158 805 pData = sOspVerReq;
zhjcpi 0:aba381fc8158 806 size = sizeof(sOspVerReq);
zhjcpi 0:aba381fc8158 807 break;
zhjcpi 0:aba381fc8158 808 case SEND_DATA_TYPE_OSP_LPM_REQ:
zhjcpi 0:aba381fc8158 809 pData = sOspLpmReq;
zhjcpi 0:aba381fc8158 810 size = sizeof(sOspLpmReq);
zhjcpi 0:aba381fc8158 811 break;
zhjcpi 0:aba381fc8158 812 case SEND_DATA_TYPE_OSP_FPM_REQ:
zhjcpi 0:aba381fc8158 813 pData = sOspFpmReq;
zhjcpi 0:aba381fc8158 814 size = sizeof(sOspFpmReq);
zhjcpi 0:aba381fc8158 815 break;
zhjcpi 0:aba381fc8158 816 case SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ:
zhjcpi 0:aba381fc8158 817 pData = sOspSwitch2NmeaReq;
zhjcpi 0:aba381fc8158 818 size = sizeof(sOspSwitch2NmeaReq);
zhjcpi 0:aba381fc8158 819 break;
zhjcpi 0:aba381fc8158 820 case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ:
zhjcpi 2:d4fe184925f2 821 pData = (const uint8_t *)sNmeaSwitch2OspReq;
zhjcpi 0:aba381fc8158 822 size = strlen(sNmeaSwitch2OspReq);
zhjcpi 0:aba381fc8158 823 break;
zhjcpi 0:aba381fc8158 824
zhjcpi 0:aba381fc8158 825 default:
zhjcpi 0:aba381fc8158 826 pData = NULL;
zhjcpi 0:aba381fc8158 827 }
zhjcpi 0:aba381fc8158 828
zhjcpi 0:aba381fc8158 829 if(pData != NULL)
zhjcpi 0:aba381fc8158 830 {
zhjcpi 0:aba381fc8158 831 for (i = 0; i < size; i++)
zhjcpi 0:aba381fc8158 832 {
zhjcpi 0:aba381fc8158 833 pLocInst->pSerialLoc->putc(pData[i]);
zhjcpi 0:aba381fc8158 834 }
zhjcpi 0:aba381fc8158 835 }
zhjcpi 0:aba381fc8158 836 }
zhjcpi 0:aba381fc8158 837
zhjcpi 0:aba381fc8158 838 void CsrLocation::_CsrLocHwOnoff(void)
zhjcpi 0:aba381fc8158 839 {
zhjcpi 0:aba381fc8158 840 csrLocInst.pPinOnoff->write(1);
zhjcpi 0:aba381fc8158 841 wait_ms(10);
zhjcpi 0:aba381fc8158 842 csrLocInst.pPinOnoff->write(0);
zhjcpi 0:aba381fc8158 843 CSR_LOG_INFO("Onoff pulse given.\r\n");
zhjcpi 0:aba381fc8158 844 }
zhjcpi 0:aba381fc8158 845
zhjcpi 0:aba381fc8158 846 void CsrLocation::_CsrLocHwReset(void)
zhjcpi 0:aba381fc8158 847 {
zhjcpi 0:aba381fc8158 848 csrLocInst.pPinReset->write(1);
zhjcpi 0:aba381fc8158 849 wait_ms(10);
zhjcpi 0:aba381fc8158 850 csrLocInst.pPinReset->write(0);
zhjcpi 0:aba381fc8158 851 CSR_LOG_INFO("Reset pulse given.\r\n");
zhjcpi 0:aba381fc8158 852 }