Harry Keane / X_NUCLEO_GNSS1A1

Demo

Dependents:   A_TeseoLocationNEW A_TeseoLocation

Components/Teseo/Teseo.cpp@3:2d568ec8da14, 2019-02-14 (annotated)

Committer:
apalmieri
Date:
Thu Feb 14 11:10:26 2019 +0000
Revision:
3:2d568ec8da14
Parent:
0:a77f1f1f8318
Child:
5:d91199cfc6a4
Minor fix (coding style); Update .json file

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apalmieri 0:a77f1f1f8318 1 /**
apalmieri 0:a77f1f1f8318 2 *******************************************************************************
apalmieri 0:a77f1f1f8318 3 * @file Teseo.h
apalmieri 0:a77f1f1f8318 4 * @author AST / Central Lab
apalmieri 0:a77f1f1f8318 5 * @version V1.0.0
apalmieri 0:a77f1f1f8318 6 * @date May-2017
apalmieri 0:a77f1f1f8318 7 * @brief Teseo Location Class
apalmieri 0:a77f1f1f8318 8 *
apalmieri 0:a77f1f1f8318 9 *******************************************************************************
apalmieri 0:a77f1f1f8318 10 * @attention
apalmieri 0:a77f1f1f8318 11 *
apalmieri 0:a77f1f1f8318 12 * <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
apalmieri 0:a77f1f1f8318 13 *
apalmieri 0:a77f1f1f8318 14 * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
apalmieri 0:a77f1f1f8318 15 * You may not use this file except in compliance with the License.
apalmieri 0:a77f1f1f8318 16 * You may obtain a copy of the License at:
apalmieri 0:a77f1f1f8318 17 *
apalmieri 0:a77f1f1f8318 18 * http://www.st.com/software_license_agreement_liberty_v2
apalmieri 0:a77f1f1f8318 19 *
apalmieri 0:a77f1f1f8318 20 * Redistribution and use in source and binary forms, with or without modification,
apalmieri 0:a77f1f1f8318 21 * are permitted provided that the following conditions are met:
apalmieri 0:a77f1f1f8318 22 * 1. Redistributions of source code must retain the above copyright notice,
apalmieri 0:a77f1f1f8318 23 * this list of conditions and the following disclaimer.
apalmieri 0:a77f1f1f8318 24 * 2. Redistributions in binary form must reproduce the above copyright notice,
apalmieri 0:a77f1f1f8318 25 * this list of conditions and the following disclaimer in the documentation
apalmieri 0:a77f1f1f8318 26 * and/or other materials provided with the distribution.
apalmieri 0:a77f1f1f8318 27 * 3. Neither the name of STMicroelectronics nor the names of its contributors
apalmieri 0:a77f1f1f8318 28 * may be used to endorse or promote products derived from this software
apalmieri 0:a77f1f1f8318 29 * without specific prior written permission.
apalmieri 0:a77f1f1f8318 30 *
apalmieri 0:a77f1f1f8318 31 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
apalmieri 0:a77f1f1f8318 32 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
apalmieri 0:a77f1f1f8318 33 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
apalmieri 0:a77f1f1f8318 34 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
apalmieri 0:a77f1f1f8318 35 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
apalmieri 0:a77f1f1f8318 36 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
apalmieri 0:a77f1f1f8318 37 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
apalmieri 0:a77f1f1f8318 38 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
apalmieri 0:a77f1f1f8318 39 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
apalmieri 0:a77f1f1f8318 40 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
apalmieri 0:a77f1f1f8318 41 *
apalmieri 0:a77f1f1f8318 42 ********************************************************************************
apalmieri 0:a77f1f1f8318 43 */
apalmieri 0:a77f1f1f8318 44
apalmieri 0:a77f1f1f8318 45 #include "Teseo.h"
apalmieri 0:a77f1f1f8318 46
apalmieri 0:a77f1f1f8318 47 static char TESEO_NAME[] = "Teseo-LIV3F";
apalmieri 0:a77f1f1f8318 48 /*
apalmieri 0:a77f1f1f8318 49 static char _OK[] = "OK";
apalmieri 0:a77f1f1f8318 50 static char _Failed[] = "Failed";
apalmieri 0:a77f1f1f8318 51 */
apalmieri 0:a77f1f1f8318 52
apalmieri 0:a77f1f1f8318 53 struct teseoCmd {
apalmieri 0:a77f1f1f8318 54 char *cmd;
apalmieri 0:a77f1f1f8318 55 };
apalmieri 0:a77f1f1f8318 56
apalmieri 0:a77f1f1f8318 57 static struct teseoCmd teseoCmds[] = {
apalmieri 0:a77f1f1f8318 58 [Teseo::TEST] = {
apalmieri 0:a77f1f1f8318 59 .cmd = (char*)"$\n\r",
apalmieri 0:a77f1f1f8318 60 },
apalmieri 0:a77f1f1f8318 61 [Teseo::GETSWVER] = {
apalmieri 0:a77f1f1f8318 62 .cmd = (char*)"$PSTMGETSWVER,6\n\r",
apalmieri 0:a77f1f1f8318 63 },
apalmieri 0:a77f1f1f8318 64 [Teseo::FORCESTANDBY] = {
apalmieri 0:a77f1f1f8318 65 .cmd = (char*)"$PSTMFORCESTANDBY,00010\n\r",
apalmieri 0:a77f1f1f8318 66 },
apalmieri 0:a77f1f1f8318 67 [Teseo::RFTESTON] = {
apalmieri 0:a77f1f1f8318 68 .cmd = (char*)"$PSTMRFTESTON,16\n\r",
apalmieri 0:a77f1f1f8318 69 },
apalmieri 0:a77f1f1f8318 70 [Teseo::RFTESTOFF] = {
apalmieri 0:a77f1f1f8318 71 .cmd = (char*)"$PSTMRFTESTOFF\n\r",
apalmieri 0:a77f1f1f8318 72 },
apalmieri 0:a77f1f1f8318 73 [Teseo::LOWPOWER] = {
apalmieri 0:a77f1f1f8318 74 .cmd = (char*)"$PSTMLOWPOWERONOFF,1,0,000,05,0,1,000,1,00010,01,0,0,1,01\n\r",
apalmieri 0:a77f1f1f8318 75 },
apalmieri 0:a77f1f1f8318 76 [Teseo::FWUPDATE] = {
apalmieri 0:a77f1f1f8318 77 .cmd = (char*)"$PSTMFWUPGRADE\n\r",
apalmieri 0:a77f1f1f8318 78 }
apalmieri 0:a77f1f1f8318 79 };
apalmieri 0:a77f1f1f8318 80
apalmieri 0:a77f1f1f8318 81 Teseo::Teseo(PinName resetPin,
apalmieri 0:a77f1f1f8318 82 PinName wakeupPin,
apalmieri 0:a77f1f1f8318 83 PinName ppsPin,
apalmieri 0:a77f1f1f8318 84 PinName uartTxPin,
apalmieri 0:a77f1f1f8318 85 PinName uartRxPin,
apalmieri 0:a77f1f1f8318 86 Serial *serialDebug):
apalmieri 0:a77f1f1f8318 87 _loc_led2(LOC_LED2),
apalmieri 0:a77f1f1f8318 88 _reset(resetPin, 1),
apalmieri 0:a77f1f1f8318 89 _pps(ppsPin),
apalmieri 0:a77f1f1f8318 90 _wakeup(wakeupPin, 0),
apalmieri 0:a77f1f1f8318 91 _uartRx(uartRxPin),
apalmieri 0:a77f1f1f8318 92 _uartTx(uartTxPin),
apalmieri 0:a77f1f1f8318 93 _serialDebug(serialDebug)
apalmieri 0:a77f1f1f8318 94 {
apalmieri 0:a77f1f1f8318 95 wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS);
apalmieri 0:a77f1f1f8318 96 _uart = NULL;
apalmieri 0:a77f1f1f8318 97 _i2c = NULL;
apalmieri 0:a77f1f1f8318 98
apalmieri 0:a77f1f1f8318 99 _locState = TESEO_LOC_STATE_IDLE;
apalmieri 0:a77f1f1f8318 100
apalmieri 0:a77f1f1f8318 101 deviceInfo = TESEO_NAME;
apalmieri 0:a77f1f1f8318 102 }
apalmieri 0:a77f1f1f8318 103
apalmieri 0:a77f1f1f8318 104 Teseo::Teseo(PinName resetPin,
apalmieri 0:a77f1f1f8318 105 PinName wakeupPin,
apalmieri 0:a77f1f1f8318 106 PinName ppsPin,
apalmieri 0:a77f1f1f8318 107 PinName uartTxPin,
apalmieri 0:a77f1f1f8318 108 PinName uartRxPin,
apalmieri 0:a77f1f1f8318 109 I2C *i2cBus,
apalmieri 0:a77f1f1f8318 110 Serial *serialDebug):
apalmieri 0:a77f1f1f8318 111 _loc_led2(LOC_LED2),
apalmieri 0:a77f1f1f8318 112 _reset(resetPin, 1),
apalmieri 0:a77f1f1f8318 113 _pps(ppsPin),
apalmieri 0:a77f1f1f8318 114 _wakeup(wakeupPin, 0),
apalmieri 0:a77f1f1f8318 115 _uartRx(uartRxPin),
apalmieri 0:a77f1f1f8318 116 _uartTx(uartTxPin),
apalmieri 0:a77f1f1f8318 117 _serialDebug(serialDebug),
apalmieri 0:a77f1f1f8318 118 _i2c(i2cBus)
apalmieri 0:a77f1f1f8318 119 {
apalmieri 0:a77f1f1f8318 120 wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS);
apalmieri 0:a77f1f1f8318 121 _uart = NULL;
apalmieri 0:a77f1f1f8318 122
apalmieri 0:a77f1f1f8318 123 _locState = TESEO_LOC_STATE_IDLE;
apalmieri 0:a77f1f1f8318 124
apalmieri 0:a77f1f1f8318 125 deviceInfo = TESEO_NAME;
apalmieri 0:a77f1f1f8318 126 }
apalmieri 0:a77f1f1f8318 127
apalmieri 0:a77f1f1f8318 128 void
apalmieri 0:a77f1f1f8318 129 Teseo::TeseoLocRegOutput(teseo_app_output_callback app_output_cb, teseo_app_event_callback app_event_cb)
apalmieri 0:a77f1f1f8318 130 {
apalmieri 0:a77f1f1f8318 131 appOutCb = app_output_cb;
apalmieri 0:a77f1f1f8318 132 appEventCb = app_event_cb;
apalmieri 0:a77f1f1f8318 133 }
apalmieri 0:a77f1f1f8318 134
apalmieri 0:a77f1f1f8318 135 int
apalmieri 0:a77f1f1f8318 136 Teseo::EnableLowPower()
apalmieri 0:a77f1f1f8318 137 {
apalmieri 0:a77f1f1f8318 138 SendCommand(LOWPOWER);
apalmieri 0:a77f1f1f8318 139 return 0;
apalmieri 0:a77f1f1f8318 140 }
apalmieri 0:a77f1f1f8318 141
apalmieri 0:a77f1f1f8318 142 void
apalmieri 0:a77f1f1f8318 143 Teseo::_ResetFast(Serial *serialDebug)
apalmieri 0:a77f1f1f8318 144 {
apalmieri 3:2d568ec8da14 145 if (serialDebug) {
apalmieri 0:a77f1f1f8318 146 serialDebug->printf("%s: Resetting...", TESEO_NAME);
apalmieri 3:2d568ec8da14 147 }
apalmieri 3:2d568ec8da14 148
apalmieri 0:a77f1f1f8318 149 _reset.write(0);
apalmieri 0:a77f1f1f8318 150
apalmieri 0:a77f1f1f8318 151 wait_ms(5);
apalmieri 0:a77f1f1f8318 152
apalmieri 0:a77f1f1f8318 153 _reset.write(1);
apalmieri 0:a77f1f1f8318 154
apalmieri 0:a77f1f1f8318 155 wait_ms(70);
apalmieri 0:a77f1f1f8318 156
apalmieri 3:2d568ec8da14 157 if (serialDebug) {
apalmieri 0:a77f1f1f8318 158 serialDebug->printf("Done...\n\r");
apalmieri 3:2d568ec8da14 159 }
apalmieri 0:a77f1f1f8318 160 }
apalmieri 0:a77f1f1f8318 161
apalmieri 0:a77f1f1f8318 162 void
apalmieri 0:a77f1f1f8318 163 Teseo::_Reset(Serial *serialDebug)
apalmieri 0:a77f1f1f8318 164 {
apalmieri 3:2d568ec8da14 165 if (serialDebug) {
apalmieri 0:a77f1f1f8318 166 serialDebug->printf("%s: Resetting...", TESEO_NAME);
apalmieri 3:2d568ec8da14 167 }
apalmieri 0:a77f1f1f8318 168
apalmieri 0:a77f1f1f8318 169 //_pps.output();
apalmieri 0:a77f1f1f8318 170 //_pps.write(0);
apalmieri 0:a77f1f1f8318 171
apalmieri 0:a77f1f1f8318 172 //wait_ms(500);
apalmieri 0:a77f1f1f8318 173
apalmieri 0:a77f1f1f8318 174 _reset.write(1);
apalmieri 0:a77f1f1f8318 175
apalmieri 0:a77f1f1f8318 176 wait_ms(500);
apalmieri 0:a77f1f1f8318 177
apalmieri 0:a77f1f1f8318 178 _reset.write(0);
apalmieri 0:a77f1f1f8318 179 //_pps.write(0);
apalmieri 0:a77f1f1f8318 180
apalmieri 0:a77f1f1f8318 181 wait_ms(500);
apalmieri 0:a77f1f1f8318 182
apalmieri 0:a77f1f1f8318 183 _reset.write(1);
apalmieri 0:a77f1f1f8318 184
apalmieri 3:2d568ec8da14 185 if (serialDebug) {
apalmieri 0:a77f1f1f8318 186 serialDebug->printf("Done...\n\r");
apalmieri 3:2d568ec8da14 187 }
apalmieri 0:a77f1f1f8318 188
apalmieri 0:a77f1f1f8318 189 //_pps.write(1);
apalmieri 0:a77f1f1f8318 190
apalmieri 0:a77f1f1f8318 191 //wait_ms(1000);
apalmieri 0:a77f1f1f8318 192
apalmieri 0:a77f1f1f8318 193 //_pps.input();
apalmieri 0:a77f1f1f8318 194 }
apalmieri 0:a77f1f1f8318 195
apalmieri 0:a77f1f1f8318 196 void
apalmieri 0:a77f1f1f8318 197 Teseo::_SendString(char *buf, int len)
apalmieri 0:a77f1f1f8318 198 {
apalmieri 0:a77f1f1f8318 199 for (int i = 0; i < len; ++i) {
apalmieri 0:a77f1f1f8318 200 while (!_uart->writeable());
apalmieri 0:a77f1f1f8318 201 _uart->putc(buf[i]);
apalmieri 0:a77f1f1f8318 202 }
apalmieri 0:a77f1f1f8318 203 }
apalmieri 0:a77f1f1f8318 204
apalmieri 0:a77f1f1f8318 205 int
apalmieri 0:a77f1f1f8318 206 Teseo::_WakeUp()
apalmieri 0:a77f1f1f8318 207 {
apalmieri 0:a77f1f1f8318 208 wait_ms(100);
apalmieri 0:a77f1f1f8318 209
apalmieri 0:a77f1f1f8318 210 _wakeup.write(1);
apalmieri 0:a77f1f1f8318 211
apalmieri 0:a77f1f1f8318 212 wait_ms(500);
apalmieri 0:a77f1f1f8318 213
apalmieri 0:a77f1f1f8318 214 _wakeup.write(0);
apalmieri 0:a77f1f1f8318 215
apalmieri 0:a77f1f1f8318 216 return 0;
apalmieri 0:a77f1f1f8318 217 }
apalmieri 0:a77f1f1f8318 218
apalmieri 0:a77f1f1f8318 219 int
apalmieri 0:a77f1f1f8318 220 Teseo::_CRC(char *buf, int size)
apalmieri 0:a77f1f1f8318 221 {
apalmieri 0:a77f1f1f8318 222 int i = 0, ch = 0;
apalmieri 0:a77f1f1f8318 223
apalmieri 0:a77f1f1f8318 224 if (buf[0] == '$') {
apalmieri 0:a77f1f1f8318 225 ++i;
apalmieri 0:a77f1f1f8318 226 }
apalmieri 0:a77f1f1f8318 227
apalmieri 0:a77f1f1f8318 228 if (size) {
apalmieri 0:a77f1f1f8318 229 for (; i < size; ++i) {
apalmieri 0:a77f1f1f8318 230 ch ^= buf[i];
apalmieri 0:a77f1f1f8318 231 }
apalmieri 0:a77f1f1f8318 232 } else {
apalmieri 0:a77f1f1f8318 233 for (; buf[i] != 0; ++i) {
apalmieri 0:a77f1f1f8318 234 ch ^= buf[i];
apalmieri 0:a77f1f1f8318 235 }
apalmieri 0:a77f1f1f8318 236 }
apalmieri 0:a77f1f1f8318 237
apalmieri 0:a77f1f1f8318 238 return ch;
apalmieri 0:a77f1f1f8318 239 }
apalmieri 0:a77f1f1f8318 240
apalmieri 0:a77f1f1f8318 241 void
apalmieri 0:a77f1f1f8318 242 Teseo::SendCommand(Teseo::eCmd c)
apalmieri 0:a77f1f1f8318 243 {
apalmieri 0:a77f1f1f8318 244 char crc[3];
apalmieri 0:a77f1f1f8318 245
apalmieri 0:a77f1f1f8318 246 sprintf(crc, "*%02X", _CRC(teseoCmds[c].cmd, -1));
apalmieri 0:a77f1f1f8318 247
apalmieri 0:a77f1f1f8318 248 _SendString(teseoCmds[c].cmd, strlen(teseoCmds[c].cmd));
apalmieri 0:a77f1f1f8318 249 _SendString(crc, 3);
apalmieri 0:a77f1f1f8318 250 }
apalmieri 0:a77f1f1f8318 251
apalmieri 0:a77f1f1f8318 252 void
apalmieri 0:a77f1f1f8318 253 Teseo::SendCommand(char *cmd)
apalmieri 0:a77f1f1f8318 254 {
apalmieri 0:a77f1f1f8318 255 char crc[5];
apalmieri 0:a77f1f1f8318 256
apalmieri 0:a77f1f1f8318 257 sprintf(crc, "*%02X\n\r", _CRC(cmd, strlen(cmd)));
apalmieri 0:a77f1f1f8318 258 //printf("CRC=%s\n\r", crc);
apalmieri 0:a77f1f1f8318 259
apalmieri 0:a77f1f1f8318 260 _SendString(cmd, strlen(cmd));
apalmieri 0:a77f1f1f8318 261 _SendString(crc, 5);
apalmieri 0:a77f1f1f8318 262 }
apalmieri 0:a77f1f1f8318 263
apalmieri 0:a77f1f1f8318 264 void
apalmieri 0:a77f1f1f8318 265 Teseo::ReadSentence(Teseo::eMsg msg)
apalmieri 0:a77f1f1f8318 266 {
apalmieri 0:a77f1f1f8318 267 eStatus status = TESEO_STATUS_FAILURE;
apalmieri 0:a77f1f1f8318 268
apalmieri 0:a77f1f1f8318 269 do {
apalmieri 0:a77f1f1f8318 270 osEvent evt = queue.get();
apalmieri 0:a77f1f1f8318 271 if (evt.status == osEventMessage) {
apalmieri 0:a77f1f1f8318 272 struct _teseoMsg *message = (struct _teseoMsg *)evt.value.p;
apalmieri 0:a77f1f1f8318 273 if (message->len > 0) {
apalmieri 0:a77f1f1f8318 274 status = _GetMsg(msg, message->buf);
apalmieri 0:a77f1f1f8318 275 }
apalmieri 0:a77f1f1f8318 276
apalmieri 0:a77f1f1f8318 277 mpool.free(message);
apalmieri 0:a77f1f1f8318 278 }
apalmieri 0:a77f1f1f8318 279 } while (status != TESEO_STATUS_SUCCESS);
apalmieri 0:a77f1f1f8318 280 }
apalmieri 0:a77f1f1f8318 281
apalmieri 0:a77f1f1f8318 282 void
apalmieri 0:a77f1f1f8318 283 Teseo::_InitUART(int br)
apalmieri 0:a77f1f1f8318 284 {
apalmieri 0:a77f1f1f8318 285 _uart = new (std::nothrow) Serial(_uartRx, _uartTx);
apalmieri 0:a77f1f1f8318 286 if(_uart != NULL) {
apalmieri 0:a77f1f1f8318 287 _uart->format(8, SerialBase::None, 1);
apalmieri 0:a77f1f1f8318 288 _uart->baud(br);
apalmieri 0:a77f1f1f8318 289 } else {
apalmieri 0:a77f1f1f8318 290 TESEO_LOG_INFO("Error allocating UART.\r\n");
apalmieri 0:a77f1f1f8318 291 }
apalmieri 0:a77f1f1f8318 292 }
apalmieri 0:a77f1f1f8318 293
apalmieri 0:a77f1f1f8318 294 bool
apalmieri 0:a77f1f1f8318 295 Teseo::setPowerMode(GPSProvider::PowerMode_t pwrMode)
apalmieri 0:a77f1f1f8318 296 {
apalmieri 0:a77f1f1f8318 297 /* TBI */
apalmieri 0:a77f1f1f8318 298 return false;
apalmieri 0:a77f1f1f8318 299 }
apalmieri 0:a77f1f1f8318 300
apalmieri 0:a77f1f1f8318 301 void
apalmieri 0:a77f1f1f8318 302 Teseo::eventHandler(eTeseoLocEventType event, uint32_t data)
apalmieri 0:a77f1f1f8318 303 {
apalmieri 0:a77f1f1f8318 304 if (appEventCb) {
apalmieri 0:a77f1f1f8318 305 appEventCb(event, data);
apalmieri 0:a77f1f1f8318 306 }
apalmieri 0:a77f1f1f8318 307 }
apalmieri 0:a77f1f1f8318 308
apalmieri 0:a77f1f1f8318 309 void
apalmieri 0:a77f1f1f8318 310 Teseo::ReadProcess(void)
apalmieri 0:a77f1f1f8318 311 {
apalmieri 0:a77f1f1f8318 312 char c;
apalmieri 0:a77f1f1f8318 313
apalmieri 0:a77f1f1f8318 314 struct _teseoMsg *msg = mpool.alloc();
apalmieri 0:a77f1f1f8318 315 msg->len = 0;
apalmieri 0:a77f1f1f8318 316
apalmieri 0:a77f1f1f8318 317 while(true) {
apalmieri 0:a77f1f1f8318 318 if (_uart->readable()) {
apalmieri 0:a77f1f1f8318 319 c = _uart->getc();
apalmieri 0:a77f1f1f8318 320
apalmieri 0:a77f1f1f8318 321 if (c == '$') {
apalmieri 0:a77f1f1f8318 322 queue.put(msg);
apalmieri 0:a77f1f1f8318 323 msg = mpool.alloc();
apalmieri 0:a77f1f1f8318 324 msg->len = 0;
apalmieri 0:a77f1f1f8318 325 }
apalmieri 0:a77f1f1f8318 326 msg->buf[msg->len++] = c;
apalmieri 0:a77f1f1f8318 327
apalmieri 0:a77f1f1f8318 328 } else {
apalmieri 0:a77f1f1f8318 329 Thread::yield(); //wait_us(100); Allow other threads to run
apalmieri 0:a77f1f1f8318 330 }
apalmieri 0:a77f1f1f8318 331 }
apalmieri 0:a77f1f1f8318 332 }
apalmieri 0:a77f1f1f8318 333
apalmieri 0:a77f1f1f8318 334 static void
apalmieri 0:a77f1f1f8318 335 _UARTStreamProcess(Teseo *gnss)
apalmieri 0:a77f1f1f8318 336 {
apalmieri 0:a77f1f1f8318 337 gnss->ReadProcess();
apalmieri 0:a77f1f1f8318 338 }
apalmieri 0:a77f1f1f8318 339
apalmieri 0:a77f1f1f8318 340 void
apalmieri 0:a77f1f1f8318 341 Teseo::start(void)
apalmieri 0:a77f1f1f8318 342 {
apalmieri 0:a77f1f1f8318 343 if(_locState == TESEO_LOC_STATE_IDLE) {
apalmieri 0:a77f1f1f8318 344
apalmieri 0:a77f1f1f8318 345 _InitUART();
apalmieri 0:a77f1f1f8318 346 _locState = TESEO_LOC_STATE_RUN;
apalmieri 0:a77f1f1f8318 347
apalmieri 0:a77f1f1f8318 348 // Start thread for UART listener and set the highest priority
apalmieri 0:a77f1f1f8318 349 serialStreamThread = new (std::nothrow) Thread();
apalmieri 0:a77f1f1f8318 350 if(serialStreamThread != NULL) {
apalmieri 0:a77f1f1f8318 351 serialStreamThread->set_priority(osPriorityRealtime);
apalmieri 0:a77f1f1f8318 352 serialStreamThread->start(callback(_UARTStreamProcess, this));
apalmieri 0:a77f1f1f8318 353 } else {
apalmieri 0:a77f1f1f8318 354 TESEO_LOG_INFO("Error allocating serialStreamThread\r\n");
apalmieri 0:a77f1f1f8318 355 }
apalmieri 0:a77f1f1f8318 356
apalmieri 0:a77f1f1f8318 357 eventHandler(TESEO_LOC_EVENT_START_RESULT, 0);
apalmieri 0:a77f1f1f8318 358 } else {
apalmieri 0:a77f1f1f8318 359 TESEO_LOG_INFO("Already started\r\n");
apalmieri 0:a77f1f1f8318 360 }
apalmieri 0:a77f1f1f8318 361 }
apalmieri 0:a77f1f1f8318 362
apalmieri 0:a77f1f1f8318 363 void
apalmieri 0:a77f1f1f8318 364 Teseo::stop(void)
apalmieri 0:a77f1f1f8318 365 {
apalmieri 0:a77f1f1f8318 366 if(_locState == TESEO_LOC_STATE_IDLE) {
apalmieri 0:a77f1f1f8318 367 return;
apalmieri 0:a77f1f1f8318 368 }
apalmieri 0:a77f1f1f8318 369 // Stop thread for UART listener
apalmieri 0:a77f1f1f8318 370 if(serialStreamThread != NULL) {
apalmieri 0:a77f1f1f8318 371 serialStreamThread->terminate();
apalmieri 0:a77f1f1f8318 372 delete serialStreamThread;
apalmieri 0:a77f1f1f8318 373 }
apalmieri 0:a77f1f1f8318 374
apalmieri 0:a77f1f1f8318 375 if(_uart != NULL) {
apalmieri 0:a77f1f1f8318 376 delete _uart;
apalmieri 0:a77f1f1f8318 377 _uart = NULL;
apalmieri 0:a77f1f1f8318 378 }
apalmieri 0:a77f1f1f8318 379
apalmieri 0:a77f1f1f8318 380 _locState = TESEO_LOC_STATE_IDLE;
apalmieri 0:a77f1f1f8318 381 eventHandler(TESEO_LOC_EVENT_STOP_RESULT, 0);
apalmieri 0:a77f1f1f8318 382 }
apalmieri 0:a77f1f1f8318 383
apalmieri 0:a77f1f1f8318 384 void
apalmieri 0:a77f1f1f8318 385 Teseo::outputHandler(uint32_t msgId, uint32_t msgType, tTeseoData *pData)
apalmieri 0:a77f1f1f8318 386 {
apalmieri 0:a77f1f1f8318 387 switch(msgId) {
apalmieri 0:a77f1f1f8318 388 case LOC_OUTPUT_LOCATION:
apalmieri 0:a77f1f1f8318 389
apalmieri 0:a77f1f1f8318 390 if(pData->gpgga_data.valid == VALID) {
apalmieri 0:a77f1f1f8318 391 lastLocation.valid = true;
apalmieri 0:a77f1f1f8318 392 lastLocation.lat = pData->gpgga_data.xyz.lat;
apalmieri 0:a77f1f1f8318 393 lastLocation.lon = pData->gpgga_data.xyz.lon;
apalmieri 0:a77f1f1f8318 394 lastLocation.altitude = pData->gpgga_data.xyz.alt;
apalmieri 0:a77f1f1f8318 395 lastLocation.numGPSSVs = pData->gpgga_data.sats;
apalmieri 0:a77f1f1f8318 396 lastLocation.utcTime = pData->gpgga_data.utc.utc;
apalmieri 0:a77f1f1f8318 397 } else {
apalmieri 0:a77f1f1f8318 398 lastLocation.valid = false;
apalmieri 0:a77f1f1f8318 399 }
apalmieri 0:a77f1f1f8318 400
apalmieri 0:a77f1f1f8318 401 if (locationCallback) {
apalmieri 0:a77f1f1f8318 402 locationCallback(&lastLocation);
apalmieri 0:a77f1f1f8318 403 }
apalmieri 0:a77f1f1f8318 404
apalmieri 0:a77f1f1f8318 405 break;
apalmieri 0:a77f1f1f8318 406
apalmieri 0:a77f1f1f8318 407 case LOC_OUTPUT_PSTM: {
apalmieri 0:a77f1f1f8318 408 Teseo::ePSTMsg msg = (Teseo::ePSTMsg)msgType;
apalmieri 0:a77f1f1f8318 409 int code;
apalmieri 0:a77f1f1f8318 410
apalmieri 0:a77f1f1f8318 411 switch(msg) {
apalmieri 0:a77f1f1f8318 412 case PSTMGEOFENCE: {
apalmieri 0:a77f1f1f8318 413
apalmieri 0:a77f1f1f8318 414 /* Geofence enabling */
apalmieri 0:a77f1f1f8318 415 if(pData->geofence_data.op == GNSS_FEATURE_EN_MSG) {
apalmieri 0:a77f1f1f8318 416 code = pData->geofence_data.result ?
apalmieri 0:a77f1f1f8318 417 GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 418
apalmieri 0:a77f1f1f8318 419 if (code == GPS_ERROR_NONE) {
apalmieri 0:a77f1f1f8318 420 saveConfigParams();
apalmieri 0:a77f1f1f8318 421 }
apalmieri 0:a77f1f1f8318 422 }
apalmieri 0:a77f1f1f8318 423
apalmieri 0:a77f1f1f8318 424 /* Geofence configuration */
apalmieri 0:a77f1f1f8318 425 if(pData->geofence_data.op == GNSS_GEOFENCE_CFG_MSG) {
apalmieri 0:a77f1f1f8318 426 code = pData->geofence_data.result ?
apalmieri 0:a77f1f1f8318 427 GPS_ERROR_GEOFENCE_CFG : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 428 if (geofenceCfgMessageCallback) {
apalmieri 0:a77f1f1f8318 429 geofenceCfgMessageCallback(code);
apalmieri 0:a77f1f1f8318 430 }
apalmieri 0:a77f1f1f8318 431 }
apalmieri 0:a77f1f1f8318 432
apalmieri 0:a77f1f1f8318 433 /* Geofence Status */
apalmieri 0:a77f1f1f8318 434 if(pData->geofence_data.op == GNSS_GEOFENCE_STATUS_MSG) {
apalmieri 0:a77f1f1f8318 435 code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 436 if(code == GPS_ERROR_NONE) {
apalmieri 0:a77f1f1f8318 437 geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh;
apalmieri 0:a77f1f1f8318 438 geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm;
apalmieri 0:a77f1f1f8318 439 geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss;
apalmieri 0:a77f1f1f8318 440 geofenceStatus.timestamp.day = pData->geofence_data.timestamp.day;
apalmieri 0:a77f1f1f8318 441 geofenceStatus.timestamp.month = pData->geofence_data.timestamp.month;
apalmieri 0:a77f1f1f8318 442 geofenceStatus.timestamp.year = pData->geofence_data.timestamp.year;
apalmieri 0:a77f1f1f8318 443 geofenceStatus.currentStatus = pData->geofence_data.status;
apalmieri 0:a77f1f1f8318 444 geofenceStatus.numGeofences = MAX_GEOFENCES_NUM;
apalmieri 0:a77f1f1f8318 445 }
apalmieri 0:a77f1f1f8318 446 if (geofenceStatusMessageCallback) {
apalmieri 0:a77f1f1f8318 447 geofenceStatusMessageCallback(&geofenceStatus, code);
apalmieri 0:a77f1f1f8318 448 }
apalmieri 0:a77f1f1f8318 449 }
apalmieri 0:a77f1f1f8318 450
apalmieri 0:a77f1f1f8318 451 /* Geofence Alarm */
apalmieri 0:a77f1f1f8318 452 if(pData->geofence_data.op == GNSS_GEOFENCE_ALARM_MSG) {
apalmieri 0:a77f1f1f8318 453 code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 454 if(code == GPS_ERROR_NONE) {
apalmieri 0:a77f1f1f8318 455 geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh;
apalmieri 0:a77f1f1f8318 456 geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm;
apalmieri 0:a77f1f1f8318 457 geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss;
apalmieri 0:a77f1f1f8318 458 geofenceStatus.currentStatus = pData->geofence_data.status;
apalmieri 0:a77f1f1f8318 459 geofenceStatus.idAlarm = pData->geofence_data.idAlarm;
apalmieri 0:a77f1f1f8318 460 }
apalmieri 0:a77f1f1f8318 461 if (geofenceStatusMessageCallback) {
apalmieri 0:a77f1f1f8318 462 geofenceStatusMessageCallback(&geofenceStatus, code);
apalmieri 0:a77f1f1f8318 463 }
apalmieri 0:a77f1f1f8318 464 }
apalmieri 0:a77f1f1f8318 465 }
apalmieri 0:a77f1f1f8318 466 break;
apalmieri 0:a77f1f1f8318 467
apalmieri 0:a77f1f1f8318 468 case PSTMODO: {
apalmieri 0:a77f1f1f8318 469 /* Odometer enabling */
apalmieri 0:a77f1f1f8318 470 if(pData->odo_data.op == GNSS_FEATURE_EN_MSG) {
apalmieri 0:a77f1f1f8318 471
apalmieri 0:a77f1f1f8318 472 code = pData->odo_data.result ?
apalmieri 0:a77f1f1f8318 473 GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 474
apalmieri 0:a77f1f1f8318 475 if (code == GPS_ERROR_NONE) {
apalmieri 0:a77f1f1f8318 476 saveConfigParams();
apalmieri 0:a77f1f1f8318 477 }
apalmieri 0:a77f1f1f8318 478 }
apalmieri 0:a77f1f1f8318 479
apalmieri 0:a77f1f1f8318 480 /* Odometer start */
apalmieri 0:a77f1f1f8318 481 if(pData->odo_data.op == GNSS_ODO_START_MSG) {
apalmieri 0:a77f1f1f8318 482 code = pData->odo_data.result ?
apalmieri 0:a77f1f1f8318 483 GPS_ERROR_ODO_START : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 484 }
apalmieri 0:a77f1f1f8318 485 /* Odometer stop */
apalmieri 0:a77f1f1f8318 486 if(pData->odo_data.op == GNSS_ODO_STOP_MSG) {
apalmieri 0:a77f1f1f8318 487 code = pData->odo_data.result ?
apalmieri 0:a77f1f1f8318 488 GPS_ERROR_ODO_STOP : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 489 }
apalmieri 0:a77f1f1f8318 490 }
apalmieri 0:a77f1f1f8318 491 break;
apalmieri 0:a77f1f1f8318 492
apalmieri 0:a77f1f1f8318 493 case PSTMDATALOG: {
apalmieri 0:a77f1f1f8318 494 /* Datalog enabling */
apalmieri 0:a77f1f1f8318 495 if(pData->datalog_data.op == GNSS_FEATURE_EN_MSG) {
apalmieri 0:a77f1f1f8318 496
apalmieri 0:a77f1f1f8318 497 code = pData->datalog_data.result ?
apalmieri 0:a77f1f1f8318 498 GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 499
apalmieri 0:a77f1f1f8318 500 if (code == GPS_ERROR_NONE) {
apalmieri 0:a77f1f1f8318 501 saveConfigParams();
apalmieri 0:a77f1f1f8318 502 }
apalmieri 0:a77f1f1f8318 503 }
apalmieri 0:a77f1f1f8318 504 /* Datalog create */
apalmieri 0:a77f1f1f8318 505 if(pData->datalog_data.op == GNSS_DATALOG_CFG_MSG) {
apalmieri 0:a77f1f1f8318 506 code = pData->datalog_data.result ?
apalmieri 0:a77f1f1f8318 507 GPS_ERROR_DATALOG_CFG : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 508 }
apalmieri 0:a77f1f1f8318 509 /* Datalog start */
apalmieri 0:a77f1f1f8318 510 if(pData->datalog_data.op == GNSS_DATALOG_START_MSG) {
apalmieri 0:a77f1f1f8318 511 code = pData->datalog_data.result ?
apalmieri 0:a77f1f1f8318 512 GPS_ERROR_DATALOG_START : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 513 }
apalmieri 0:a77f1f1f8318 514 /* Datalog stop */
apalmieri 0:a77f1f1f8318 515 if(pData->datalog_data.op == GNSS_DATALOG_STOP_MSG) {
apalmieri 0:a77f1f1f8318 516 code = pData->datalog_data.result ?
apalmieri 0:a77f1f1f8318 517 GPS_ERROR_DATALOG_STOP : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 518 }
apalmieri 0:a77f1f1f8318 519 /* Datalog erase */
apalmieri 0:a77f1f1f8318 520 if(pData->datalog_data.op == GNSS_DATALOG_ERASE_MSG) {
apalmieri 0:a77f1f1f8318 521 code = pData->datalog_data.result ?
apalmieri 0:a77f1f1f8318 522 GPS_ERROR_DATALOG_ERASE : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 523 }
apalmieri 0:a77f1f1f8318 524 }
apalmieri 0:a77f1f1f8318 525 break;
apalmieri 0:a77f1f1f8318 526
apalmieri 0:a77f1f1f8318 527 case PSTMSGL: {
apalmieri 0:a77f1f1f8318 528 /* Msg List cfg */
apalmieri 0:a77f1f1f8318 529 code = pData->ack ?
apalmieri 0:a77f1f1f8318 530 GPS_ERROR_MSGLIST_CFG : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 531
apalmieri 0:a77f1f1f8318 532 if (code == GPS_ERROR_NONE) {
apalmieri 0:a77f1f1f8318 533 saveConfigParams();
apalmieri 0:a77f1f1f8318 534 }
apalmieri 0:a77f1f1f8318 535 }
apalmieri 0:a77f1f1f8318 536 break;
apalmieri 0:a77f1f1f8318 537
apalmieri 0:a77f1f1f8318 538 case PSTMSAVEPAR: {
apalmieri 0:a77f1f1f8318 539 code = pData->ack ?
apalmieri 0:a77f1f1f8318 540 GPS_ERROR_SAVEPAR : GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 541
apalmieri 0:a77f1f1f8318 542 if (code == GPS_ERROR_NONE) {
apalmieri 0:a77f1f1f8318 543 reset();
apalmieri 0:a77f1f1f8318 544 }
apalmieri 0:a77f1f1f8318 545 }
apalmieri 0:a77f1f1f8318 546 break;
apalmieri 0:a77f1f1f8318 547
apalmieri 0:a77f1f1f8318 548 } /* end switch */
apalmieri 0:a77f1f1f8318 549 } /* end case LOC_OUTPUT_PSTM */
apalmieri 0:a77f1f1f8318 550
apalmieri 0:a77f1f1f8318 551 break;
apalmieri 0:a77f1f1f8318 552
apalmieri 0:a77f1f1f8318 553 default:
apalmieri 0:a77f1f1f8318 554 break;
apalmieri 0:a77f1f1f8318 555 }
apalmieri 0:a77f1f1f8318 556
apalmieri 0:a77f1f1f8318 557 if (appOutCb) {
apalmieri 0:a77f1f1f8318 558 appOutCb(msgId, msgType, pData);
apalmieri 0:a77f1f1f8318 559 }
apalmieri 0:a77f1f1f8318 560
apalmieri 0:a77f1f1f8318 561 }
apalmieri 0:a77f1f1f8318 562
apalmieri 0:a77f1f1f8318 563
apalmieri 0:a77f1f1f8318 564 eStatus
apalmieri 0:a77f1f1f8318 565 Teseo::_GetMsg(Teseo::eMsg msg, uint8_t *buffer)
apalmieri 0:a77f1f1f8318 566 {
apalmieri 0:a77f1f1f8318 567 eStatus status = TESEO_STATUS_FAILURE;
apalmieri 0:a77f1f1f8318 568
apalmieri 0:a77f1f1f8318 569 switch(msg) {
apalmieri 0:a77f1f1f8318 570
apalmieri 0:a77f1f1f8318 571 case GPGGA:
apalmieri 0:a77f1f1f8318 572 status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer);
apalmieri 0:a77f1f1f8318 573 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 574 outputHandler(LOC_OUTPUT_LOCATION, msg, &pData);
apalmieri 0:a77f1f1f8318 575 }
apalmieri 0:a77f1f1f8318 576 break;
apalmieri 0:a77f1f1f8318 577
apalmieri 0:a77f1f1f8318 578 case GNS:
apalmieri 0:a77f1f1f8318 579 status = (eStatus)parse_gnsmsg(&pData.gns_data, buffer);
apalmieri 0:a77f1f1f8318 580 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 581 outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
apalmieri 0:a77f1f1f8318 582 }
apalmieri 0:a77f1f1f8318 583 break;
apalmieri 0:a77f1f1f8318 584
apalmieri 0:a77f1f1f8318 585 case GPGST:
apalmieri 0:a77f1f1f8318 586 status = (eStatus)parse_gpgst(&pData.gpgst_data, buffer);
apalmieri 0:a77f1f1f8318 587 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 588 outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
apalmieri 0:a77f1f1f8318 589 }
apalmieri 0:a77f1f1f8318 590 break;
apalmieri 0:a77f1f1f8318 591
apalmieri 0:a77f1f1f8318 592 case GPRMC:
apalmieri 0:a77f1f1f8318 593 status = (eStatus)parse_gprmc(&pData.gprmc_data, buffer);
apalmieri 0:a77f1f1f8318 594 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 595 outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
apalmieri 0:a77f1f1f8318 596 }
apalmieri 0:a77f1f1f8318 597 break;
apalmieri 0:a77f1f1f8318 598
apalmieri 0:a77f1f1f8318 599 case GSA:
apalmieri 0:a77f1f1f8318 600 status = (eStatus)parse_gsamsg(&pData.gsa_data, buffer);
apalmieri 0:a77f1f1f8318 601 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 602 outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
apalmieri 0:a77f1f1f8318 603 }
apalmieri 0:a77f1f1f8318 604 break;
apalmieri 0:a77f1f1f8318 605
apalmieri 0:a77f1f1f8318 606 case GSV:
apalmieri 0:a77f1f1f8318 607 status = (eStatus)parse_gsvmsg(&pData.gsv_data, buffer);
apalmieri 0:a77f1f1f8318 608 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 609 outputHandler(LOC_OUTPUT_NMEA, msg, &pData);
apalmieri 0:a77f1f1f8318 610 }
apalmieri 0:a77f1f1f8318 611 break;
apalmieri 0:a77f1f1f8318 612
apalmieri 0:a77f1f1f8318 613 default:
apalmieri 0:a77f1f1f8318 614 break;
apalmieri 0:a77f1f1f8318 615
apalmieri 0:a77f1f1f8318 616 }
apalmieri 0:a77f1f1f8318 617
apalmieri 0:a77f1f1f8318 618 return status;
apalmieri 0:a77f1f1f8318 619 }
apalmieri 0:a77f1f1f8318 620
apalmieri 0:a77f1f1f8318 621 void
apalmieri 0:a77f1f1f8318 622 Teseo::setVerboseMode(int level)
apalmieri 0:a77f1f1f8318 623 {
apalmieri 0:a77f1f1f8318 624 cfgMessageList(level);
apalmieri 0:a77f1f1f8318 625 }
apalmieri 0:a77f1f1f8318 626
apalmieri 0:a77f1f1f8318 627 void
apalmieri 0:a77f1f1f8318 628 Teseo::_GetLocationMsg(Teseo::eMsg msg, uint8_t *buffer)
apalmieri 0:a77f1f1f8318 629 {
apalmieri 0:a77f1f1f8318 630 eStatus status;
apalmieri 0:a77f1f1f8318 631
apalmieri 0:a77f1f1f8318 632 #if 0
apalmieri 0:a77f1f1f8318 633 _serialDebug->printf("\n\r --------------------->\n\r");
apalmieri 0:a77f1f1f8318 634 for (int i = 0; i < TESEO_RXBUF_LEN ; ++i) {
apalmieri 0:a77f1f1f8318 635 _serialDebug->putc((int)buffer[i]);
apalmieri 0:a77f1f1f8318 636 }
apalmieri 0:a77f1f1f8318 637 _serialDebug->printf("\n\r<---------------------\n\r");
apalmieri 0:a77f1f1f8318 638 #endif
apalmieri 0:a77f1f1f8318 639
apalmieri 0:a77f1f1f8318 640 status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer);
apalmieri 0:a77f1f1f8318 641 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 642 outputHandler(LOC_OUTPUT_LOCATION, msg, &pData);
apalmieri 0:a77f1f1f8318 643 }
apalmieri 0:a77f1f1f8318 644 }
apalmieri 0:a77f1f1f8318 645
apalmieri 0:a77f1f1f8318 646 void
apalmieri 0:a77f1f1f8318 647 Teseo::_GetPSTMsg(Teseo::ePSTMsg msg, uint8_t *buffer)
apalmieri 0:a77f1f1f8318 648 {
apalmieri 0:a77f1f1f8318 649 eStatus status;
apalmieri 0:a77f1f1f8318 650
apalmieri 0:a77f1f1f8318 651 #if 0
apalmieri 0:a77f1f1f8318 652 _serialDebug->printf("\n\r --------------------->\n\r");
apalmieri 0:a77f1f1f8318 653 for (int i = 0; i < TESEO_RXBUF_LEN ; ++i) {
apalmieri 0:a77f1f1f8318 654 _serialDebug->putc((int)buffer[i]);
apalmieri 0:a77f1f1f8318 655 }
apalmieri 0:a77f1f1f8318 656 _serialDebug->printf("\n\r<---------------------\n\r");
apalmieri 0:a77f1f1f8318 657 #endif
apalmieri 0:a77f1f1f8318 658
apalmieri 0:a77f1f1f8318 659 switch(msg) {
apalmieri 0:a77f1f1f8318 660
apalmieri 0:a77f1f1f8318 661 case PSTMGEOFENCE:
apalmieri 0:a77f1f1f8318 662 status = (eStatus)parse_pstmgeofence(&pData.geofence_data, buffer);
apalmieri 0:a77f1f1f8318 663 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 664 outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
apalmieri 0:a77f1f1f8318 665 }
apalmieri 0:a77f1f1f8318 666 break;
apalmieri 0:a77f1f1f8318 667 case PSTMODO:
apalmieri 0:a77f1f1f8318 668 status = (eStatus)parse_pstmodo(&pData.odo_data, buffer);
apalmieri 0:a77f1f1f8318 669 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 670 outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
apalmieri 0:a77f1f1f8318 671 }
apalmieri 0:a77f1f1f8318 672 break;
apalmieri 0:a77f1f1f8318 673 case PSTMDATALOG:
apalmieri 0:a77f1f1f8318 674 status = (eStatus)parse_pstmdatalog(&pData.datalog_data, buffer);
apalmieri 0:a77f1f1f8318 675 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 676 outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
apalmieri 0:a77f1f1f8318 677 }
apalmieri 0:a77f1f1f8318 678 break;
apalmieri 0:a77f1f1f8318 679 case PSTMSGL:
apalmieri 0:a77f1f1f8318 680 status = (eStatus)parse_pstmsgl(&pData.ack, buffer);
apalmieri 0:a77f1f1f8318 681 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 682 outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
apalmieri 0:a77f1f1f8318 683 }
apalmieri 0:a77f1f1f8318 684 break;
apalmieri 0:a77f1f1f8318 685 case PSTMSAVEPAR:
apalmieri 0:a77f1f1f8318 686 status = (eStatus)parse_pstmsavepar(&pData.ack, buffer);
apalmieri 0:a77f1f1f8318 687 if(status == TESEO_STATUS_SUCCESS) {
apalmieri 0:a77f1f1f8318 688 outputHandler(LOC_OUTPUT_PSTM, msg, &pData);
apalmieri 0:a77f1f1f8318 689 }
apalmieri 0:a77f1f1f8318 690 break;
apalmieri 0:a77f1f1f8318 691
apalmieri 0:a77f1f1f8318 692 default:
apalmieri 0:a77f1f1f8318 693 break;
apalmieri 0:a77f1f1f8318 694 }
apalmieri 0:a77f1f1f8318 695 /* Recover the normal state */
apalmieri 0:a77f1f1f8318 696
apalmieri 0:a77f1f1f8318 697 }
apalmieri 0:a77f1f1f8318 698
apalmieri 0:a77f1f1f8318 699 void
apalmieri 0:a77f1f1f8318 700 Teseo::process(void)
apalmieri 0:a77f1f1f8318 701 {
apalmieri 0:a77f1f1f8318 702 osEvent evt = queue.get();
apalmieri 0:a77f1f1f8318 703 if (evt.status == osEventMessage) {
apalmieri 0:a77f1f1f8318 704 struct _teseoMsg *message = (struct _teseoMsg *)evt.value.p;
apalmieri 0:a77f1f1f8318 705 if (message->len > 0) {
apalmieri 0:a77f1f1f8318 706
apalmieri 0:a77f1f1f8318 707 for(int m = 0; m < PSTM_NMEA_MSGS_NUM; m++) {
apalmieri 0:a77f1f1f8318 708 _GetPSTMsg((ePSTMsg)m, message->buf);
apalmieri 0:a77f1f1f8318 709 }
apalmieri 0:a77f1f1f8318 710 for(int m = 0; m < NMEA_MSGS_NUM; m++) {
apalmieri 0:a77f1f1f8318 711 _GetMsg((eMsg)m, message->buf);
apalmieri 0:a77f1f1f8318 712 }
apalmieri 0:a77f1f1f8318 713 }
apalmieri 0:a77f1f1f8318 714
apalmieri 0:a77f1f1f8318 715 mpool.free(message);
apalmieri 0:a77f1f1f8318 716 }
apalmieri 0:a77f1f1f8318 717 }
apalmieri 0:a77f1f1f8318 718
apalmieri 0:a77f1f1f8318 719 const GPSProvider::LocationUpdateParams_t *
apalmieri 0:a77f1f1f8318 720 Teseo::getLastLocation(void) const
apalmieri 0:a77f1f1f8318 721 {
apalmieri 0:a77f1f1f8318 722 return &lastLocation;
apalmieri 0:a77f1f1f8318 723 }
apalmieri 0:a77f1f1f8318 724
apalmieri 0:a77f1f1f8318 725 void
apalmieri 0:a77f1f1f8318 726 Teseo::reset(void)
apalmieri 0:a77f1f1f8318 727 {
apalmieri 0:a77f1f1f8318 728 _ResetFast(_serialDebug);
apalmieri 0:a77f1f1f8318 729 }
apalmieri 0:a77f1f1f8318 730
apalmieri 0:a77f1f1f8318 731 gps_provider_error_t
apalmieri 0:a77f1f1f8318 732 Teseo::cfgMessageList(int level)
apalmieri 0:a77f1f1f8318 733 {
apalmieri 0:a77f1f1f8318 734 int lowMask = 0x2;
apalmieri 0:a77f1f1f8318 735 int highMask = 0x80000;
apalmieri 0:a77f1f1f8318 736
apalmieri 0:a77f1f1f8318 737 if(level == 2) {
apalmieri 0:a77f1f1f8318 738 lowMask = 0x18004F;
apalmieri 0:a77f1f1f8318 739 }
apalmieri 0:a77f1f1f8318 740
apalmieri 0:a77f1f1f8318 741 sprintf(_teseoCmd, "$PSTMCFGMSGL,%d,%d,%x,%x",
apalmieri 0:a77f1f1f8318 742 0, /*NMEA 0*/
apalmieri 0:a77f1f1f8318 743 1, /*Rate*/
apalmieri 0:a77f1f1f8318 744 lowMask,
apalmieri 0:a77f1f1f8318 745 highMask);
apalmieri 0:a77f1f1f8318 746
apalmieri 0:a77f1f1f8318 747 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 748
apalmieri 0:a77f1f1f8318 749 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 750 }
apalmieri 0:a77f1f1f8318 751
apalmieri 0:a77f1f1f8318 752 gps_provider_error_t
apalmieri 0:a77f1f1f8318 753 Teseo::saveConfigParams(void)
apalmieri 0:a77f1f1f8318 754 {
apalmieri 0:a77f1f1f8318 755 sprintf(_teseoCmd, "$PSTMSAVEPAR");
apalmieri 0:a77f1f1f8318 756 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 757
apalmieri 0:a77f1f1f8318 758 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 759 }
apalmieri 0:a77f1f1f8318 760
apalmieri 0:a77f1f1f8318 761 bool
apalmieri 0:a77f1f1f8318 762 Teseo::isGeofencingSupported(void)
apalmieri 0:a77f1f1f8318 763 {
apalmieri 0:a77f1f1f8318 764 return true;
apalmieri 0:a77f1f1f8318 765 }
apalmieri 0:a77f1f1f8318 766
apalmieri 0:a77f1f1f8318 767 gps_provider_error_t
apalmieri 0:a77f1f1f8318 768 Teseo::enableGeofence(void)
apalmieri 0:a77f1f1f8318 769 {
apalmieri 0:a77f1f1f8318 770 //$PSTMCFGGEOFENCE,<en>,<tol>*<checksum><cr><lf>
apalmieri 0:a77f1f1f8318 771 sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d",1,1);
apalmieri 0:a77f1f1f8318 772 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 773
apalmieri 0:a77f1f1f8318 774 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 775 }
apalmieri 0:a77f1f1f8318 776
apalmieri 0:a77f1f1f8318 777 gps_provider_error_t
apalmieri 0:a77f1f1f8318 778 Teseo::cfgGeofenceCircle(void)
apalmieri 0:a77f1f1f8318 779 {
apalmieri 0:a77f1f1f8318 780 GPSGeofence::GeofenceCircle_t circle = {
apalmieri 0:a77f1f1f8318 781 .id = 0,
apalmieri 0:a77f1f1f8318 782 .enabled = 1,
apalmieri 0:a77f1f1f8318 783 .tolerance = 1,
apalmieri 0:a77f1f1f8318 784 .lat = 40.336055,
apalmieri 0:a77f1f1f8318 785 .lon = 18.120611,
apalmieri 0:a77f1f1f8318 786 .radius = 200
apalmieri 0:a77f1f1f8318 787 };
apalmieri 0:a77f1f1f8318 788
apalmieri 0:a77f1f1f8318 789 sprintf(_teseoCmd, "$PSTMCFGGEOCIR,%d,%d,%lf,%lf,%lf",
apalmieri 0:a77f1f1f8318 790 circle.id,
apalmieri 0:a77f1f1f8318 791 circle.enabled,
apalmieri 0:a77f1f1f8318 792 circle.lat,
apalmieri 0:a77f1f1f8318 793 circle.lon,
apalmieri 0:a77f1f1f8318 794 circle.radius);
apalmieri 0:a77f1f1f8318 795 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 796
apalmieri 0:a77f1f1f8318 797 sprintf(_teseoCmd, "$PSTMSAVEPAR");
apalmieri 0:a77f1f1f8318 798 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 799
apalmieri 0:a77f1f1f8318 800 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 801 }
apalmieri 0:a77f1f1f8318 802
apalmieri 0:a77f1f1f8318 803
apalmieri 0:a77f1f1f8318 804 gps_provider_error_t
apalmieri 0:a77f1f1f8318 805 Teseo::configGeofences(GPSGeofence *geofences[], unsigned geofenceCount)
apalmieri 0:a77f1f1f8318 806 {
apalmieri 0:a77f1f1f8318 807 uint8_t trials;
apalmieri 0:a77f1f1f8318 808
apalmieri 0:a77f1f1f8318 809 if(geofenceCount > MAX_GEOFENCES_NUM) {
apalmieri 0:a77f1f1f8318 810 return GPS_ERROR_GEOFENCE_MAX_EXCEEDED;
apalmieri 0:a77f1f1f8318 811 }
apalmieri 0:a77f1f1f8318 812
apalmieri 0:a77f1f1f8318 813 for(uint8_t i = 0; i < geofenceCount; i++) {
apalmieri 0:a77f1f1f8318 814 trials = 1;
apalmieri 0:a77f1f1f8318 815 //printf("Teseo::configGeofences id=%d\r\n", (geofences[i]->getGeofenceCircle()).id);
apalmieri 0:a77f1f1f8318 816 /*
apalmieri 0:a77f1f1f8318 817 printf("Teseo::configGeofences en=%d\r\n", en);
apalmieri 0:a77f1f1f8318 818 printf("Teseo::configGeofences tol=%d\r\n", (geofences[i]->getGeofenceCircle()).tolerance);
apalmieri 0:a77f1f1f8318 819 printf("Teseo::configGeofences lat=%02f\r\n", (geofences[i]->getGeofenceCircle()).lat);
apalmieri 0:a77f1f1f8318 820 printf("Teseo::configGeofences lon=%02f\r\n", (geofences[i]->getGeofenceCircle()).lon);
apalmieri 0:a77f1f1f8318 821 printf("Teseo::configGeofences radius=%02f\r\n", (geofences[i]->getGeofenceCircle()).radius);
apalmieri 0:a77f1f1f8318 822 */
apalmieri 0:a77f1f1f8318 823 sprintf(_teseoCmd, "$PSTMGEOFENCECFG,%d,%d,%d,%lf,%lf,%lf",
apalmieri 0:a77f1f1f8318 824 (geofences[i]->getGeofenceCircle()).id,
apalmieri 0:a77f1f1f8318 825 (geofences[i]->getGeofenceCircle()).enabled,
apalmieri 0:a77f1f1f8318 826 (geofences[i]->getGeofenceCircle()).tolerance,
apalmieri 0:a77f1f1f8318 827 (geofences[i]->getGeofenceCircle()).lat,
apalmieri 0:a77f1f1f8318 828 (geofences[i]->getGeofenceCircle()).lon,
apalmieri 0:a77f1f1f8318 829 (geofences[i]->getGeofenceCircle()).radius);
apalmieri 0:a77f1f1f8318 830
apalmieri 0:a77f1f1f8318 831 do{
apalmieri 0:a77f1f1f8318 832 trials--;
apalmieri 0:a77f1f1f8318 833
apalmieri 0:a77f1f1f8318 834 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 835 /*
apalmieri 0:a77f1f1f8318 836 sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d\n\r",1,1);
apalmieri 0:a77f1f1f8318 837
apalmieri 0:a77f1f1f8318 838 //sprintf(_teseoCmd, "$PSTMGETSWVER,6");
apalmieri 0:a77f1f1f8318 839 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 840
apalmieri 0:a77f1f1f8318 841 sprintf(_teseoCmd, "$PSTMSAVEPAR\n\r");
apalmieri 0:a77f1f1f8318 842 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 843 */
apalmieri 0:a77f1f1f8318 844 //printf("Teseo::configGeofences _teseoCmd=%s\r\n", _teseoCmd);
apalmieri 0:a77f1f1f8318 845 } while (trials > 0);
apalmieri 0:a77f1f1f8318 846 }
apalmieri 0:a77f1f1f8318 847 //printf("Teseo::configGeofences sizeof(geofences)=%d numGeofences=%d strlen(_teseoCmd)=%d\r\n", sizeof(geofences), numGeofences, strlen(_teseoCmd));
apalmieri 0:a77f1f1f8318 848
apalmieri 0:a77f1f1f8318 849 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 850 }
apalmieri 0:a77f1f1f8318 851
apalmieri 0:a77f1f1f8318 852 gps_provider_error_t
apalmieri 0:a77f1f1f8318 853 Teseo::geofenceReq(void)
apalmieri 0:a77f1f1f8318 854 {
apalmieri 0:a77f1f1f8318 855 sprintf(_teseoCmd, "$PSTMGEOFENCEREQ");
apalmieri 0:a77f1f1f8318 856 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 857
apalmieri 0:a77f1f1f8318 858 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 859 }
apalmieri 0:a77f1f1f8318 860
apalmieri 0:a77f1f1f8318 861 bool
apalmieri 0:a77f1f1f8318 862 Teseo::isDataloggingSupported(void)
apalmieri 0:a77f1f1f8318 863 {
apalmieri 0:a77f1f1f8318 864 return true;
apalmieri 0:a77f1f1f8318 865 }
apalmieri 0:a77f1f1f8318 866
apalmieri 0:a77f1f1f8318 867 gps_provider_error_t
apalmieri 0:a77f1f1f8318 868 Teseo::enableDatalog(void)
apalmieri 0:a77f1f1f8318 869 {
apalmieri 0:a77f1f1f8318 870 //$PSTMCFGLOG,<en>,<circ>,<rectype>,<oneshot>,<rate>,<speed>,<dist>*<checksum><cr><lf>
apalmieri 0:a77f1f1f8318 871 sprintf(_teseoCmd, "$PSTMCFGLOG,%d,%d,%d,%d,%u,%u,%u",
apalmieri 0:a77f1f1f8318 872 1, //Enable/Disable the log
apalmieri 0:a77f1f1f8318 873 1, //Enable/Disable circular mode
apalmieri 0:a77f1f1f8318 874 1, //Record type
apalmieri 0:a77f1f1f8318 875 0, //Enable/Disable one shot mode
apalmieri 0:a77f1f1f8318 876 5, //time interval in seconds between two consecutive logged records
apalmieri 0:a77f1f1f8318 877 0, //minimum speed threshold
apalmieri 0:a77f1f1f8318 878 0 //distance threshold
apalmieri 0:a77f1f1f8318 879 );
apalmieri 0:a77f1f1f8318 880 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 881
apalmieri 0:a77f1f1f8318 882 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 883 }
apalmieri 0:a77f1f1f8318 884
apalmieri 0:a77f1f1f8318 885 gps_provider_error_t
apalmieri 0:a77f1f1f8318 886 Teseo::configDatalog(GPSDatalog *datalog)
apalmieri 0:a77f1f1f8318 887 {
apalmieri 0:a77f1f1f8318 888 //printf("Teseo::configDatalog 0x%03x\r\n", (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer()));
apalmieri 0:a77f1f1f8318 889 //$PSTMLOGCREATE,<cfg>,<min-rate>,<min-speed>,<min-position>,<logmask>*<checksum><cr><lf>
apalmieri 0:a77f1f1f8318 890 sprintf(_teseoCmd, "$PSTMLOGCREATE,%03x,%u,%u,%u,%d",
apalmieri 0:a77f1f1f8318 891 (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer()),
apalmieri 0:a77f1f1f8318 892 datalog->getMinRate(),
apalmieri 0:a77f1f1f8318 893 datalog->getMinSpeed(),
apalmieri 0:a77f1f1f8318 894 datalog->getMinPosition(),
apalmieri 0:a77f1f1f8318 895 datalog->getLogMask()
apalmieri 0:a77f1f1f8318 896 );
apalmieri 0:a77f1f1f8318 897 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 898
apalmieri 0:a77f1f1f8318 899 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 900 }
apalmieri 0:a77f1f1f8318 901
apalmieri 0:a77f1f1f8318 902 gps_provider_error_t
apalmieri 0:a77f1f1f8318 903 Teseo::startDatalog(void)
apalmieri 0:a77f1f1f8318 904 {
apalmieri 0:a77f1f1f8318 905 sprintf(_teseoCmd, "$PSTMLOGSTART");
apalmieri 0:a77f1f1f8318 906 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 907
apalmieri 0:a77f1f1f8318 908 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 909 }
apalmieri 0:a77f1f1f8318 910
apalmieri 0:a77f1f1f8318 911 gps_provider_error_t
apalmieri 0:a77f1f1f8318 912 Teseo::stopDatalog(void)
apalmieri 0:a77f1f1f8318 913 {
apalmieri 0:a77f1f1f8318 914 sprintf(_teseoCmd, "$PSTMLOGSTOP");
apalmieri 0:a77f1f1f8318 915 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 916
apalmieri 0:a77f1f1f8318 917 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 918 }
apalmieri 0:a77f1f1f8318 919
apalmieri 0:a77f1f1f8318 920 gps_provider_error_t
apalmieri 0:a77f1f1f8318 921 Teseo::eraseDatalog(void)
apalmieri 0:a77f1f1f8318 922 {
apalmieri 0:a77f1f1f8318 923 sprintf(_teseoCmd, "$PSTMLOGERASE");
apalmieri 0:a77f1f1f8318 924 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 925
apalmieri 0:a77f1f1f8318 926 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 927 }
apalmieri 0:a77f1f1f8318 928
apalmieri 0:a77f1f1f8318 929 gps_provider_error_t
apalmieri 0:a77f1f1f8318 930 Teseo::logReqStatus(void)
apalmieri 0:a77f1f1f8318 931 {
apalmieri 0:a77f1f1f8318 932 /* TBI */
apalmieri 0:a77f1f1f8318 933 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 934 }
apalmieri 0:a77f1f1f8318 935
apalmieri 0:a77f1f1f8318 936 gps_provider_error_t
apalmieri 0:a77f1f1f8318 937 Teseo::logReqQuery(GPSProvider::LogQueryParams_t &logReqQuery)
apalmieri 0:a77f1f1f8318 938 {
apalmieri 0:a77f1f1f8318 939 /* TBI */
apalmieri 0:a77f1f1f8318 940 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 941 }
apalmieri 0:a77f1f1f8318 942
apalmieri 0:a77f1f1f8318 943 bool
apalmieri 0:a77f1f1f8318 944 Teseo::isOdometerSupported(void)
apalmieri 0:a77f1f1f8318 945 {
apalmieri 0:a77f1f1f8318 946 return true;
apalmieri 0:a77f1f1f8318 947 }
apalmieri 0:a77f1f1f8318 948
apalmieri 0:a77f1f1f8318 949 gps_provider_error_t
apalmieri 0:a77f1f1f8318 950 Teseo::enableOdo(void)
apalmieri 0:a77f1f1f8318 951 {
apalmieri 0:a77f1f1f8318 952 //$PSTMCFGODO,<en>,<enmsg>,<alarm>*<checksum><cr><lf>
apalmieri 0:a77f1f1f8318 953 sprintf(_teseoCmd, "$PSTMCFGODO,1,1,1");
apalmieri 0:a77f1f1f8318 954 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 955
apalmieri 0:a77f1f1f8318 956 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 957 }
apalmieri 0:a77f1f1f8318 958
apalmieri 0:a77f1f1f8318 959 gps_provider_error_t
apalmieri 0:a77f1f1f8318 960 Teseo::startOdo(unsigned alarmDistance)
apalmieri 0:a77f1f1f8318 961 {
apalmieri 0:a77f1f1f8318 962 sprintf(_teseoCmd, "$PSTMODOSTART,%08x", alarmDistance);
apalmieri 0:a77f1f1f8318 963 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 964
apalmieri 0:a77f1f1f8318 965 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 966 }
apalmieri 0:a77f1f1f8318 967
apalmieri 0:a77f1f1f8318 968 gps_provider_error_t
apalmieri 0:a77f1f1f8318 969 Teseo::stopOdo(void)
apalmieri 0:a77f1f1f8318 970 {
apalmieri 0:a77f1f1f8318 971 sprintf(_teseoCmd, "$PSTMODOSTOP");
apalmieri 0:a77f1f1f8318 972 SendCommand(_teseoCmd);
apalmieri 0:a77f1f1f8318 973
apalmieri 0:a77f1f1f8318 974 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 975 }
apalmieri 0:a77f1f1f8318 976 gps_provider_error_t
apalmieri 0:a77f1f1f8318 977 Teseo::resetOdo(void)
apalmieri 0:a77f1f1f8318 978 {
apalmieri 0:a77f1f1f8318 979 /* TBI */
apalmieri 0:a77f1f1f8318 980 return GPS_ERROR_NONE;
apalmieri 0:a77f1f1f8318 981 }