CsrLocation.cpp

00001 /* CSRLocation class for mbed Microcontroller
00002  * Copyright 2014 CSR plc
00003  */
00004 
00005 #include "mbed.h"
00006 #include "CsrLocation.h"
00007 
00008 static uint8_t sOspStopReq[] = {0xa0, 0xa2, 0x00, 0x02, 0xcd, 0x10, 0x00, 0xdd, 0xb0, 0xb3};
00009 static uint8_t sOspVerReq[] = {0xA0, 0xA2, 0x00, 0x02, 0x84, 0x00, 0x00, 0x84, 0xB0, 0xB3};
00010 static uint8_t sOspLpmReq[] = {0xA0, 0xA2, 0x00, 0x2A, 0xDA, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1E, 0x00, 0x01, 0x00, 0x78, 0x00, 0x1E,
00011                                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00012                                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x63, 0xB0, 0xB3};
00013 static uint8_t sOspFpmReq[] = {0xA0, 0xA2, 0x00, 0x03, 0xDA, 0x00, 0x00, 0x00, 0xDA, 0xB0, 0xB3};
00014 static uint8_t sOspSwitch2NmeaReq[] = {0xA0, 0xA2, 0x00, 0x18, 0x81, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00,
00015                                        0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x12, 0xC0, 0x01, 0x64, 0xB0, 0xB3};
00016 static char    sNmeaSwitch2OspReq[] = "$PSRF100,0,115200,8,1,0*04\r\n";
00017 static char    sNmeaStopReq[]       = "$PSRF117,16*0B\r\n";
00018 
00019 CSRLocation::CSRLocation(RawSerial  &_serialLoc,
00020                          DigitalOut &_pinOnoff,
00021                          DigitalOut &_pinReset,
00022                          DigitalIn  &_wakeup,
00023                          Serial     *debugP):
00024     serialLoc(_serialLoc),
00025     pinOnoff(_pinOnoff),
00026     pinReset(_pinReset),
00027     wakeup(_wakeup),
00028     proto(PROTO_OSP),
00029     locState(CSR_LOC_STATE_IDLE),
00030     protoState(PROTO_STATE_DET_INVALID),
00031     pwrMode(PWR_FULL),
00032     baudRate(),
00033     computedCheckSum(),
00034     checksum(),
00035     msgSize(),
00036     decodeIndex(),
00037     protoDetState(STATE_START1),
00038     pTimeoutChk(NULL),
00039     bTimeoutFlag(false),
00040     engStatus(ENGINE_STATUS_NOTOK2SEND),
00041     svStatus(),
00042     serialBuf(),
00043     serialPkt(),
00044     in(),
00045     out(),
00046     pSerialDebug(debugP)
00047 {
00048     pTimeoutChk = new Timeout();
00049 
00050     pinReset.write(1);
00051     pinOnoff.write(0);
00052 }
00053 
00054 CSRLocation::~CSRLocation(void)
00055 {
00056     serialLoc.attach(NULL);
00057 
00058     if (pTimeoutChk != NULL) {
00059         delete pTimeoutChk;
00060         pTimeoutChk = NULL;
00061     }
00062     pinReset.write(0);
00063     pinOnoff.write(0);
00064 }
00065 
00066 void CSRLocation::CsrLocRegOutput(csr_app_output_callback app_output_cb, csr_app_event_callback app_event_cb)
00067 {
00068     appOutCb   = app_output_cb;
00069     appEventCb = app_event_cb;
00070 }
00071 
00072 void
00073 CSRLocation::outputHandler(uint32_t msgId, void *const pMsgData, uint32_t msgLength)
00074 {
00075     switch (msgId) {
00076         case LOC_OUTPUT_LOCATION: {
00077             const tLocPosResp *pPosRsp      = (const tLocPosResp *)pMsgData;
00078             lastLocation.version            = 1;
00079             lastLocation.valid              = true;
00080             lastLocation.gpsTime.gps_week   = pPosRsp->gpsTime.gps_week;
00081             lastLocation.gpsTime.tow        = pPosRsp->gpsTime.tow;
00082             lastLocation.utcTime            = pPosRsp->utcTime;
00083             lastLocation.lat                = pPosRsp->lat;
00084             lastLocation.lon                = pPosRsp->lon;
00085             lastLocation.altitude           = pPosRsp->alt;
00086 
00087             if (locationCallback) {
00088                 locationCallback(&lastLocation);
00089             }
00090             break;
00091         }
00092         case LOC_OUTPUT_SV_STATUS: {
00093             const tLocSvStatus *pSvStatus   = (const tLocSvStatus *)pMsgData;
00094             lastLocation.version            = 1;
00095             lastLocation.valid              = true;
00096             lastLocation.numGPSSVs          = pSvStatus->numOfSVs;
00097             lastLocation.numGLOSVs          = pSvStatus->numOfGloSVs;
00098 
00099             break;
00100         }
00101 
00102         default:
00103             break;
00104     }
00105 
00106     if (appOutCb) {
00107         appOutCb(msgId, pMsgData, msgLength);
00108     }
00109 }
00110 
00111 void
00112 CSRLocation::eventHandler(eCsrLocEventType event, uint32_t data)
00113 {
00114     if (appEventCb) {
00115         appEventCb(event, data);
00116     }
00117 }
00118 
00119 void CSRLocation::reset(void)
00120 {
00121     _CsrLocHwReset();
00122     lastLocation.valid = false;
00123 }
00124 
00125 void CSRLocation::start()
00126 {
00127     if (locState == CSR_LOC_STATE_IDLE) {
00128         /* Csr Location SDK version */
00129         CSR_LOG_INFO("==== CSR LOC SDK version: %s Date: %s ====\r\n", CSR_LOC_SDK_VER, __DATE__ "  " __TIME__);
00130 
00131         /* trigger on_off */
00132         _CsrLocHwOnoff();
00133 
00134         /* open UART */
00135         if (proto == PROTO_NMEA) {
00136             CSR_LOG_INFO("Checking NMEA protocol...\r\n");
00137             protoState = PROTO_STATE_DET_NMEA;
00138             baudRate   = BAUDRATE_NMEA;
00139         } else {
00140             CSR_LOG_INFO("Checking OSP protocol...\r\n");
00141             protoState = PROTO_STATE_DET_OSP;
00142             baudRate   = BAUDRATE_OSP;
00143         }
00144         _CsrLocUartInit();
00145 
00146         locState    = CSR_LOC_STATE_RUN;
00147         bPwrModeRsp = false;
00148         bVerRsp     = false;
00149     } else {
00150         CSR_LOG_INFO("Already started.\r\n");
00151     }
00152 }
00153 
00154 void CSRLocation::process(void)
00155 {
00156     if (locState == CSR_LOC_STATE_RUN) {
00157         /* wait and process uart data */
00158         _CsrLocProcessRawStream();
00159     }
00160 }
00161 
00162 void CSRLocation::stop(void)
00163 {
00164     if (locState == CSR_LOC_STATE_IDLE) {
00165         return;
00166     }
00167 
00168     serialLoc.attach(NULL);
00169     CSR_LOG_INFO("Stop command processed.\r\n");
00170 
00171     if ((locState == CSR_LOC_STATE_RUN) && (pwrMode == PWR_PTF) && (engStatus == ENGINE_STATUS_NOTOK2SEND)) {
00172         /* in sleep mode, trigger on_off firstly */
00173         _CsrLocHwOnoff();
00174         wait_ms(500);
00175     }
00176     if (proto == PROTO_NMEA) {
00177         _CsrLocSendData(SEND_DATA_TYPE_NMEA_STOP_REQ);
00178     } else {
00179         _CsrLocSendData(SEND_DATA_TYPE_OSP_STOP_REQ);
00180     }
00181     wait_ms(200);
00182     if (_CsrLocIsWakeup()) {
00183         _CsrLocHwOnoff();
00184         wait_ms(300);
00185         if (_CsrLocIsWakeup()) {
00186             _CsrLocHwReset();
00187         }
00188     }
00189 
00190     locState = CSR_LOC_STATE_IDLE;
00191     eventHandler(CSR_LOC_EVENT_STOP_RESULT, 0);
00192 }
00193 
00194 uint32_t CSRLocation::ioctl(uint32_t command, void *arg)
00195 {
00196     uint32_t ret = 0;
00197     
00198     CSR_LOG_INFO("ioctl command 0x%lx\r\n", command);
00199     switch(command)
00200     {
00201     case CSR_IOCTL_CMD_PROTO_NMEA:
00202         CSR_LOG_INFO("set NMEA protocol.\r\n");
00203         proto = PROTO_NMEA;
00204         break;
00205     case CSR_IOCTL_CMD_PROTO_OSP:
00206         CSR_LOG_INFO("set OSP protocol.\r\n");
00207         proto = PROTO_OSP;
00208         break;
00209     case CSR_IOCTL_CMD_WAKEUP_STATUS:
00210         CSR_LOG_INFO("wakeup status : %d.\r\n", wakeup.read());
00211         break;
00212     case CSR_IOCTL_CMD_ONOFF_ON:
00213         CSR_LOG_INFO("onoff ON.\r\n");
00214         pinOnoff.write(1);
00215         break;
00216     case CSR_IOCTL_CMD_ONOFF_OFF:
00217         CSR_LOG_INFO("onoff OFF.\r\n");
00218         pinOnoff.write(0);
00219         break;
00220     case CSR_IOCTL_CMD_ONOFF_PULSE:
00221         CSR_LOG_INFO("onoff pulse.\r\n");
00222         pinOnoff.write(1);
00223         wait_ms(100);
00224         pinOnoff.write(0);
00225         break;
00226     case CSR_IOCTL_CMD_RESET_ON:
00227         CSR_LOG_INFO("reset ON.\r\n");
00228         pinReset.write(1);
00229         break;
00230     case CSR_IOCTL_CMD_RESET_OFF:
00231         CSR_LOG_INFO("reset OFF.\r\n");
00232         pinReset.write(0);
00233         break;
00234 
00235     default:
00236         CSR_LOG_INFO("Unknown ioctl command 0x%lx\r\n", command);
00237         ret = 0xFFFFFFFF;
00238         break;
00239     }
00240     
00241     return ret;
00242 }
00243 
00244 void CSRLocation::lpmGetImmediateLocation(void)
00245 {
00246     if ((locState == CSR_LOC_STATE_RUN) && (pwrMode == PWR_PTF) && (engStatus == ENGINE_STATUS_NOTOK2SEND)) {
00247         CSR_LOG_INFO("LpmGetPos ");
00248         _CsrLocHwOnoff();
00249     } else {
00250         if (locState != CSR_LOC_STATE_RUN) {
00251             CSR_LOG_INFO("Not in run state.\r\n");
00252         } else if (pwrMode != PWR_PTF) {
00253             CSR_LOG_INFO("Not in low power PTF mode.\r\n");
00254         }
00255     }
00256 }
00257 
00258 void CSRLocation::CsrLocDebugSwitch2Nmea(void)
00259 {
00260     if (locState == CSR_LOC_STATE_RUN) {
00261         _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ);
00262         wait_ms(200);
00263         _CsrLocHwReset();
00264     }
00265 }
00266 
00267 void CSRLocation::_CsrLocUartInit(void)
00268 {
00269     in  = 0;
00270     out = 0;
00271     memset(serialBuf, 0, MAX_SERIAL_BUF_LEN);
00272     memset(serialPkt, 0, MAX_SERIAL_PKT_LEN);
00273 
00274     checksum      = 0;
00275     msgSize       = 0;
00276     decodeIndex   = 0;
00277     protoDetState = STATE_START1;
00278     pTimeoutChk->attach(this, &CSRLocation::_CsrLocTimeout, PROTO_CHECK_TIMEOUT);
00279 
00280     serialLoc.baud(baudRate);
00281     serialLoc.attach(this, &CSRLocation::_CsrLocRxHandler );
00282 }
00283 
00284 void CSRLocation::_CsrLocProcessRawStream(void)
00285 {
00286     uint8_t data;
00287 
00288     if (bTimeoutFlag) {
00289         pTimeoutChk->detach();
00290         bTimeoutFlag = false;
00291         if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) {
00292             _CsrLocDetProtoOspTimeout();
00293         } else if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) {
00294             _CsrLocDetProtoNmeaTimeout();
00295         } else {
00296             CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", protoState);
00297         }
00298         return;
00299     }
00300 
00301     while (in != out) {
00302         if (bTimeoutFlag) {
00303             pTimeoutChk->detach();
00304             bTimeoutFlag = false;
00305             if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) {
00306                 _CsrLocDetProtoOspTimeout();
00307             } else if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) {
00308                 _CsrLocDetProtoNmeaTimeout();
00309             } else {
00310                 CSR_LOG_INFO("timeout in unknown protocol state %d.\r\n", protoState);
00311             }
00312             return;
00313         }
00314 
00315         data = serialBuf[out++];
00316         out &= (MAX_SERIAL_BUF_LEN - 1);
00317         switch (protoState) {
00318             case PROTO_STATE_DET_OSP:
00319             case PROTO_STATE_DET_OSP_FROM_NMEA:
00320                 _CsrLocProcessRawOspStream(data);
00321                 break;
00322             case PROTO_STATE_DET_NMEA:
00323             case PROTO_STATE_DET_NMEA_FROM_OSP:
00324                 _CsrLocProcessRawNmeaStream(data);
00325                 break;
00326             case PROTO_STATE_DET_OK:
00327                 if (proto == PROTO_NMEA) {
00328                     _CsrLocProcessRawNmeaStream(data);
00329                 } else {
00330                     _CsrLocProcessRawOspStream(data);
00331                 }
00332                 break;
00333             default:
00334                 /* Discard received data */
00335                 //out = pLocInst->in;
00336                 break;
00337         }
00338     }
00339 }
00340 
00341 void CSRLocation::_CsrLocDetProtoOspTimeout(void)
00342 {
00343     if (proto == PROTO_NMEA) {
00344         /* Failed to detect OSP */
00345         serialLoc.attach(NULL);
00346         protoState = PROTO_STATE_DET_INVALID;
00347         baudRate   = BAUDRATE_NMEA;
00348         CSR_LOG_INFO("Checking OSP failed.\r\n");
00349         eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
00350     } else {
00351         /* Failed to detect OSP and try to detect NMEA */
00352         serialLoc.attach(NULL);
00353         if (protoState == PROTO_STATE_DET_OSP) {
00354             protoState = PROTO_STATE_DET_NMEA;
00355             baudRate   = BAUDRATE_NMEA;
00356             CSR_LOG_INFO("Checking OSP protocol failed, now check NMEA protocol...\r\n");
00357             _CsrLocUartInit();
00358         } else {
00359             protoState = PROTO_STATE_DET_INVALID;
00360             CSR_LOG_INFO("Checking switched OSP protocol failed.\r\n");
00361             eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
00362         }
00363     }
00364 }
00365 
00366 void CSRLocation::_CsrLocDetProtoNmeaTimeout(void)
00367 {
00368     CSR_LOG_INFO("Checking NMEA protocol failed\r\n");
00369 
00370     if (proto == PROTO_NMEA) {
00371         /* Failed to detect NMEA and try to detect OSP */
00372         serialLoc.attach(NULL);
00373         if (protoState == PROTO_STATE_DET_NMEA) {
00374             protoState = PROTO_STATE_DET_OSP;
00375             baudRate   = BAUDRATE_OSP;
00376             CSR_LOG_INFO("Checking NMEA protocol failed, now check OSP protocol...\r\n");
00377             _CsrLocUartInit();
00378         } else {
00379             protoState = PROTO_STATE_DET_INVALID;
00380             CSR_LOG_INFO("Checking switched NMEA protocol failed.\r\n");
00381             //            eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
00382         }
00383     } else {
00384         /* Failed to detect NEMA */
00385         serialLoc.attach(NULL);
00386         protoState = PROTO_STATE_DET_INVALID;
00387         baudRate   = BAUDRATE_OSP;
00388         CSR_LOG_INFO("Checking NMEA failed.\r\n");
00389         eventHandler(CSR_LOC_EVENT_START_RESULT, 1);
00390     }
00391 }
00392 
00393 void CSRLocation::_CsrLocProcessRawNmeaStream(uint8_t data)
00394 {
00395     switch (protoDetState) {
00396         case STATE_START1:
00397             if (NMEA_MSG_HEAD0 == data) {
00398                 protoDetState = STATE_START2;
00399             }
00400             break;
00401 
00402         case STATE_START2:
00403             if (NMEA_MSG_HEAD1 == data) {
00404                 protoDetState            = STATE_END1;
00405                 decodeIndex              = 0;
00406                 serialPkt[decodeIndex++] = data;
00407             } else if (NMEA_MSG_HEAD0 == data) {
00408                 protoDetState = STATE_START2;
00409             } else {
00410                 protoDetState = STATE_START1;
00411             }
00412             break;
00413 
00414         case STATE_END1:
00415             if (NMEA_MSG_TAIL0 == data) {
00416                 pTimeoutChk->detach();
00417                 bTimeoutFlag = false;
00418                 if (proto == PROTO_NMEA) {
00419                     protoDetState = STATE_START1;
00420                     if ((protoState == PROTO_STATE_DET_NMEA) || (protoState == PROTO_STATE_DET_NMEA_FROM_OSP)) {
00421                         CSR_LOG_INFO("Checking NMEA protocol OK.\r\n");
00422                         protoState = PROTO_STATE_DET_OK;
00423                         eventHandler(CSR_LOC_EVENT_START_RESULT, 0);
00424                     }
00425 
00426                     serialPkt[decodeIndex++] = '\0';
00427                     _CsrLocProcessRawNmeaPkt();
00428                 } else {
00429                     serialLoc.attach(NULL);
00430 
00431                     CSR_LOG_INFO("Checking NMEA protocol OK, switching to OSP...\r\n");
00432                     _CsrLocSendData(SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ);
00433                     wait_ms(100);
00434                     protoState = PROTO_STATE_DET_OSP_FROM_NMEA;
00435                     baudRate   = BAUDRATE_OSP;
00436                     CSR_LOG_INFO("Checking switched OSP protocol...\r\n");
00437                     _CsrLocUartInit();
00438                 }
00439             } else if (NMEA_MSG_HEAD0 == data) {
00440                 protoDetState = STATE_START2;
00441             } else {
00442                 if (decodeIndex < (MAX_SERIAL_PKT_LEN - 2)) {
00443                     serialPkt[decodeIndex++] = data;
00444                 } else {
00445                     protoDetState = STATE_START1;
00446                 }
00447             }
00448             break;
00449 
00450         default:
00451             break;
00452     }
00453 }
00454 
00455 void CSRLocation::_CsrLocProcessRawOspStream(uint8_t data)
00456 {
00457     switch (protoDetState) {
00458         case STATE_START1:
00459             if (OSP_MSG_HEAD0 == data) {
00460                 protoDetState = STATE_START2;
00461             }
00462             break;
00463 
00464         case STATE_START2:
00465             if (OSP_MSG_HEAD1 == data) {
00466                 protoDetState = STATE_SIZE1;
00467             } else if (OSP_MSG_HEAD0 == data) {
00468                 protoDetState = STATE_START2;
00469             } else {
00470                 protoDetState = STATE_START1;
00471             }
00472             break;
00473 
00474         case STATE_SIZE1:
00475             msgSize       = data;
00476             msgSize     <<= 8; /* high uint8_t */
00477             protoDetState = STATE_SIZE2;
00478             break;
00479 
00480         case STATE_SIZE2:
00481             msgSize += data;
00482             if ((MAX_SERIAL_PKT_LEN < msgSize) || (0 == msgSize)) {
00483                 if (OSP_MSG_HEAD0 == data) {
00484                     protoDetState = STATE_START2;
00485                 } else {
00486                     protoDetState = STATE_START1;
00487                 }
00488             } else {
00489                 computedCheckSum = 0;
00490                 decodeIndex      = 0;
00491                 protoDetState    = STATE_PAYLOAD;
00492             }
00493             break;
00494 
00495         case STATE_PAYLOAD:
00496             /* check for a catastrophic error case */
00497             if (MAX_SERIAL_PKT_LEN <= decodeIndex) {
00498                 /* This is really bad.  And should never happen since we
00499                  * gurantee that msgSize is always less than RECEIVE_PACKET_SIZE
00500                  * Change the state back to STATE_START1 and start over */
00501                 if (OSP_MSG_HEAD0 == data) {
00502                     protoDetState = STATE_START2;
00503                 } else {
00504                     protoDetState = STATE_START1;
00505                 }
00506                 break;
00507             }
00508             /* Store the byte */
00509             serialPkt[decodeIndex++] = data;
00510             computedCheckSum        += data;
00511 
00512             /* Check to see if we've read the full payload */
00513             if (0 == (--msgSize)) {
00514                 computedCheckSum &= 0x7FFF;
00515                 protoDetState     = STATE_CHECKSUM1;
00516             }
00517             break;
00518 
00519         case STATE_CHECKSUM1:
00520             checksum      = data;
00521             checksum    <<= 8;
00522             protoDetState = STATE_CHECKSUM2;
00523             break;
00524 
00525         case STATE_CHECKSUM2:
00526             checksum += data;
00527             if (computedCheckSum != checksum) {
00528                 if (OSP_MSG_HEAD0 == data) {
00529                     protoDetState = STATE_START2;
00530                 } else {
00531                     protoDetState = STATE_START1;
00532                 }
00533             } else {
00534                 protoDetState = STATE_END1;
00535             }
00536             break;
00537 
00538         case STATE_END1:
00539             if (OSP_MSG_TAIL0 == data) {
00540                 protoDetState = STATE_END2;
00541             } else {
00542                 if (OSP_MSG_HEAD0 == data) {
00543                     protoDetState = STATE_START2;
00544                 } else {
00545                     protoDetState = STATE_START1;
00546                 }
00547             }
00548             break;
00549 
00550         case STATE_END2:
00551             if (OSP_MSG_TAIL1 == data) {
00552                 pTimeoutChk->detach();
00553                 bTimeoutFlag  = false;
00554                 protoDetState = STATE_START1;
00555 
00556                 if (proto == PROTO_NMEA) {
00557                     serialLoc.attach(NULL);
00558 
00559                     CSR_LOG_INFO("Checking OSP protocol OK, switching to NMEA...\r\n");
00560                     _CsrLocSendData(SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ);
00561                     wait_ms(100);
00562                     protoState = PROTO_STATE_DET_NMEA_FROM_OSP;
00563                     baudRate   = BAUDRATE_NMEA;
00564                     CSR_LOG_INFO("Checking switched NMEA protocol...\r\n");
00565                     _CsrLocUartInit();
00566                 } else {
00567                     if ((protoState == PROTO_STATE_DET_OSP) || (protoState == PROTO_STATE_DET_OSP_FROM_NMEA)) {
00568                         CSR_LOG_INFO("Checking OSP protocol OK.\r\n");
00569                         protoState = PROTO_STATE_DET_OK;
00570                         eventHandler(CSR_LOC_EVENT_START_RESULT, 0);
00571                     }
00572 
00573                     _CsrLocProcessRawOspPkt();
00574                 }
00575             } else {
00576                 if (OSP_MSG_HEAD0 == data) {
00577                     protoDetState = STATE_START2;
00578                 } else {
00579                     protoDetState = STATE_START1;
00580                 }
00581             }
00582             break;
00583     }   /* switch. */
00584 }
00585 
00586 void CSRLocation::_CsrLocProcessRawNmeaPkt(void)
00587 {
00588     /* report NMEA */
00589     outputHandler(LOC_OUTPUT_NMEA, serialPkt, strlen((char *)serialPkt));
00590 
00591 #if 0    
00592     tLocPosResp pos;
00593     const char *pNmeaGga = "GPGGA";
00594     float       deg, min;
00595     char        ns, ew;
00596     int         svUsed;
00597     float       horDop;
00598     int         valid;
00599     int         i, cnt;
00600 
00601 
00602     if (strncmp((char *)serialPkt, pNmeaGga, strlen(pNmeaGga)) == 0) {
00603         cnt = 0;
00604         for (i = 0; i < (int)strlen((char *)serialPkt); i++) {
00605             if (serialPkt[i] == ',') {
00606                 cnt++;
00607                 if (cnt == 6) {
00608                     break;
00609                 }
00610             }
00611         }
00612         if (cnt != 6) {
00613             return;
00614         }
00615         i++;
00616         sscanf((char *)(&serialPkt[i]), "%d,", &valid);
00617         if (valid == 0) {
00618             return;
00619         }
00620         
00621         /* Parse GPGGA and output position information */
00622         memset(&pos, 0, sizeof(tLocPosResp));
00623         if (sscanf((char *)serialPkt, "GPGGA,%f,%lf,%c,%lf,%c,%d,%d,%f,%lf", &pos.utcTime, &pos.lat, &ns, &pos.lon, &ew, &valid, &svUsed, &horDop,
00624                    &pos.alt) >= 1) {
00625             if (ns == 'S') {
00626                 pos.lat *= -1.0;
00627             }
00628             if (ew == 'W') {
00629                 pos.lon *= -1.0;
00630             }
00631             deg     = (float)(static_cast<int>(pos.lat * 0.01f));
00632             min     = pos.lat - (deg * 100.0f);
00633             pos.lat = deg + min / 60.0f;
00634             deg     = (float)(static_cast<int>(pos.lon * 0.01f));
00635             min     = pos.lon - (deg * 100.0f);
00636             pos.lon = deg + min / 60.0f;
00637 
00638             outputHandler(LOC_OUTPUT_LOCATION, &pos, sizeof(tLocPosResp));
00639         }
00640     }
00641 #endif
00642 }
00643 
00644 void CSRLocation::_CsrLocProcessRawOspPkt(void)
00645 {
00646     tOspMsg  *pOspMsg;
00647     uint32_t  msgSize;
00648     CsrResult result;
00649 
00650     static uint8_t buffer[512];
00651     static unsigned maxMessageSize = 0;
00652 
00653     msgSize = _CsrLocCalcMsgSize();
00654     if (msgSize > 0) {
00655         msgSize += sizeof(tOspMsg);
00656         if (msgSize > maxMessageSize) {
00657             maxMessageSize = msgSize;
00658             CSR_LOG_INFO("max message size %u\r\n", maxMessageSize);
00659         }
00660         pOspMsg  = (tOspMsg *)buffer;
00661         memset(pOspMsg, 0, msgSize);
00662     } else {
00663         /* discard the unprocessed message */
00664         return;
00665     }
00666 
00667     result = _CsrLocDecodeOspPkt(serialPkt, decodeIndex, &pOspMsg->msgId, pOspMsg->payload, &pOspMsg->length);
00668     if (CSR_RESULT_SUCCESS == result) {
00669         _CsrLocProcessOspPkt(pOspMsg);
00670     }
00671 }
00672 
00673 uint32_t CSRLocation::_CsrLocCalcMsgSize(void)
00674 {
00675     uint8_t *ptr     = serialPkt;
00676     uint32_t msgSize = 0;
00677     uint32_t msgId;
00678     uint8_t  mid, sid = 0;
00679 
00680     mid   = BINARY_IMPORT_UINT8(ptr);
00681     msgId = OSP_MAKE_MSG_ID(mid, sid);
00682 
00683     if ((OSP_MSG_PWR_MODE_RSP == msgId) || (OSP_MSG_MULTI_CONSTELLATION == msgId)) {
00684         /* add the sub-id to the message id */
00685         sid   = BINARY_IMPORT_UINT8(ptr);
00686         msgId = OSP_MAKE_MSG_ID(mid, sid);
00687     }
00688 
00689     switch (msgId) {
00690         case OSP_MSG_OK_TO_SEND:
00691         case OSP_MSG_PWR_MODE_FPM_RSP:
00692         case OSP_MSG_PWR_MODE_LPM_RSP:
00693         case OSP_MSG_HW_CONFIG_REQ:
00694             msgSize = sizeof(uint8_t);
00695             break;
00696         case OSP_MSG_SW_VERSION:
00697             msgSize = MAX_VERSION_LENGTH;
00698             break;
00699         case OSP_MSG_GEODETIC_NAVIGATION:
00700             msgSize = sizeof(tLocPosResp);
00701             break;
00702         case OSP_MSG_GNSS_SAT_DATA:
00703             msgSize = sizeof(uint8_t);
00704             break;
00705         case OSP_MSG_GNSS_NAV_DATA:
00706             msgSize = sizeof(uint8_t);
00707             break;
00708 
00709         default:
00710             msgSize = 0;
00711             break;
00712     }
00713 
00714     return msgSize;
00715 }
00716 
00717 CsrResult CSRLocation::_CsrLocDecodeOspPkt(uint8_t *pPayload, uint32_t payloadLen, uint32_t *pMsgId, void *pMsgData, uint32_t *pMsgLen)
00718 {
00719     CsrResult tRet = CSR_RESULT_SUCCESS;
00720     uint8_t  *ptr  = pPayload;
00721     uint32_t  i;
00722     uint8_t   mid, sid = 0;
00723 
00724     mid      = BINARY_IMPORT_UINT8(ptr);
00725     *pMsgId  = OSP_MAKE_MSG_ID(mid, sid);
00726     *pMsgLen = 0;
00727 
00728     /*   add the sub-id to the message id */
00729     if ((OSP_MSG_PWR_MODE_RSP == *pMsgId) || (OSP_MSG_MULTI_CONSTELLATION == *pMsgId)) {
00730         sid     = BINARY_IMPORT_UINT8(ptr);
00731         *pMsgId = OSP_MAKE_MSG_ID(mid, sid);
00732     }
00733 
00734     switch (*pMsgId) {
00735         case OSP_MSG_SW_VERSION: /* 0x06 */
00736             *pMsgLen = BINARY_IMPORT_UINT8(ptr);
00737             ptr++;
00738             if (*pMsgLen >= MAX_VERSION_LENGTH) {
00739                 tRet = CSR_RESULT_FAILURE;
00740             } else {
00741                 memcpy(pMsgData, ptr, *pMsgLen);
00742             }
00743             break;
00744 
00745         case OSP_MSG_OK_TO_SEND: /* 0x12 */
00746             *((uint8_t *)pMsgData) = BINARY_IMPORT_UINT8(ptr);
00747             *pMsgLen               = sizeof(uint8_t);
00748             break;
00749 
00750         case OSP_MSG_GEODETIC_NAVIGATION: /* 0x29 */
00751         {
00752             tLocPosResp *pPos = (tLocPosResp *) pMsgData;
00753             uint16_t     valid;
00754 
00755             valid = BINARY_IMPORT_UINT16(ptr);
00756             if (valid != 0) {
00757                 tRet = CSR_RESULT_FAILURE;
00758             } else {
00759                 *pMsgLen = sizeof(*pPos);
00760 
00761                 ptr                     += 2;
00762                 pPos->gpsTime.gps_week   = BINARY_IMPORT_UINT16(ptr);
00763                 pPos->gpsTime.tow        = BINARY_IMPORT_UINT32(ptr);
00764                 pPos->utcTime            = CSR_ULOC_UTC_GPS_OFFSET_MS + (uint64_t)pPos->gpsTime.gps_week * (uint64_t)CSR_ULOC_SEC_IN_WEEK_MS + (uint64_t)pPos->gpsTime.tow - (uint64_t)CSR_ULOC_UTC_LEAP_OFFSET_MS;
00765                 ptr                     += 12;
00766                 pPos->lat                = (double)BINARY_IMPORT_SINT32(ptr);
00767                 pPos->lat               *= 1e-7;
00768                 pPos->lon                = (double)BINARY_IMPORT_SINT32(ptr);
00769                 pPos->lon               *= 1e-7;
00770                 ptr                     += 4;
00771                 pPos->alt                = (double)BINARY_IMPORT_SINT32(ptr);
00772                 pPos->alt               *= 1e-2;
00773             }
00774             break;
00775         }
00776 
00777         case OSP_MSG_GNSS_NAV_DATA: /* 0x43, 0x01 */
00778         {
00779             tLocSvStatus *pSvStatus = &svStatus;
00780 
00781             *pMsgLen = sizeof(*pSvStatus);
00782 
00783             ptr                           += 100;
00784             pSvStatus->svUsedInFixMask     = BINARY_IMPORT_UINT32(ptr);
00785             pSvStatus->sbasSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
00786             pSvStatus->gloSvUsedInFixMask  = BINARY_IMPORT_UINT32(ptr);
00787             pSvStatus->qzssSvUsedInFixMask = BINARY_IMPORT_UINT32(ptr);
00788             break;
00789         }
00790 
00791         case OSP_MSG_GNSS_SAT_DATA: /* 0x43, 0x10 */
00792         {
00793             tLocSvStatus *pSvStatus = &svStatus;
00794             uint16_t      week;
00795             uint32_t      tow;
00796             uint32_t      towSubMs;
00797             uint8_t       info;
00798             int32_t       nMsg = 0;
00799             uint16_t      satInfo;
00800             uint16_t      az;
00801             uint16_t      el;
00802             uint16_t      cno;
00803             uint8_t       gnssType;
00804             uint16_t      index = 0;
00805 
00806             *pMsgLen = sizeof(*pSvStatus);
00807 
00808             week     = BINARY_IMPORT_UINT16(ptr);
00809             tow      = BINARY_IMPORT_UINT32(ptr);
00810             towSubMs = BINARY_IMPORT_UINT32(ptr);
00811             ptr     += 4;
00812             info     = BINARY_IMPORT_UINT8(ptr);
00813 
00814             nMsg = info & 0x0F;
00815             if (nMsg == 1) {
00816                 memset(pSvStatus, 0, sizeof(tLocSvStatus));
00817                 pSvStatus->gps_week   = week;
00818                 pSvStatus->tow        = tow;
00819                 pSvStatus->tow_sub_ms = towSubMs;
00820             }
00821 
00822             ptr++;
00823             for (i = 0; i < GNSS_SAT_DATA_NUM_OF_SATS; i++) {
00824                 satInfo = BINARY_IMPORT_UINT16(ptr);
00825                 az      = BINARY_IMPORT_UINT16(ptr);
00826                 el      = BINARY_IMPORT_UINT16(ptr);
00827                 cno     = BINARY_IMPORT_UINT16(ptr);
00828                 ptr    += 4;
00829 
00830                 gnssType = (uint8_t)((satInfo >> 13) & 0x0003);
00831                 if ((0 == gnssType) || (1 == gnssType)) { // GPS, SBAS, QZSS
00832                     index = pSvStatus->numOfSVs;
00833                     if ((index < LOC_MAX_GNSS_SVS) && (cno >0)) {
00834                         pSvStatus->svList[index].prn       = (uint8_t)(satInfo & 0xFF);
00835                         pSvStatus->svList[index].cno       = (float)(cno / 10.0); // Scale: 10
00836                         pSvStatus->svList[index].elevation = (float)(el / 10.0); // Scale: 10
00837                         pSvStatus->svList[index].azimuth   = (float)(az / 10.0); // Scale: 10
00838                         pSvStatus->numOfSVs++;
00839                         pSvStatus->ephemerisMask |= 0x1 << (pSvStatus->svList[index].prn - 1); // prn range: 1-32
00840                     }
00841                 } else if (2 == gnssType) { // GLONASS
00842                     index = pSvStatus->numOfGloSVs;
00843                     if ((index < CODEC_GLO_MAX_CHANNELS) && (cno>0)) {
00844                         int16_t freqChan = (satInfo & 0X1F00) >> 8;
00845                         int16_t slotNum  = (satInfo & 0X00FF);
00846                         if (slotNum > 0) {
00847                             if (freqChan & 0X0010) {
00848                                 freqChan |= 0xFFE0;
00849                             }
00850                             pSvStatus->gloSvList[index].prn       = (uint8_t)(freqChan + LOC_GLO_FREQ_OFFSET);
00851                             pSvStatus->gloSvList[index].sno       = (uint8_t)slotNum;
00852                             pSvStatus->gloSvList[index].cno       = (float)(cno / 10.0); // Scale: 10
00853                             pSvStatus->gloSvList[index].elevation = (float)(el / 10.0); // Scale: 10
00854                             pSvStatus->gloSvList[index].azimuth   = (float)(az / 10.0); // Scale: 10
00855                             pSvStatus->numOfGloSVs++;
00856                             pSvStatus->gloEphemerisMask |= 0x1 << (pSvStatus->gloSvList[index].prn - LOC_GLO_FREQ_ID_START);
00857                         }
00858                     }
00859                 }
00860             }
00861 
00862             break;
00863         }
00864 
00865         case  OSP_MSG_HW_CONFIG_REQ: /* 0x47 */
00866             break;
00867 
00868         case OSP_MSG_PWR_MODE_FPM_RSP: /* 0x5A, 0x00 */
00869             break;
00870 
00871         case OSP_MSG_PWR_MODE_LPM_RSP: /* 0x5A, 0x06 */
00872             *((uint8_t *)pMsgData) = *ptr;
00873             *pMsgLen               = sizeof(uint8_t);
00874             break;
00875 
00876         default:
00877             tRet = CSR_RESULT_FAILURE;
00878             break;
00879     }
00880 
00881     /* check if length does not match */
00882     if (tRet == CSR_RESULT_FAILURE) {
00883         *pMsgId = *pMsgLen = 0;
00884     }
00885 
00886     return tRet;
00887 } /* CsrUlocCodecSsbDecode() */
00888 
00889 void CSRLocation::_CsrLocProcessOspPkt(tOspMsg *pOspMsg)
00890 {
00891     switch (pOspMsg->msgId) {
00892         case OSP_MSG_GEODETIC_NAVIGATION:
00893             outputHandler(LOC_OUTPUT_LOCATION, pOspMsg->payload, sizeof(tLocPosResp));
00894             break;
00895         case OSP_MSG_GNSS_SAT_DATA:
00896             break;
00897         case OSP_MSG_GNSS_NAV_DATA:
00898             outputHandler(LOC_OUTPUT_SV_STATUS, &svStatus, sizeof(tLocSvStatus));
00899             break;
00900         case OSP_MSG_OK_TO_SEND:
00901             engStatus = (*(pOspMsg->payload)) ? ENGINE_STATUS_OK2SEND : ENGINE_STATUS_NOTOK2SEND;
00902             CSR_LOG_INFO("Ok to send %u\r\n", engStatus);
00903             break;
00904         case OSP_MSG_SW_VERSION:
00905             bVerRsp = true;
00906             CSR_LOG_INFO("Ver: %s\r\n", pOspMsg->payload);
00907             break;
00908         case OSP_MSG_HW_CONFIG_REQ:
00909             CSR_LOG_INFO("hw config req.\r\n");
00910             if (!bVerRsp) {
00911                 _CsrLocSendData(SEND_DATA_TYPE_OSP_VER_REQ);
00912             }
00913 
00914             if (!bPwrModeRsp) {
00915                 if (pwrMode == PWR_PTF) {
00916                     CSR_LOG_INFO("Send PTF command.\r\n");
00917                     _CsrLocSendData(SEND_DATA_TYPE_OSP_LPM_REQ);
00918                 } else {
00919                     CSR_LOG_INFO("Send FPM command.\r\n");
00920                     _CsrLocSendData(SEND_DATA_TYPE_OSP_FPM_REQ);
00921                 }
00922             }
00923             break;
00924         case OSP_MSG_PWR_MODE_LPM_RSP:
00925             bPwrModeRsp = true;
00926             CSR_LOG_INFO("lpm response.\r\n");
00927             break;
00928         case OSP_MSG_PWR_MODE_FPM_RSP:
00929             bPwrModeRsp = true;
00930             CSR_LOG_INFO("fpm response.\r\n");
00931             break;
00932         default:
00933             CSR_LOG_INFO("Unknown OSP message 0x%lx.\r\n", pOspMsg->msgId);
00934             break;
00935     }
00936 }
00937 
00938 void CSRLocation::_CsrLocTimeout(void)
00939 {
00940     bTimeoutFlag = true;
00941 }
00942 
00943 void CSRLocation::_CsrLocRxHandler(void)
00944 {
00945     serialBuf[in++] = serialLoc.getc();
00946     in             &= (MAX_SERIAL_BUF_LEN - 1);
00947     if (in == out) {
00948         CSR_LOG_INFO("rx overwritten %lu %lu.\r\n", in, out);
00949     }
00950 }
00951 
00952 void CSRLocation::_CsrLocSendData(eSendDataType type)
00953 {
00954     uint32_t       i, size;
00955     const uint8_t *pData;
00956 
00957     switch (type) {
00958         case SEND_DATA_TYPE_OSP_STOP_REQ:
00959             pData = sOspStopReq;
00960             size  = sizeof(sOspStopReq);
00961             break;
00962         case SEND_DATA_TYPE_OSP_VER_REQ:
00963             pData = sOspVerReq;
00964             size  = sizeof(sOspVerReq);
00965             break;
00966         case SEND_DATA_TYPE_OSP_LPM_REQ:
00967             pData = sOspLpmReq;
00968             size  = sizeof(sOspLpmReq);
00969             break;
00970         case SEND_DATA_TYPE_OSP_FPM_REQ:
00971             pData = sOspFpmReq;
00972             size  = sizeof(sOspFpmReq);
00973             break;
00974         case SEND_DATA_TYPE_OSP_SWITCH2NMEA_REQ:
00975             pData = sOspSwitch2NmeaReq;
00976             size  = sizeof(sOspSwitch2NmeaReq);
00977             break;
00978         case SEND_DATA_TYPE_NMEA_SWITCH2OSP_REQ:
00979             pData = (const uint8_t *)sNmeaSwitch2OspReq;
00980             size  = strlen(sNmeaSwitch2OspReq);
00981             break;
00982         case SEND_DATA_TYPE_NMEA_STOP_REQ:
00983             pData = (const uint8_t *)sNmeaStopReq;
00984             size  = strlen(sNmeaStopReq);
00985             break;
00986 
00987         default:
00988             pData = NULL;
00989     }
00990 
00991     if (pData != NULL) {
00992         for (i = 0; i < size; i++) {
00993             serialLoc.putc(pData[i]);
00994         }
00995     }
00996 }
00997 
00998 void CSRLocation::_CsrLocHwOnoff(void)
00999 {
01000     pinOnoff.write(1);
01001     wait_ms(100);
01002     pinOnoff.write(0);
01003     CSR_LOG_INFO("Onoff pulse given.\r\n");
01004 }
01005 
01006 void CSRLocation::_CsrLocHwReset(void)
01007 {
01008     pinReset.write(0);
01009     wait_ms(100);
01010     pinReset.write(1);
01011     CSR_LOG_INFO("Reset pulse given.\r\n");
01012 }
01013 
01014 bool CSRLocation::_CsrLocIsWakeup(void)
01015 {
01016     CSR_LOG_INFO("Is wakeup %d.\r\n", wakeup.read());
01017     return wakeup.read() != 0 ? true : false;
01018 }
01019 
01020 bool CSRLocation::setPowerMode(GPSProvider::PowerMode_t _pwrMode)
01021 {
01022     switch (_pwrMode) {
01023         case GPSProvider::POWER_FULL:
01024             pwrMode = PWR_FULL;
01025             break;
01026         case GPSProvider::POWER_LOW:
01027         default:
01028             pwrMode = PWR_PTF; /* push to fix */
01029             break;
01030     }
01031 
01032     return (true);
01033 }