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@1:bbaf9b8d646a, 2014-03-25 (annotated)

Committer:
zhjcpi
Date:
Tue Mar 25 05:26:26 2014 +0000
Revision:
1:bbaf9b8d646a
Parent:
0:aba381fc8158
Child:
2:d4fe184925f2
optimization csrLocation class

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