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@0:aba381fc8158, 2014-03-24 (annotated)

Committer:
zhjcpi
Date:
Mon Mar 24 08:23:25 2014 +0000
Revision:
0:aba381fc8158
Child:
1:bbaf9b8d646a
First edition for the CsrLocation lib.

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