Teseo.cpp

00001 /**
00002 *******************************************************************************
00003 * @file    Teseo.h
00004 * @author  AST / Central Lab
00005 * @version V1.0.0
00006 * @date    May-2017
00007 * @brief   Teseo Location Class
00008 *
00009 *******************************************************************************
00010 * @attention
00011 *
00012 * <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
00013 *
00014 * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
00015 * You may not use this file except in compliance with the License.
00016 * You may obtain a copy of the License at:
00017 *
00018 *        http://www.st.com/software_license_agreement_liberty_v2
00019 *
00020 * Redistribution and use in source and binary forms, with or without modification,
00021 * are permitted provided that the following conditions are met:
00022 *   1. Redistributions of source code must retain the above copyright notice,
00023 *      this list of conditions and the following disclaimer.
00024 *   2. Redistributions in binary form must reproduce the above copyright notice,
00025 *      this list of conditions and the following disclaimer in the documentation
00026 *      and/or other materials provided with the distribution.
00027 *   3. Neither the name of STMicroelectronics nor the names of its contributors
00028 *      may be used to endorse or promote products derived from this software
00029 *      without specific prior written permission.
00030 *
00031 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00032 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00033 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00034 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00035 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00036 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00037 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00038 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00039 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00040 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00041 *
00042 ********************************************************************************
00043 */
00044 
00045 #include "Teseo.h"
00046 
00047 #if MBED_CONF_TESEO_TRACE_ENABLED == 1
00048 #define TESEO_LOG_INFO(...) printf(__VA_ARGS__)
00049 #endif
00050 
00051 static char TESEO_NAME[] = "Teseo-LIV3F";
00052 /*
00053 static char _OK[] = "OK";
00054 static char _Failed[] = "Failed";
00055 */
00056 
00057 struct teseoCmd {
00058     char *cmd;
00059 };
00060 
00061 static struct teseoCmd teseoCmds[] = {
00062   [Teseo::TEST] = {
00063     .cmd = (char*)"$\n\r",
00064   },
00065   [Teseo::GETSWVER] = {
00066     .cmd = (char*)"$PSTMGETSWVER,6\n\r",
00067   },
00068   [Teseo::FORCESTANDBY] = {
00069     .cmd = (char*)"$PSTMFORCESTANDBY,00010\n\r",
00070   },
00071   [Teseo::RFTESTON] = {
00072     .cmd  = (char*)"$PSTMRFTESTON,16\n\r",
00073   },
00074   [Teseo::RFTESTOFF] = {
00075     .cmd  = (char*)"$PSTMRFTESTOFF\n\r",
00076   },
00077   [Teseo::LOWPOWER] = {
00078     .cmd  = (char*)"$PSTMLOWPOWERONOFF,1,0,000,05,0,1,000,1,00010,01,0,0,1,01\n\r",
00079   },
00080   [Teseo::FWUPDATE] = {
00081     .cmd  = (char*)"$PSTMFWUPGRADE\n\r",
00082   }
00083 };
00084 
00085 Teseo::Teseo(PinName resetPin,
00086              PinName wakeupPin,
00087              PinName ppsPin,
00088              PinName uartTxPin,
00089              PinName uartRxPin):
00090   _loc_led2(LOC_LED2),
00091   _reset(resetPin, 1),
00092   _pps(ppsPin),
00093   _wakeup(wakeupPin, 0),
00094   _uartRx(uartRxPin),
00095   _uartTx(uartTxPin)
00096 {
00097   wait_us(POWERON_STABLE_SIGNAL_DELAY_MS*1000);
00098   _uart = NULL;
00099   _i2c = NULL;
00100   
00101   _locState = TESEO_LOC_STATE_IDLE;
00102   
00103   deviceInfo = TESEO_NAME;
00104 }
00105 
00106 Teseo::Teseo(PinName resetPin,
00107              PinName wakeupPin,
00108              PinName ppsPin,
00109              PinName uartTxPin,
00110              PinName uartRxPin,
00111              I2C    *i2cBus):
00112     _loc_led2(LOC_LED2),
00113     _reset(resetPin, 1),
00114     _pps(ppsPin),
00115     _wakeup(wakeupPin, 0),
00116     _uartRx(uartRxPin),
00117     _uartTx(uartTxPin),
00118     _i2c(i2cBus)
00119 {
00120   wait_us(POWERON_STABLE_SIGNAL_DELAY_MS*1000);
00121   _uart = NULL;
00122   
00123   _locState = TESEO_LOC_STATE_IDLE;
00124 
00125   deviceInfo = TESEO_NAME;
00126 }
00127 
00128 void
00129 Teseo::TeseoLocRegOutput(teseo_app_output_callback app_output_cb, teseo_app_event_callback app_event_cb)
00130 {
00131   appOutCb   = app_output_cb;
00132   appEventCb = app_event_cb;
00133 }
00134 
00135 int
00136 Teseo::EnableLowPower()
00137 {
00138   SendCommand(LOWPOWER);
00139   return 0;
00140 }
00141 
00142 void
00143 Teseo::_ResetFast(void)
00144 {
00145   TESEO_LOG_INFO("%s: Resetting...", TESEO_NAME);
00146 
00147   _reset.write(0);
00148 
00149   wait_us(5000);
00150   
00151   _reset.write(1);
00152 
00153   wait_us(70000);
00154 
00155   TESEO_LOG_INFO("Done...\n\r");
00156 }
00157 
00158 void
00159 Teseo::_Reset(void)
00160 {
00161   TESEO_LOG_INFO("%s: Resetting...", TESEO_NAME);
00162 
00163   //_pps.output();
00164   //_pps.write(0);
00165   
00166   _reset.write(1);
00167   
00168   wait_us(500000); //500ms
00169   
00170   _reset.write(0);
00171   //_pps.write(0);
00172   
00173   wait_us(500000); //500ms
00174   
00175   _reset.write(1);
00176 
00177   TESEO_LOG_INFO("Done...\n\r");
00178 
00179   //_pps.write(1);
00180   
00181   //wait_ms(1000);
00182   
00183   //_pps.input();
00184 }
00185 
00186 void
00187 Teseo::_SendString(char *buf, int len)
00188 {
00189   for (int i = 0; i < len; ++i) {
00190     while (!_uart->writable());
00191     _uart->write((const void *)&buf[i], 1);
00192   }
00193 }
00194 
00195 int
00196 Teseo::_WakeUp()
00197 {
00198   wait_us(100000); //100ms
00199   
00200   _wakeup.write(1);
00201   
00202   wait_us(500000); //500ms
00203   
00204   _wakeup.write(0);
00205   
00206   return 0;
00207 }
00208 
00209 int
00210 Teseo::_CRC(char *buf, int size)
00211 {
00212   int i = 0, ch = 0;
00213   
00214   if (buf[0] == '$') {
00215     ++i;
00216   }
00217   
00218   if (size) {
00219     for (; i < size; ++i) {
00220       ch ^= buf[i];
00221     }
00222   } else {
00223     for (; buf[i] != 0; ++i) {
00224       ch ^= buf[i];
00225     }
00226   }
00227   
00228   return ch;
00229 }
00230 
00231 void
00232 Teseo::SendCommand(Teseo::eCmd c)
00233 {
00234   char crc[3];
00235   
00236   snprintf(crc, 3, "*%02X", _CRC(teseoCmds[c].cmd, -1));
00237 
00238   _SendString(teseoCmds[c].cmd, strlen(teseoCmds[c].cmd));
00239   _SendString(crc, 3);
00240 }
00241 
00242 void
00243 Teseo::SendCommand(char *cmd)
00244 {
00245   char crc[5];
00246   
00247   snprintf(crc, 5, "*%02X\n\r", _CRC(cmd, strlen(cmd)));
00248   //printf("CRC=%s\n\r", crc);
00249   
00250   _SendString(cmd, strlen(cmd));
00251   _SendString(crc, 5);
00252 }
00253 
00254 void
00255 Teseo::ReadSentence(Teseo::eMsg msg)
00256 {
00257   struct _teseoMsg *message;
00258   eStatus status = TESEO_STATUS_FAILURE;
00259 
00260   do {
00261     bool stat = queue.try_get_for(Kernel::wait_for_u32_forever, &message);
00262     if (stat == true) {
00263       if (message->len > 0) {
00264         status = _GetMsg(msg, message->buf);
00265       }
00266       
00267       mpool.free(message);
00268     }
00269   } while (status != TESEO_STATUS_SUCCESS);
00270 }
00271 
00272 void
00273 Teseo::_InitUART(int br)
00274 {  
00275   _uart = new (std::nothrow) BufferedSerial(_uartRx, _uartTx);
00276   if(_uart == NULL) {
00277     TESEO_LOG_INFO("Error allocating UART.\r\n");
00278   }
00279 }
00280 
00281 bool
00282 Teseo::setPowerMode(GPSProvider::PowerMode_t pwrMode)
00283 {
00284   //TBI
00285   return false;
00286 }
00287 
00288 void
00289 Teseo::eventHandler(eTeseoLocEventType event, uint32_t data)
00290 {
00291   if (appEventCb) {
00292     appEventCb(event, data);
00293   }
00294 }
00295 
00296 void
00297 Teseo::ReadProcess(void)
00298 {
00299   char c;
00300   
00301   struct _teseoMsg *msg = mpool.try_alloc();
00302   msg->len = 0;
00303   
00304   while(true) {
00305     if (_uart->readable()) {
00306       _uart->read(&c, 1);
00307       
00308       if (c == '$') {
00309         queue.try_put(msg);
00310         msg = mpool.try_alloc();
00311         msg->len = 0;
00312       }
00313       msg->buf[msg->len++] = c;
00314 
00315     } else {
00316       ThisThread::yield(); //wait_us(100); Allow other threads to run
00317     }
00318   }
00319 }
00320 
00321 static void
00322 _UARTStreamProcess(Teseo *gnss)
00323 {
00324   gnss->ReadProcess();
00325 }
00326 
00327 void
00328 Teseo::start(void)
00329 {
00330   if(_locState == TESEO_LOC_STATE_IDLE) {
00331 
00332     _InitUART();
00333     _locState = TESEO_LOC_STATE_RUN;
00334     
00335     // Start thread for UART listener and set the highest priority
00336     serialStreamThread = new (std::nothrow) Thread();
00337     if(serialStreamThread != NULL) {
00338       serialStreamThread->start(callback(_UARTStreamProcess, this));
00339     } else {
00340       TESEO_LOG_INFO("Error allocating serialStreamThread\r\n");
00341     }
00342 
00343     eventHandler(TESEO_LOC_EVENT_START_RESULT, 0);
00344   } else {
00345     TESEO_LOG_INFO("Already started\r\n");
00346   }
00347 }
00348 
00349 void
00350 Teseo::stop(void)
00351 {
00352   if(_locState == TESEO_LOC_STATE_IDLE) {
00353     return;
00354   }
00355   // Stop thread for UART listener
00356   if(serialStreamThread != NULL) {
00357     serialStreamThread->terminate();
00358     delete serialStreamThread;
00359   }
00360   
00361   if(_uart != NULL) {
00362     delete _uart;
00363     _uart = NULL;
00364   }
00365 
00366   _locState = TESEO_LOC_STATE_IDLE;
00367   eventHandler(TESEO_LOC_EVENT_STOP_RESULT, 0);
00368 }
00369 
00370 void
00371 Teseo::outputHandler(uint32_t msgId, uint32_t msgType, tTeseoData *pData)
00372 {
00373   switch(msgId) {
00374   case LOC_OUTPUT_LOCATION:
00375     
00376     if(pData->gpgga_data.valid == VALID) {
00377       lastLocation.valid = true;
00378       lastLocation.lat = pData->gpgga_data.xyz.lat;
00379       lastLocation.lon = pData->gpgga_data.xyz.lon;
00380       lastLocation.altitude = pData->gpgga_data.xyz.alt;
00381       lastLocation.numGPSSVs = pData->gpgga_data.sats;
00382       lastLocation.utcTime = pData->gpgga_data.utc.utc;
00383     } else {
00384       lastLocation.valid = false;
00385     }
00386     
00387     if (locationCallback) {
00388       locationCallback(&lastLocation);
00389     }
00390 
00391     break;
00392    
00393   case LOC_OUTPUT_PSTM: {
00394     Teseo::ePSTMsg msg = (Teseo::ePSTMsg)msgType;
00395     int code;
00396 
00397     switch(msg) {
00398     case PSTMGEOFENCE: {
00399 
00400       /* Geofence enabling */
00401       if(pData->geofence_data.op == GNSS_FEATURE_EN_MSG) {
00402         code = pData->geofence_data.result ?
00403           GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE;
00404           
00405         if (code == GPS_ERROR_NONE) {
00406           saveConfigParams();
00407         }
00408       }
00409 
00410       /* Geofence configuration */
00411       if(pData->geofence_data.op == GNSS_GEOFENCE_CFG_MSG) {
00412         code = pData->geofence_data.result ?
00413           GPS_ERROR_GEOFENCE_CFG : GPS_ERROR_NONE;
00414         if (geofenceCfgMessageCallback) {
00415           geofenceCfgMessageCallback(code);
00416         }
00417       }
00418 
00419       /* Geofence Status */
00420       if(pData->geofence_data.op == GNSS_GEOFENCE_STATUS_MSG) {
00421         code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE;
00422         if(code == GPS_ERROR_NONE) {
00423           geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh;
00424           geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm;
00425           geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss;
00426           geofenceStatus.timestamp.day = pData->geofence_data.timestamp.day;
00427           geofenceStatus.timestamp.month = pData->geofence_data.timestamp.month;
00428           geofenceStatus.timestamp.year = pData->geofence_data.timestamp.year;
00429           geofenceStatus.currentStatus = pData->geofence_data.status;
00430           geofenceStatus.numGeofences = MAX_GEOFENCES_NUM;
00431         }
00432         if (geofenceStatusMessageCallback) {
00433           geofenceStatusMessageCallback(&geofenceStatus, code);
00434         }
00435       }
00436 
00437       /* Geofence Alarm */
00438       if(pData->geofence_data.op == GNSS_GEOFENCE_ALARM_MSG) {
00439         code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE;
00440         if(code == GPS_ERROR_NONE) {
00441           geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh;
00442           geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm;
00443           geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss;
00444           geofenceStatus.currentStatus = pData->geofence_data.status;
00445           geofenceStatus.idAlarm = pData->geofence_data.idAlarm;
00446         }
00447         if (geofenceStatusMessageCallback) {
00448           geofenceStatusMessageCallback(&geofenceStatus, code);
00449         }
00450       }
00451     }
00452     break;
00453 
00454     case PSTMODO: {
00455       /* Odometer enabling */
00456       if(pData->odo_data.op == GNSS_FEATURE_EN_MSG) {
00457         
00458         code = pData->odo_data.result ?
00459           GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE;
00460 
00461         if (code == GPS_ERROR_NONE) {
00462           saveConfigParams();
00463         }
00464       }
00465 
00466       /* Odometer start */
00467       if(pData->odo_data.op == GNSS_ODO_START_MSG) {
00468         code = pData->odo_data.result ?
00469           GPS_ERROR_ODO_START : GPS_ERROR_NONE;
00470       }
00471       /* Odometer stop */
00472       if(pData->odo_data.op == GNSS_ODO_STOP_MSG) {
00473         code = pData->odo_data.result ?
00474           GPS_ERROR_ODO_STOP : GPS_ERROR_NONE;
00475       }
00476     }
00477     break;
00478     
00479     case PSTMDATALOG: {
00480       /* Datalog enabling */
00481       if(pData->datalog_data.op == GNSS_FEATURE_EN_MSG) {
00482         
00483         code = pData->datalog_data.result ?
00484           GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE;
00485 
00486         if (code == GPS_ERROR_NONE) {
00487           saveConfigParams();
00488         }
00489       }
00490       /* Datalog create */
00491       if(pData->datalog_data.op == GNSS_DATALOG_CFG_MSG) {
00492         code = pData->datalog_data.result ?
00493           GPS_ERROR_DATALOG_CFG : GPS_ERROR_NONE;
00494       }
00495       /* Datalog start */
00496       if(pData->datalog_data.op == GNSS_DATALOG_START_MSG) {
00497         code = pData->datalog_data.result ?
00498           GPS_ERROR_DATALOG_START : GPS_ERROR_NONE;
00499       }
00500       /* Datalog stop */
00501       if(pData->datalog_data.op == GNSS_DATALOG_STOP_MSG) {
00502         code = pData->datalog_data.result ?
00503           GPS_ERROR_DATALOG_STOP : GPS_ERROR_NONE;
00504       }
00505       /* Datalog erase */
00506       if(pData->datalog_data.op == GNSS_DATALOG_ERASE_MSG) {
00507         code = pData->datalog_data.result ?
00508           GPS_ERROR_DATALOG_ERASE : GPS_ERROR_NONE;
00509       }
00510     }
00511     break;
00512 
00513     case PSTMSGL: {
00514       /* Msg List cfg */
00515       code = pData->ack ?
00516           GPS_ERROR_MSGLIST_CFG : GPS_ERROR_NONE;
00517 
00518       if (code == GPS_ERROR_NONE) {
00519         saveConfigParams();
00520       }
00521     }
00522     break;
00523     
00524     case PSTMSAVEPAR: {
00525       code = pData->ack ?
00526           GPS_ERROR_SAVEPAR : GPS_ERROR_NONE;
00527 
00528       if (code == GPS_ERROR_NONE) {
00529         reset();
00530       }
00531     }
00532     break;
00533     
00534   } /* end switch */
00535   } /* end case LOC_OUTPUT_PSTM */
00536 
00537   break;
00538 
00539   default:
00540     break;
00541   }
00542   
00543   if (appOutCb) {
00544     appOutCb(msgId, msgType, pData);
00545   }
00546   
00547 }
00548 
00549 
00550 eStatus
00551 Teseo::_GetMsg(Teseo::eMsg msg, uint8_t *buffer)
00552 {  
00553   eStatus status = TESEO_STATUS_FAILURE;
00554 
00555   switch(msg) {
00556 
00557   case GPGGA:
00558     status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer);
00559     if(status == TESEO_STATUS_SUCCESS) {
00560       outputHandler(LOC_OUTPUT_LOCATION, msg, &pData);
00561     }
00562     break;
00563     
00564   case GNS:
00565     status = (eStatus)parse_gnsmsg(&pData.gns_data, buffer);
00566     if(status == TESEO_STATUS_SUCCESS) {
00567       outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
00568     }
00569     break;
00570     
00571   case GPGST:
00572     status = (eStatus)parse_gpgst(&pData.gpgst_data, buffer);
00573     if(status == TESEO_STATUS_SUCCESS) {
00574       outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
00575     }
00576     break;
00577     
00578   case GPRMC:
00579     status = (eStatus)parse_gprmc(&pData.gprmc_data, buffer);
00580     if(status == TESEO_STATUS_SUCCESS) {
00581       outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
00582     }
00583     break;
00584     
00585   case GSA:
00586     status = (eStatus)parse_gsamsg(&pData.gsa_data, buffer);
00587     if(status == TESEO_STATUS_SUCCESS) {
00588       outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
00589     }
00590     break;
00591     
00592   case GSV:
00593     status = (eStatus)parse_gsvmsg(&pData.gsv_data, buffer);
00594     if(status == TESEO_STATUS_SUCCESS) {
00595       outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
00596     }    
00597     break;
00598     
00599   default:
00600     break;
00601     
00602   }
00603   
00604   return status;
00605 }
00606 
00607 void
00608 Teseo::setVerboseMode(int level)
00609 {
00610   cfgMessageList(level);
00611 }
00612 
00613 void
00614 Teseo::_GetLocationMsg(Teseo::eMsg msg, uint8_t *buffer)
00615 {  
00616   eStatus status;
00617 
00618   status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer);
00619   if(status == TESEO_STATUS_SUCCESS) {
00620     outputHandler(LOC_OUTPUT_LOCATION, msg, &pData);
00621   }
00622 }
00623   
00624 void
00625 Teseo::_GetPSTMsg(Teseo::ePSTMsg msg, uint8_t *buffer)
00626 {
00627   eStatus status;
00628   
00629   switch(msg) {
00630 
00631   case PSTMGEOFENCE:
00632     status = (eStatus)parse_pstmgeofence (&pData.geofence_data, buffer);
00633     if(status == TESEO_STATUS_SUCCESS) {
00634       outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
00635     }
00636     break;
00637   case PSTMODO:
00638     status = (eStatus)parse_pstmodo (&pData.odo_data, buffer);
00639     if(status == TESEO_STATUS_SUCCESS) {
00640       outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
00641     }
00642     break;
00643   case PSTMDATALOG:
00644     status = (eStatus)parse_pstmdatalog (&pData.datalog_data, buffer);
00645     if(status == TESEO_STATUS_SUCCESS) {
00646       outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
00647     }
00648     break;
00649   case PSTMSGL:
00650     status = (eStatus)parse_pstmsgl (&pData.ack, buffer);
00651     if(status == TESEO_STATUS_SUCCESS) {
00652       outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
00653     }
00654     break;
00655   case PSTMSAVEPAR:
00656     status = (eStatus)parse_pstmsavepar (&pData.ack, buffer);
00657     if(status == TESEO_STATUS_SUCCESS) {
00658       outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
00659     }
00660     break;
00661 
00662   default:
00663     break;
00664   }
00665   /* Recover the normal state */
00666 
00667 }
00668 
00669 void
00670 Teseo::process(void)
00671 {
00672   struct _teseoMsg *message;
00673 
00674   bool stat = queue.try_get_for(Kernel::wait_for_u32_forever, &message);
00675 
00676   if (stat == true) {
00677     if (message->len > 0) {
00678 
00679       for(int m = 0; m < PSTM_NMEA_MSGS_NUM; m++) {
00680         _GetPSTMsg((ePSTMsg)m, message->buf);
00681       }
00682       for(int m = 0; m < NMEA_MSGS_NUM; m++) {
00683         _GetMsg((eMsg)m, message->buf);
00684       }
00685     }
00686 
00687     mpool.free(message);
00688   }
00689 }
00690 
00691 const GPSProvider::LocationUpdateParams_t *
00692 Teseo::getLastLocation(void) const
00693 {
00694   return &lastLocation;
00695 }
00696 
00697 void
00698 Teseo::reset(void)
00699 {
00700   _ResetFast();
00701 }
00702 
00703 gps_provider_error_t
00704 Teseo::cfgMessageList(int level)
00705 {
00706   int lowMask = 0x2;
00707   int highMask = 0x80000;
00708 
00709   if(level == 2) {
00710     lowMask = 0x18004F;
00711   }
00712 
00713   sprintf(_teseoCmd, "$PSTMCFGMSGL,%d,%d,%x,%x",
00714           0, /*NMEA 0*/
00715           1, /*Rate*/
00716           lowMask,
00717           highMask);
00718 
00719   SendCommand(_teseoCmd);
00720   
00721   return GPS_ERROR_NONE;  
00722 }
00723 
00724 gps_provider_error_t
00725 Teseo::saveConfigParams(void)
00726 {  
00727   sprintf(_teseoCmd, "$PSTMSAVEPAR");
00728   SendCommand(_teseoCmd);
00729   
00730   return GPS_ERROR_NONE;  
00731 }
00732 
00733 bool
00734 Teseo::isGeofencingSupported(void)
00735 {
00736   return true;
00737 }
00738 
00739 gps_provider_error_t
00740 Teseo::enableGeofence(void)
00741 {
00742   //$PSTMCFGGEOFENCE,<en>,<tol>*<checksum><cr><lf>
00743   sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d",1,1);
00744   SendCommand(_teseoCmd);
00745 
00746   return GPS_ERROR_NONE;
00747 }
00748 
00749 gps_provider_error_t
00750 Teseo::cfgGeofenceCircle(void)
00751 {
00752   GPSGeofence::GeofenceCircle_t circle = {
00753     .id = 0,
00754     .enabled = 1,
00755     .tolerance = 1,
00756     .lat = 40.336055,
00757     .lon = 18.120611,
00758     .radius = 200
00759   };
00760   
00761   sprintf(_teseoCmd, "$PSTMCFGGEOCIR,%d,%d,%lf,%lf,%lf",
00762           circle.id,
00763           circle.enabled,
00764           circle.lat,
00765           circle.lon,
00766           circle.radius);
00767   SendCommand(_teseoCmd);
00768 
00769   sprintf(_teseoCmd, "$PSTMSAVEPAR");
00770   SendCommand(_teseoCmd);
00771   
00772   return GPS_ERROR_NONE;
00773 }
00774 
00775 
00776 gps_provider_error_t
00777 Teseo::configGeofences(GPSGeofence *geofences[], unsigned geofenceCount)
00778 {
00779   uint8_t trials;
00780      
00781   if(geofenceCount > MAX_GEOFENCES_NUM) {
00782     return GPS_ERROR_GEOFENCE_MAX_EXCEEDED;
00783   }
00784   
00785   for(uint8_t i = 0; i < geofenceCount; i++) {
00786     trials = 1;
00787     //printf("Teseo::configGeofences id=%d\r\n", (geofences[i]->getGeofenceCircle()).id);
00788     /*
00789     printf("Teseo::configGeofences en=%d\r\n", en);
00790     printf("Teseo::configGeofences tol=%d\r\n", (geofences[i]->getGeofenceCircle()).tolerance);
00791     printf("Teseo::configGeofences lat=%02f\r\n", (geofences[i]->getGeofenceCircle()).lat);
00792     printf("Teseo::configGeofences lon=%02f\r\n", (geofences[i]->getGeofenceCircle()).lon);
00793     printf("Teseo::configGeofences radius=%02f\r\n", (geofences[i]->getGeofenceCircle()).radius);
00794     */
00795     sprintf(_teseoCmd, "$PSTMGEOFENCECFG,%d,%d,%d,%lf,%lf,%lf",
00796             (geofences[i]->getGeofenceCircle()).id,
00797             (geofences[i]->getGeofenceCircle()).enabled,
00798             (geofences[i]->getGeofenceCircle()).tolerance,
00799             (geofences[i]->getGeofenceCircle()).lat,
00800             (geofences[i]->getGeofenceCircle()).lon,
00801             (geofences[i]->getGeofenceCircle()).radius);
00802     
00803     do{
00804       trials--; 
00805       
00806       SendCommand(_teseoCmd);
00807       /*
00808       sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d\n\r",1,1);
00809       
00810       //sprintf(_teseoCmd, "$PSTMGETSWVER,6");
00811       SendCommand(_teseoCmd);
00812       
00813       sprintf(_teseoCmd, "$PSTMSAVEPAR\n\r");
00814       SendCommand(_teseoCmd);
00815       */
00816       //printf("Teseo::configGeofences _teseoCmd=%s\r\n", _teseoCmd);
00817     } while (trials > 0);
00818   }
00819   //printf("Teseo::configGeofences sizeof(geofences)=%d numGeofences=%d strlen(_teseoCmd)=%d\r\n", sizeof(geofences), numGeofences, strlen(_teseoCmd));
00820 
00821   return GPS_ERROR_NONE;
00822 }
00823 
00824 gps_provider_error_t
00825 Teseo::geofenceReq(void)
00826 {
00827   sprintf(_teseoCmd, "$PSTMGEOFENCEREQ");
00828   SendCommand(_teseoCmd);
00829 
00830   return GPS_ERROR_NONE;
00831 }
00832 
00833 bool
00834 Teseo::isDataloggingSupported(void)
00835 {
00836   return true;
00837 }
00838 
00839 gps_provider_error_t
00840 Teseo::enableDatalog(void)
00841 {
00842   //$PSTMCFGLOG,<en>,<circ>,<rectype>,<oneshot>,<rate>,<speed>,<dist>*<checksum><cr><lf>
00843   sprintf(_teseoCmd, "$PSTMCFGLOG,%d,%d,%d,%d,%u,%u,%u",
00844           1, //Enable/Disable the log
00845           1, //Enable/Disable circular mode
00846           1, //Record type
00847           0, //Enable/Disable one shot mode
00848           5, //time interval in seconds between two consecutive logged records
00849           0, //minimum speed threshold
00850           0  //distance threshold
00851           );
00852   SendCommand(_teseoCmd);
00853   
00854   return GPS_ERROR_NONE;
00855 }
00856 
00857 gps_provider_error_t
00858 Teseo::configDatalog(GPSDatalog *datalog)
00859 {
00860   //printf("Teseo::configDatalog 0x%03x\r\n", (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer()));
00861   //$PSTMLOGCREATE,<cfg>,<min-rate>,<min-speed>,<min-position>,<logmask>*<checksum><cr><lf>
00862   sprintf(_teseoCmd, "$PSTMLOGCREATE,%03x,%u,%u,%u,%d",
00863           (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer()),
00864           datalog->getMinRate(),
00865           datalog->getMinSpeed(),
00866           datalog->getMinPosition(),
00867           datalog->getLogMask()
00868           );
00869   SendCommand(_teseoCmd);
00870 
00871   return GPS_ERROR_NONE;
00872 }
00873 
00874 gps_provider_error_t
00875 Teseo::startDatalog(void)
00876 {
00877   sprintf(_teseoCmd, "$PSTMLOGSTART");
00878   SendCommand(_teseoCmd);
00879 
00880   return GPS_ERROR_NONE;
00881 }
00882 
00883 gps_provider_error_t
00884 Teseo::stopDatalog(void)
00885 {
00886   sprintf(_teseoCmd, "$PSTMLOGSTOP");
00887   SendCommand(_teseoCmd);
00888 
00889   return GPS_ERROR_NONE;
00890 }
00891 
00892 gps_provider_error_t
00893 Teseo::eraseDatalog(void)
00894 {
00895   sprintf(_teseoCmd, "$PSTMLOGERASE");
00896   SendCommand(_teseoCmd);
00897 
00898   return GPS_ERROR_NONE;
00899 }
00900 
00901 gps_provider_error_t
00902 Teseo::logReqStatus(void)
00903 {
00904   /* TBI */
00905   return GPS_ERROR_NONE;
00906 }
00907 
00908 gps_provider_error_t
00909 Teseo::logReqQuery(GPSProvider::LogQueryParams_t &logReqQuery)
00910 {
00911   /* TBI */
00912   return GPS_ERROR_NONE;
00913 }
00914 
00915 bool
00916 Teseo::isOdometerSupported(void)
00917 {
00918   return true;
00919 }
00920 
00921 gps_provider_error_t
00922 Teseo::enableOdo(void)
00923 {
00924   //$PSTMCFGODO,<en>,<enmsg>,<alarm>*<checksum><cr><lf>
00925   sprintf(_teseoCmd, "$PSTMCFGODO,1,1,1");
00926   SendCommand(_teseoCmd);
00927   
00928   return GPS_ERROR_NONE;
00929 }
00930 
00931 gps_provider_error_t
00932 Teseo::startOdo(unsigned alarmDistance)
00933 {
00934   sprintf(_teseoCmd, "$PSTMODOSTART,%08x", alarmDistance);
00935   SendCommand(_teseoCmd);
00936 
00937   return GPS_ERROR_NONE;
00938 }
00939 
00940 gps_provider_error_t
00941 Teseo::stopOdo(void)
00942 {
00943   sprintf(_teseoCmd, "$PSTMODOSTOP");
00944   SendCommand(_teseoCmd);
00945 
00946   return GPS_ERROR_NONE;
00947 }
00948 gps_provider_error_t
00949 Teseo::resetOdo(void)
00950 {
00951   /* TBI */
00952   return GPS_ERROR_NONE;
00953 }