GPSProvider wrapper library for STMicroelectronics' X-NUCLEO-GNSS1A1 Expansion Board.
X_NUCLEO_GNSS1A1 Library
GPS_Provider wrapper library for STMicroelectronics' X-NUCLEO-GNSS1A1 component.
Overview
This library includes drivers for ST’s Teseo-LIV3F Global Navigation Satellite System (GNSS) device and middleware for the NMEA protocol support. This firmware package implements the port of the GPS_Provider to STMicroelectronics' X-NUCLEO-GNSS1A1 GNSS Expansion Board.
The key features of the library are:
- Complete software to build applications using Teseo-LIV3F GNSS device
- Middleware for the NMEA protocol support
Furthermore the library provides the following advanced features:
- Geofencing - allows the Teseo-LIV3F receiver to raise a NMEA message when the resolved GNSS position is close to or entering or exiting from a specific circle
- Odometer - provides information on the traveled distance using only the resolved GNSS position
- Data Logging - allows the Teseo-LIV3F receiver to save locally on the flash the resolved GNSS position to be retrieved on demand from the Host
Hardware description
The X-NUCLEO-GNSS1A1 is a Global Navigation Satellite System Expansion Board usable with the STM32 Nucleo system and other Arduino compatible platforms. It is designed around the STMicroelectronics Teseo-LIV3F GNSS receiver IC working on multiple constellations (GPS/Galileo/Glonass/BeiDou/QZSS).
The Teseo-LIV3F module is designed for top performance in a minimal space. Within its 10x10mm compact size, Teseo-LIV3F offers superior accuracy thanks to the on board 26MHz Temperature Compensated Crystal Oscillator (TCXO) and a reduced Time To First Fix (TTFF) relying to its dedicated 32KHz Real Time Clock (RTC) oscillator.
The X-NUCLEO-GNSS1A1, hosting the Teseo-LIV3F, is compatible with Arduino UNO R3 connector layout and interfaces with the MCU via the UART channel. To connect by serial port the GNSS receiver and the host the following parameters must be used:
- 8 data bits
- No parity
- 1 stop bit
- 9600 bauds
A GPS/GLONASS/Beidou antenna, distributed along with the X-NUCLEO-GNSS1A1 Expansion Board, must be connected to the antenna connector present on the Expansion Board. For the X-NUCLEO-GNSS1A1 proper operations, the following jumper settings must be used:
- J2 open
- J3 closed
- J4 closed
- J5 open
- J6 closed
- J7 closed
- J8 open
- J9 closed
- J10 open
- J11 closed
- J12 closed
- J13 closed
- J14 closed
- J15 closed
Tested platforms
This firmware has been tested on STM32 NUCLEO-F401RE
Example Application
To run GNSS example applications using X-NUCLEO-GNSS1A1 Expansion Board based on mbed OS, please refer to TeseoLocation page.
Diff: Components/Teseo/Teseo.cpp
- Revision:
- 0:a77f1f1f8318
- Child:
- 3:2d568ec8da14
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Components/Teseo/Teseo.cpp Fri Nov 09 17:06:11 2018 +0000 @@ -0,0 +1,1010 @@ +/** +******************************************************************************* +* @file Teseo.h +* @author AST / Central Lab +* @version V1.0.0 +* @date May-2017 +* @brief Teseo Location Class +* +******************************************************************************* +* @attention +* +* <h2><center>© COPYRIGHT(c) 2017 STMicroelectronics</center></h2> +* +* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); +* You may not use this file except in compliance with the License. +* You may obtain a copy of the License at: +* +* http://www.st.com/software_license_agreement_liberty_v2 +* +* Redistribution and use in source and binary forms, with or without modification, +* are permitted provided that the following conditions are met: +* 1. Redistributions of source code must retain the above copyright notice, +* this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* 3. Neither the name of STMicroelectronics nor the names of its contributors +* may be used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* +******************************************************************************** +*/ + +#include "Teseo.h" + +static char TESEO_NAME[] = "Teseo-LIV3F"; +/* +static char _OK[] = "OK"; +static char _Failed[] = "Failed"; +*/ + +struct teseoCmd { + char *cmd; +}; + +static struct teseoCmd teseoCmds[] = { + [Teseo::TEST] = { + .cmd = (char*)"$\n\r", + }, + [Teseo::GETSWVER] = { + .cmd = (char*)"$PSTMGETSWVER,6\n\r", + }, + [Teseo::FORCESTANDBY] = { + .cmd = (char*)"$PSTMFORCESTANDBY,00010\n\r", + }, + [Teseo::RFTESTON] = { + .cmd = (char*)"$PSTMRFTESTON,16\n\r", + }, + [Teseo::RFTESTOFF] = { + .cmd = (char*)"$PSTMRFTESTOFF\n\r", + }, + [Teseo::LOWPOWER] = { + .cmd = (char*)"$PSTMLOWPOWERONOFF,1,0,000,05,0,1,000,1,00010,01,0,0,1,01\n\r", + }, + [Teseo::FWUPDATE] = { + .cmd = (char*)"$PSTMFWUPGRADE\n\r", + } +}; + +Teseo::Teseo(PinName resetPin, + PinName wakeupPin, + PinName ppsPin, + PinName uartTxPin, + PinName uartRxPin, + Serial *serialDebug): + _loc_led2(LOC_LED2), + _reset(resetPin, 1), + _pps(ppsPin), + _wakeup(wakeupPin, 0), + _uartRx(uartRxPin), + _uartTx(uartTxPin), + _serialDebug(serialDebug) +{ + wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS); + _uart = NULL; + _i2c = NULL; + + _locState = TESEO_LOC_STATE_IDLE; + + deviceInfo = TESEO_NAME; +} + +Teseo::Teseo(PinName resetPin, + PinName wakeupPin, + PinName ppsPin, + PinName uartTxPin, + PinName uartRxPin, + I2C *i2cBus, + Serial *serialDebug): + _loc_led2(LOC_LED2), + _reset(resetPin, 1), + _pps(ppsPin), + _wakeup(wakeupPin, 0), + _uartRx(uartRxPin), + _uartTx(uartTxPin), + _serialDebug(serialDebug), + _i2c(i2cBus) +{ + wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS); + _uart = NULL; + + _locState = TESEO_LOC_STATE_IDLE; + + deviceInfo = TESEO_NAME; +} + +void +Teseo::TeseoLocRegOutput(teseo_app_output_callback app_output_cb, teseo_app_event_callback app_event_cb) +{ + appOutCb = app_output_cb; + appEventCb = app_event_cb; +} + +int +Teseo::EnableLowPower() +{ + SendCommand(LOWPOWER); + return 0; +} + +void +Teseo::_ResetFast(Serial *serialDebug) +{ + if (serialDebug) + serialDebug->printf("%s: Resetting...", TESEO_NAME); + + _reset.write(0); + + wait_ms(5); + + _reset.write(1); + + wait_ms(70); + + if (serialDebug) + serialDebug->printf("Done...\n\r"); +} + +void +Teseo::_Reset(Serial *serialDebug) +{ + if (serialDebug) + serialDebug->printf("%s: Resetting...", TESEO_NAME); + + //_pps.output(); + //_pps.write(0); + + //wait_ms(500); + + _reset.write(1); + + wait_ms(500); + + _reset.write(0); + //_pps.write(0); + + wait_ms(500); + + _reset.write(1); + + if (serialDebug) + serialDebug->printf("Done...\n\r"); + + //_pps.write(1); + + //wait_ms(1000); + + //_pps.input(); +} + +void +Teseo::_SendString(char *buf, int len) +{ + for (int i = 0; i < len; ++i) { + while (!_uart->writeable()); + _uart->putc(buf[i]); + } +} + +int +Teseo::_WakeUp() +{ + wait_ms(100); + + _wakeup.write(1); + + wait_ms(500); + + _wakeup.write(0); + + return 0; +} + +int +Teseo::_CRC(char *buf, int size) +{ + int i = 0, ch = 0; + + if (buf[0] == '$') { + ++i; + } + + if (size) { + for (; i < size; ++i) { + ch ^= buf[i]; + } + } else { + for (; buf[i] != 0; ++i) { + ch ^= buf[i]; + } + } + + return ch; +} + +void +Teseo::SendCommand(Teseo::eCmd c) +{ + char crc[3]; + + sprintf(crc, "*%02X", _CRC(teseoCmds[c].cmd, -1)); + + _SendString(teseoCmds[c].cmd, strlen(teseoCmds[c].cmd)); + _SendString(crc, 3); +} + +void +Teseo::SendCommand(char *cmd) +{ + char crc[5]; + + sprintf(crc, "*%02X\n\r", _CRC(cmd, strlen(cmd))); + //printf("CRC=%s\n\r", crc); + + _SendString(cmd, strlen(cmd)); + _SendString(crc, 5); +} + +/** TBC */ +int +Teseo::_CheckI2C() +{ + if (!_i2c) + return -1; + + _i2c->start(); + int res = _i2c->write((TESEO_I2C_ADDRESS << 1) | 1); + _i2c->stop(); + /* + * @returns + * '0' - NAK was received + * '1' - ACK was received, + * '2' - timeout + */ + return res == 1 ? 0 : -1; +} + +void +Teseo::ReadSentence(Teseo::eMsg msg) +{ + eStatus status = TESEO_STATUS_FAILURE; + + do { + osEvent evt = queue.get(); + if (evt.status == osEventMessage) { + struct _teseoMsg *message = (struct _teseoMsg *)evt.value.p; + if (message->len > 0) { + status = _GetMsg(msg, message->buf); + } + + mpool.free(message); + } + } while (status != TESEO_STATUS_SUCCESS); +} + +void +Teseo::_InitUART(int br) +{ + _uart = new (std::nothrow) Serial(_uartRx, _uartTx); + if(_uart != NULL) { + _uart->format(8, SerialBase::None, 1); + _uart->baud(br); + } else { + TESEO_LOG_INFO("Error allocating UART.\r\n"); + } +} + +bool +Teseo::setPowerMode(GPSProvider::PowerMode_t pwrMode) +{ + /* TBI */ + return false; +} + +void +Teseo::eventHandler(eTeseoLocEventType event, uint32_t data) +{ + if (appEventCb) { + appEventCb(event, data); + } +} + +void +Teseo::ReadProcess(void) +{ + char c; + + struct _teseoMsg *msg = mpool.alloc(); + msg->len = 0; + + while(true) { + if (_uart->readable()) { + c = _uart->getc(); + + if (c == '$') { + queue.put(msg); + msg = mpool.alloc(); + msg->len = 0; + } + msg->buf[msg->len++] = c; + + } else { + Thread::yield(); //wait_us(100); Allow other threads to run + } + } +} + +static void +_UARTStreamProcess(Teseo *gnss) +{ + gnss->ReadProcess(); +} + +void +Teseo::start(void) +{ + if(_locState == TESEO_LOC_STATE_IDLE) { + + _InitUART(); + _locState = TESEO_LOC_STATE_RUN; + + // Start thread for UART listener and set the highest priority + serialStreamThread = new (std::nothrow) Thread(); + if(serialStreamThread != NULL) { + serialStreamThread->set_priority(osPriorityRealtime); + serialStreamThread->start(callback(_UARTStreamProcess, this)); + } else { + TESEO_LOG_INFO("Error allocating serialStreamThread\r\n"); + } + + eventHandler(TESEO_LOC_EVENT_START_RESULT, 0); + } else { + TESEO_LOG_INFO("Already started\r\n"); + } +} + +void +Teseo::stop(void) +{ + if(_locState == TESEO_LOC_STATE_IDLE) { + return; + } + // Stop thread for UART listener + if(serialStreamThread != NULL) { + serialStreamThread->terminate(); + delete serialStreamThread; + } + + if(_uart != NULL) { + delete _uart; + _uart = NULL; + } + + _locState = TESEO_LOC_STATE_IDLE; + eventHandler(TESEO_LOC_EVENT_STOP_RESULT, 0); +} + +void +Teseo::outputHandler(uint32_t msgId, uint32_t msgType, tTeseoData *pData) +{ + switch(msgId) { + case LOC_OUTPUT_LOCATION: + + if(pData->gpgga_data.valid == VALID) { + lastLocation.valid = true; + lastLocation.lat = pData->gpgga_data.xyz.lat; + lastLocation.lon = pData->gpgga_data.xyz.lon; + lastLocation.altitude = pData->gpgga_data.xyz.alt; + lastLocation.numGPSSVs = pData->gpgga_data.sats; + lastLocation.utcTime = pData->gpgga_data.utc.utc; + } else { + lastLocation.valid = false; + } + + if (locationCallback) { + locationCallback(&lastLocation); + } + + break; + + case LOC_OUTPUT_PSTM: { + Teseo::ePSTMsg msg = (Teseo::ePSTMsg)msgType; + int code; + + switch(msg) { + case PSTMGEOFENCE: { + + /* Geofence enabling */ + if(pData->geofence_data.op == GNSS_FEATURE_EN_MSG) { + code = pData->geofence_data.result ? + GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE; + + if (code == GPS_ERROR_NONE) { + saveConfigParams(); + } + } + + /* Geofence configuration */ + if(pData->geofence_data.op == GNSS_GEOFENCE_CFG_MSG) { + code = pData->geofence_data.result ? + GPS_ERROR_GEOFENCE_CFG : GPS_ERROR_NONE; + if (geofenceCfgMessageCallback) { + geofenceCfgMessageCallback(code); + } + } + + /* Geofence Status */ + if(pData->geofence_data.op == GNSS_GEOFENCE_STATUS_MSG) { + code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE; + if(code == GPS_ERROR_NONE) { + geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh; + geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm; + geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss; + geofenceStatus.timestamp.day = pData->geofence_data.timestamp.day; + geofenceStatus.timestamp.month = pData->geofence_data.timestamp.month; + geofenceStatus.timestamp.year = pData->geofence_data.timestamp.year; + geofenceStatus.currentStatus = pData->geofence_data.status; + geofenceStatus.numGeofences = MAX_GEOFENCES_NUM; + } + if (geofenceStatusMessageCallback) { + geofenceStatusMessageCallback(&geofenceStatus, code); + } + } + + /* Geofence Alarm */ + if(pData->geofence_data.op == GNSS_GEOFENCE_ALARM_MSG) { + code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE; + if(code == GPS_ERROR_NONE) { + geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh; + geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm; + geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss; + geofenceStatus.currentStatus = pData->geofence_data.status; + geofenceStatus.idAlarm = pData->geofence_data.idAlarm; + } + if (geofenceStatusMessageCallback) { + geofenceStatusMessageCallback(&geofenceStatus, code); + } + } + } + break; + + case PSTMODO: { + /* Odometer enabling */ + if(pData->odo_data.op == GNSS_FEATURE_EN_MSG) { + + code = pData->odo_data.result ? + GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE; + + if (code == GPS_ERROR_NONE) { + saveConfigParams(); + } + } + + /* Odometer start */ + if(pData->odo_data.op == GNSS_ODO_START_MSG) { + code = pData->odo_data.result ? + GPS_ERROR_ODO_START : GPS_ERROR_NONE; + } + /* Odometer stop */ + if(pData->odo_data.op == GNSS_ODO_STOP_MSG) { + code = pData->odo_data.result ? + GPS_ERROR_ODO_STOP : GPS_ERROR_NONE; + } + } + break; + + case PSTMDATALOG: { + /* Datalog enabling */ + if(pData->datalog_data.op == GNSS_FEATURE_EN_MSG) { + + code = pData->datalog_data.result ? + GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE; + + if (code == GPS_ERROR_NONE) { + saveConfigParams(); + } + } + /* Datalog create */ + if(pData->datalog_data.op == GNSS_DATALOG_CFG_MSG) { + code = pData->datalog_data.result ? + GPS_ERROR_DATALOG_CFG : GPS_ERROR_NONE; + } + /* Datalog start */ + if(pData->datalog_data.op == GNSS_DATALOG_START_MSG) { + code = pData->datalog_data.result ? + GPS_ERROR_DATALOG_START : GPS_ERROR_NONE; + } + /* Datalog stop */ + if(pData->datalog_data.op == GNSS_DATALOG_STOP_MSG) { + code = pData->datalog_data.result ? + GPS_ERROR_DATALOG_STOP : GPS_ERROR_NONE; + } + /* Datalog erase */ + if(pData->datalog_data.op == GNSS_DATALOG_ERASE_MSG) { + code = pData->datalog_data.result ? + GPS_ERROR_DATALOG_ERASE : GPS_ERROR_NONE; + } + } + break; + + case PSTMSGL: { + /* Msg List cfg */ + code = pData->ack ? + GPS_ERROR_MSGLIST_CFG : GPS_ERROR_NONE; + + if (code == GPS_ERROR_NONE) { + saveConfigParams(); + } + } + break; + + case PSTMSAVEPAR: { + code = pData->ack ? + GPS_ERROR_SAVEPAR : GPS_ERROR_NONE; + + if (code == GPS_ERROR_NONE) { + reset(); + } + } + break; + + } /* end switch */ + } /* end case LOC_OUTPUT_PSTM */ + + break; + + default: + break; + } + + if (appOutCb) { + appOutCb(msgId, msgType, pData); + } + +} + + +eStatus +Teseo::_GetMsg(Teseo::eMsg msg, uint8_t *buffer) +{ + eStatus status = TESEO_STATUS_FAILURE; + + switch(msg) { + + case GPGGA: + status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_LOCATION, msg, &pData); + } + break; + + case GNS: + status = (eStatus)parse_gnsmsg(&pData.gns_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_NMEA, msg, &pData); + } + break; + + case GPGST: + status = (eStatus)parse_gpgst(&pData.gpgst_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_NMEA, msg, &pData); + } + break; + + case GPRMC: + status = (eStatus)parse_gprmc(&pData.gprmc_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_NMEA, msg, &pData); + } + break; + + case GSA: + status = (eStatus)parse_gsamsg(&pData.gsa_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_NMEA, msg, &pData); + } + break; + + case GSV: + status = (eStatus)parse_gsvmsg(&pData.gsv_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_NMEA, msg, &pData); + } + break; + + default: + break; + + } + + return status; +} + +void +Teseo::setVerboseMode(int level) +{ + cfgMessageList(level); +} + +void +Teseo::_GetLocationMsg(Teseo::eMsg msg, uint8_t *buffer) +{ + eStatus status; + +#if 0 + _serialDebug->printf("\n\r --------------------->\n\r"); + for (int i = 0; i < TESEO_RXBUF_LEN ; ++i) { + _serialDebug->putc((int)buffer[i]); + } + _serialDebug->printf("\n\r<---------------------\n\r"); +#endif + + status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_LOCATION, msg, &pData); + } +} + +void +Teseo::_GetPSTMsg(Teseo::ePSTMsg msg, uint8_t *buffer) +{ + eStatus status; + +#if 0 + _serialDebug->printf("\n\r --------------------->\n\r"); + for (int i = 0; i < TESEO_RXBUF_LEN ; ++i) { + _serialDebug->putc((int)buffer[i]); + } + _serialDebug->printf("\n\r<---------------------\n\r"); +#endif + + switch(msg) { + + case PSTMGEOFENCE: + status = (eStatus)parse_pstmgeofence(&pData.geofence_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_PSTM, msg, &pData); + } + break; + case PSTMODO: + status = (eStatus)parse_pstmodo(&pData.odo_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_PSTM, msg, &pData); + } + break; + case PSTMDATALOG: + status = (eStatus)parse_pstmdatalog(&pData.datalog_data, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_PSTM, msg, &pData); + } + break; + case PSTMSGL: + status = (eStatus)parse_pstmsgl(&pData.ack, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_PSTM, msg, &pData); + } + break; + case PSTMSAVEPAR: + status = (eStatus)parse_pstmsavepar(&pData.ack, buffer); + if(status == TESEO_STATUS_SUCCESS) { + outputHandler(LOC_OUTPUT_PSTM, msg, &pData); + } + break; + + default: + break; + } + /* Recover the normal state */ + +} + +void +Teseo::process(void) +{ + osEvent evt = queue.get(); + if (evt.status == osEventMessage) { + struct _teseoMsg *message = (struct _teseoMsg *)evt.value.p; + if (message->len > 0) { + + for(int m = 0; m < PSTM_NMEA_MSGS_NUM; m++) { + _GetPSTMsg((ePSTMsg)m, message->buf); + } + for(int m = 0; m < NMEA_MSGS_NUM; m++) { + _GetMsg((eMsg)m, message->buf); + } + } + + mpool.free(message); + } +} + +uint32_t +Teseo::ioctl(uint32_t command, void *arg) +{ + /* TBI */ + return 0; +} + +void +Teseo::lpmGetImmediateLocation(void) +{ + /* TBI */ +} + +const GPSProvider::LocationUpdateParams_t * +Teseo::getLastLocation(void) const +{ + return &lastLocation; +} + +void +Teseo::reset(void) +{ + _ResetFast(_serialDebug); +} + +gps_provider_error_t +Teseo::cfgMessageList(int level) +{ + int lowMask = 0x2; + int highMask = 0x80000; + + if(level == 2) { + lowMask = 0x18004F; + } + + sprintf(_teseoCmd, "$PSTMCFGMSGL,%d,%d,%x,%x", + 0, /*NMEA 0*/ + 1, /*Rate*/ + lowMask, + highMask); + + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::saveConfigParams(void) +{ + sprintf(_teseoCmd, "$PSTMSAVEPAR"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +bool +Teseo::isGeofencingSupported(void) +{ + return true; +} + +gps_provider_error_t +Teseo::enableGeofence(void) +{ + //$PSTMCFGGEOFENCE,<en>,<tol>*<checksum><cr><lf> + sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d",1,1); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +//FIXME! +gps_provider_error_t +Teseo::cfgGeofenceCircle(void) +{ + GPSGeofence::GeofenceCircle_t circle = { + .id = 0, + .enabled = 1, + .tolerance = 1, + .lat = 40.336055, + .lon = 18.120611, + .radius = 200 + }; + + sprintf(_teseoCmd, "$PSTMCFGGEOCIR,%d,%d,%lf,%lf,%lf", + circle.id, + circle.enabled, + circle.lat, + circle.lon, + circle.radius); + SendCommand(_teseoCmd); + + sprintf(_teseoCmd, "$PSTMSAVEPAR"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + + +gps_provider_error_t +Teseo::configGeofences(GPSGeofence *geofences[], unsigned geofenceCount) +{ + uint8_t trials; + + if(geofenceCount > MAX_GEOFENCES_NUM) { + return GPS_ERROR_GEOFENCE_MAX_EXCEEDED; + } + + for(uint8_t i = 0; i < geofenceCount; i++) { + trials = 1; + //printf("Teseo::configGeofences id=%d\r\n", (geofences[i]->getGeofenceCircle()).id); + /* + printf("Teseo::configGeofences en=%d\r\n", en); + printf("Teseo::configGeofences tol=%d\r\n", (geofences[i]->getGeofenceCircle()).tolerance); + printf("Teseo::configGeofences lat=%02f\r\n", (geofences[i]->getGeofenceCircle()).lat); + printf("Teseo::configGeofences lon=%02f\r\n", (geofences[i]->getGeofenceCircle()).lon); + printf("Teseo::configGeofences radius=%02f\r\n", (geofences[i]->getGeofenceCircle()).radius); + */ + sprintf(_teseoCmd, "$PSTMGEOFENCECFG,%d,%d,%d,%lf,%lf,%lf", + (geofences[i]->getGeofenceCircle()).id, + (geofences[i]->getGeofenceCircle()).enabled, + (geofences[i]->getGeofenceCircle()).tolerance, + (geofences[i]->getGeofenceCircle()).lat, + (geofences[i]->getGeofenceCircle()).lon, + (geofences[i]->getGeofenceCircle()).radius); + + do{ + trials--; + + SendCommand(_teseoCmd); + /* + sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d\n\r",1,1); + + //sprintf(_teseoCmd, "$PSTMGETSWVER,6"); + SendCommand(_teseoCmd); + + sprintf(_teseoCmd, "$PSTMSAVEPAR\n\r"); + SendCommand(_teseoCmd); + */ + //printf("Teseo::configGeofences _teseoCmd=%s\r\n", _teseoCmd); + } while (trials > 0); + } + //printf("Teseo::configGeofences sizeof(geofences)=%d numGeofences=%d strlen(_teseoCmd)=%d\r\n", sizeof(geofences), numGeofences, strlen(_teseoCmd)); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::geofenceReq(void) +{ + sprintf(_teseoCmd, "$PSTMGEOFENCEREQ"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +bool +Teseo::isDataloggingSupported(void) +{ + return true; +} + +gps_provider_error_t +Teseo::enableDatalog(void) +{ + //$PSTMCFGLOG,<en>,<circ>,<rectype>,<oneshot>,<rate>,<speed>,<dist>*<checksum><cr><lf> + sprintf(_teseoCmd, "$PSTMCFGLOG,%d,%d,%d,%d,%u,%u,%u", + 1, //Enable/Disable the log + 1, //Enable/Disable circular mode + 1, //Record type + 0, //Enable/Disable one shot mode + 5, //time interval in seconds between two consecutive logged records + 0, //minimum speed threshold + 0 //distance threshold + ); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::configDatalog(GPSDatalog *datalog) +{ + //printf("Teseo::configDatalog 0x%03x\r\n", (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer())); + //$PSTMLOGCREATE,<cfg>,<min-rate>,<min-speed>,<min-position>,<logmask>*<checksum><cr><lf> + sprintf(_teseoCmd, "$PSTMLOGCREATE,%03x,%u,%u,%u,%d", + (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer()), + datalog->getMinRate(), + datalog->getMinSpeed(), + datalog->getMinPosition(), + datalog->getLogMask() + ); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::startDatalog(void) +{ + sprintf(_teseoCmd, "$PSTMLOGSTART"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::stopDatalog(void) +{ + sprintf(_teseoCmd, "$PSTMLOGSTOP"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::eraseDatalog(void) +{ + sprintf(_teseoCmd, "$PSTMLOGERASE"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::logReqStatus(void) +{ + /* TBI */ + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::logReqQuery(GPSProvider::LogQueryParams_t &logReqQuery) +{ + /* TBI */ + return GPS_ERROR_NONE; +} + +bool +Teseo::isOdometerSupported(void) +{ + return true; +} + +gps_provider_error_t +Teseo::enableOdo(void) +{ + //$PSTMCFGODO,<en>,<enmsg>,<alarm>*<checksum><cr><lf> + sprintf(_teseoCmd, "$PSTMCFGODO,1,1,1"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::startOdo(unsigned alarmDistance) +{ + sprintf(_teseoCmd, "$PSTMODOSTART,%08x", alarmDistance); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} + +gps_provider_error_t +Teseo::stopOdo(void) +{ + sprintf(_teseoCmd, "$PSTMODOSTOP"); + SendCommand(_teseoCmd); + + return GPS_ERROR_NONE; +} +gps_provider_error_t +Teseo::resetOdo(void) +{ + /* TBI */ + return GPS_ERROR_NONE; +}