Harry Keane
/
X_NUCLEO_GNSS1A1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: A_TeseoLocationNEW A_TeseoLocation
Components/Teseo/Teseo.cpp@0:a77f1f1f8318, 2018-11-09 (annotated)
- Committer:
- apalmieri
- Date:
- Fri Nov 09 17:06:11 2018 +0000
- Revision:
- 0:a77f1f1f8318
- Child:
- 3:2d568ec8da14
Initial commit
Who changed what in which revision?
| User | Revision | Line number | New 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>© 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 | 0:a77f1f1f8318 | 145 | if (serialDebug) |
| apalmieri | 0:a77f1f1f8318 | 146 | serialDebug->printf("%s: Resetting...", TESEO_NAME); |
| apalmieri | 0:a77f1f1f8318 | 147 | |
| apalmieri | 0:a77f1f1f8318 | 148 | _reset.write(0); |
| apalmieri | 0:a77f1f1f8318 | 149 | |
| apalmieri | 0:a77f1f1f8318 | 150 | wait_ms(5); |
| apalmieri | 0:a77f1f1f8318 | 151 | |
| apalmieri | 0:a77f1f1f8318 | 152 | _reset.write(1); |
| apalmieri | 0:a77f1f1f8318 | 153 | |
| apalmieri | 0:a77f1f1f8318 | 154 | wait_ms(70); |
| apalmieri | 0:a77f1f1f8318 | 155 | |
| apalmieri | 0:a77f1f1f8318 | 156 | if (serialDebug) |
| apalmieri | 0:a77f1f1f8318 | 157 | serialDebug->printf("Done...\n\r"); |
| apalmieri | 0:a77f1f1f8318 | 158 | } |
| apalmieri | 0:a77f1f1f8318 | 159 | |
| apalmieri | 0:a77f1f1f8318 | 160 | void |
| apalmieri | 0:a77f1f1f8318 | 161 | Teseo::_Reset(Serial *serialDebug) |
| apalmieri | 0:a77f1f1f8318 | 162 | { |
| apalmieri | 0:a77f1f1f8318 | 163 | if (serialDebug) |
| apalmieri | 0:a77f1f1f8318 | 164 | serialDebug->printf("%s: Resetting...", TESEO_NAME); |
| apalmieri | 0:a77f1f1f8318 | 165 | |
| apalmieri | 0:a77f1f1f8318 | 166 | //_pps.output(); |
| apalmieri | 0:a77f1f1f8318 | 167 | //_pps.write(0); |
| apalmieri | 0:a77f1f1f8318 | 168 | |
| apalmieri | 0:a77f1f1f8318 | 169 | //wait_ms(500); |
| apalmieri | 0:a77f1f1f8318 | 170 | |
| apalmieri | 0:a77f1f1f8318 | 171 | _reset.write(1); |
| apalmieri | 0:a77f1f1f8318 | 172 | |
| apalmieri | 0:a77f1f1f8318 | 173 | wait_ms(500); |
| apalmieri | 0:a77f1f1f8318 | 174 | |
| apalmieri | 0:a77f1f1f8318 | 175 | _reset.write(0); |
| apalmieri | 0:a77f1f1f8318 | 176 | //_pps.write(0); |
| apalmieri | 0:a77f1f1f8318 | 177 | |
| apalmieri | 0:a77f1f1f8318 | 178 | wait_ms(500); |
| apalmieri | 0:a77f1f1f8318 | 179 | |
| apalmieri | 0:a77f1f1f8318 | 180 | _reset.write(1); |
| apalmieri | 0:a77f1f1f8318 | 181 | |
| apalmieri | 0:a77f1f1f8318 | 182 | if (serialDebug) |
| apalmieri | 0:a77f1f1f8318 | 183 | serialDebug->printf("Done...\n\r"); |
| apalmieri | 0:a77f1f1f8318 | 184 | |
| apalmieri | 0:a77f1f1f8318 | 185 | //_pps.write(1); |
| apalmieri | 0:a77f1f1f8318 | 186 | |
| apalmieri | 0:a77f1f1f8318 | 187 | //wait_ms(1000); |
| apalmieri | 0:a77f1f1f8318 | 188 | |
| apalmieri | 0:a77f1f1f8318 | 189 | //_pps.input(); |
| apalmieri | 0:a77f1f1f8318 | 190 | } |
| apalmieri | 0:a77f1f1f8318 | 191 | |
| apalmieri | 0:a77f1f1f8318 | 192 | void |
| apalmieri | 0:a77f1f1f8318 | 193 | Teseo::_SendString(char *buf, int len) |
| apalmieri | 0:a77f1f1f8318 | 194 | { |
| apalmieri | 0:a77f1f1f8318 | 195 | for (int i = 0; i < len; ++i) { |
| apalmieri | 0:a77f1f1f8318 | 196 | while (!_uart->writeable()); |
| apalmieri | 0:a77f1f1f8318 | 197 | _uart->putc(buf[i]); |
| apalmieri | 0:a77f1f1f8318 | 198 | } |
| apalmieri | 0:a77f1f1f8318 | 199 | } |
| apalmieri | 0:a77f1f1f8318 | 200 | |
| apalmieri | 0:a77f1f1f8318 | 201 | int |
| apalmieri | 0:a77f1f1f8318 | 202 | Teseo::_WakeUp() |
| apalmieri | 0:a77f1f1f8318 | 203 | { |
| apalmieri | 0:a77f1f1f8318 | 204 | wait_ms(100); |
| apalmieri | 0:a77f1f1f8318 | 205 | |
| apalmieri | 0:a77f1f1f8318 | 206 | _wakeup.write(1); |
| apalmieri | 0:a77f1f1f8318 | 207 | |
| apalmieri | 0:a77f1f1f8318 | 208 | wait_ms(500); |
| apalmieri | 0:a77f1f1f8318 | 209 | |
| apalmieri | 0:a77f1f1f8318 | 210 | _wakeup.write(0); |
| apalmieri | 0:a77f1f1f8318 | 211 | |
| apalmieri | 0:a77f1f1f8318 | 212 | return 0; |
| apalmieri | 0:a77f1f1f8318 | 213 | } |
| apalmieri | 0:a77f1f1f8318 | 214 | |
| apalmieri | 0:a77f1f1f8318 | 215 | int |
| apalmieri | 0:a77f1f1f8318 | 216 | Teseo::_CRC(char *buf, int size) |
| apalmieri | 0:a77f1f1f8318 | 217 | { |
| apalmieri | 0:a77f1f1f8318 | 218 | int i = 0, ch = 0; |
| apalmieri | 0:a77f1f1f8318 | 219 | |
| apalmieri | 0:a77f1f1f8318 | 220 | if (buf[0] == '$') { |
| apalmieri | 0:a77f1f1f8318 | 221 | ++i; |
| apalmieri | 0:a77f1f1f8318 | 222 | } |
| apalmieri | 0:a77f1f1f8318 | 223 | |
| apalmieri | 0:a77f1f1f8318 | 224 | if (size) { |
| apalmieri | 0:a77f1f1f8318 | 225 | for (; i < size; ++i) { |
| apalmieri | 0:a77f1f1f8318 | 226 | ch ^= buf[i]; |
| apalmieri | 0:a77f1f1f8318 | 227 | } |
| apalmieri | 0:a77f1f1f8318 | 228 | } else { |
| apalmieri | 0:a77f1f1f8318 | 229 | for (; buf[i] != 0; ++i) { |
| apalmieri | 0:a77f1f1f8318 | 230 | ch ^= buf[i]; |
| apalmieri | 0:a77f1f1f8318 | 231 | } |
| apalmieri | 0:a77f1f1f8318 | 232 | } |
| apalmieri | 0:a77f1f1f8318 | 233 | |
| apalmieri | 0:a77f1f1f8318 | 234 | return ch; |
| apalmieri | 0:a77f1f1f8318 | 235 | } |
| apalmieri | 0:a77f1f1f8318 | 236 | |
| apalmieri | 0:a77f1f1f8318 | 237 | void |
| apalmieri | 0:a77f1f1f8318 | 238 | Teseo::SendCommand(Teseo::eCmd c) |
| apalmieri | 0:a77f1f1f8318 | 239 | { |
| apalmieri | 0:a77f1f1f8318 | 240 | char crc[3]; |
| apalmieri | 0:a77f1f1f8318 | 241 | |
| apalmieri | 0:a77f1f1f8318 | 242 | sprintf(crc, "*%02X", _CRC(teseoCmds[c].cmd, -1)); |
| apalmieri | 0:a77f1f1f8318 | 243 | |
| apalmieri | 0:a77f1f1f8318 | 244 | _SendString(teseoCmds[c].cmd, strlen(teseoCmds[c].cmd)); |
| apalmieri | 0:a77f1f1f8318 | 245 | _SendString(crc, 3); |
| apalmieri | 0:a77f1f1f8318 | 246 | } |
| apalmieri | 0:a77f1f1f8318 | 247 | |
| apalmieri | 0:a77f1f1f8318 | 248 | void |
| apalmieri | 0:a77f1f1f8318 | 249 | Teseo::SendCommand(char *cmd) |
| apalmieri | 0:a77f1f1f8318 | 250 | { |
| apalmieri | 0:a77f1f1f8318 | 251 | char crc[5]; |
| apalmieri | 0:a77f1f1f8318 | 252 | |
| apalmieri | 0:a77f1f1f8318 | 253 | sprintf(crc, "*%02X\n\r", _CRC(cmd, strlen(cmd))); |
| apalmieri | 0:a77f1f1f8318 | 254 | //printf("CRC=%s\n\r", crc); |
| apalmieri | 0:a77f1f1f8318 | 255 | |
| apalmieri | 0:a77f1f1f8318 | 256 | _SendString(cmd, strlen(cmd)); |
| apalmieri | 0:a77f1f1f8318 | 257 | _SendString(crc, 5); |
| apalmieri | 0:a77f1f1f8318 | 258 | } |
| apalmieri | 0:a77f1f1f8318 | 259 | |
| apalmieri | 0:a77f1f1f8318 | 260 | /** TBC */ |
| apalmieri | 0:a77f1f1f8318 | 261 | int |
| apalmieri | 0:a77f1f1f8318 | 262 | Teseo::_CheckI2C() |
| apalmieri | 0:a77f1f1f8318 | 263 | { |
| apalmieri | 0:a77f1f1f8318 | 264 | if (!_i2c) |
| apalmieri | 0:a77f1f1f8318 | 265 | return -1; |
| apalmieri | 0:a77f1f1f8318 | 266 | |
| apalmieri | 0:a77f1f1f8318 | 267 | _i2c->start(); |
| apalmieri | 0:a77f1f1f8318 | 268 | int res = _i2c->write((TESEO_I2C_ADDRESS << 1) | 1); |
| apalmieri | 0:a77f1f1f8318 | 269 | _i2c->stop(); |
| apalmieri | 0:a77f1f1f8318 | 270 | /* |
| apalmieri | 0:a77f1f1f8318 | 271 | * @returns |
| apalmieri | 0:a77f1f1f8318 | 272 | * '0' - NAK was received |
| apalmieri | 0:a77f1f1f8318 | 273 | * '1' - ACK was received, |
| apalmieri | 0:a77f1f1f8318 | 274 | * '2' - timeout |
| apalmieri | 0:a77f1f1f8318 | 275 | */ |
| apalmieri | 0:a77f1f1f8318 | 276 | return res == 1 ? 0 : -1; |
| apalmieri | 0:a77f1f1f8318 | 277 | } |
| apalmieri | 0:a77f1f1f8318 | 278 | |
| apalmieri | 0:a77f1f1f8318 | 279 | void |
| apalmieri | 0:a77f1f1f8318 | 280 | Teseo::ReadSentence(Teseo::eMsg msg) |
| apalmieri | 0:a77f1f1f8318 | 281 | { |
| apalmieri | 0:a77f1f1f8318 | 282 | eStatus status = TESEO_STATUS_FAILURE; |
| apalmieri | 0:a77f1f1f8318 | 283 | |
| apalmieri | 0:a77f1f1f8318 | 284 | do { |
| apalmieri | 0:a77f1f1f8318 | 285 | osEvent evt = queue.get(); |
| apalmieri | 0:a77f1f1f8318 | 286 | if (evt.status == osEventMessage) { |
| apalmieri | 0:a77f1f1f8318 | 287 | struct _teseoMsg *message = (struct _teseoMsg *)evt.value.p; |
| apalmieri | 0:a77f1f1f8318 | 288 | if (message->len > 0) { |
| apalmieri | 0:a77f1f1f8318 | 289 | status = _GetMsg(msg, message->buf); |
| apalmieri | 0:a77f1f1f8318 | 290 | } |
| apalmieri | 0:a77f1f1f8318 | 291 | |
| apalmieri | 0:a77f1f1f8318 | 292 | mpool.free(message); |
| apalmieri | 0:a77f1f1f8318 | 293 | } |
| apalmieri | 0:a77f1f1f8318 | 294 | } while (status != TESEO_STATUS_SUCCESS); |
| apalmieri | 0:a77f1f1f8318 | 295 | } |
| apalmieri | 0:a77f1f1f8318 | 296 | |
| apalmieri | 0:a77f1f1f8318 | 297 | void |
| apalmieri | 0:a77f1f1f8318 | 298 | Teseo::_InitUART(int br) |
| apalmieri | 0:a77f1f1f8318 | 299 | { |
| apalmieri | 0:a77f1f1f8318 | 300 | _uart = new (std::nothrow) Serial(_uartRx, _uartTx); |
| apalmieri | 0:a77f1f1f8318 | 301 | if(_uart != NULL) { |
| apalmieri | 0:a77f1f1f8318 | 302 | _uart->format(8, SerialBase::None, 1); |
| apalmieri | 0:a77f1f1f8318 | 303 | _uart->baud(br); |
| apalmieri | 0:a77f1f1f8318 | 304 | } else { |
| apalmieri | 0:a77f1f1f8318 | 305 | TESEO_LOG_INFO("Error allocating UART.\r\n"); |
| apalmieri | 0:a77f1f1f8318 | 306 | } |
| apalmieri | 0:a77f1f1f8318 | 307 | } |
| apalmieri | 0:a77f1f1f8318 | 308 | |
| apalmieri | 0:a77f1f1f8318 | 309 | bool |
| apalmieri | 0:a77f1f1f8318 | 310 | Teseo::setPowerMode(GPSProvider::PowerMode_t pwrMode) |
| apalmieri | 0:a77f1f1f8318 | 311 | { |
| apalmieri | 0:a77f1f1f8318 | 312 | /* TBI */ |
| apalmieri | 0:a77f1f1f8318 | 313 | return false; |
| apalmieri | 0:a77f1f1f8318 | 314 | } |
| apalmieri | 0:a77f1f1f8318 | 315 | |
| apalmieri | 0:a77f1f1f8318 | 316 | void |
| apalmieri | 0:a77f1f1f8318 | 317 | Teseo::eventHandler(eTeseoLocEventType event, uint32_t data) |
| apalmieri | 0:a77f1f1f8318 | 318 | { |
| apalmieri | 0:a77f1f1f8318 | 319 | if (appEventCb) { |
| apalmieri | 0:a77f1f1f8318 | 320 | appEventCb(event, data); |
| apalmieri | 0:a77f1f1f8318 | 321 | } |
| apalmieri | 0:a77f1f1f8318 | 322 | } |
| apalmieri | 0:a77f1f1f8318 | 323 | |
| apalmieri | 0:a77f1f1f8318 | 324 | void |
| apalmieri | 0:a77f1f1f8318 | 325 | Teseo::ReadProcess(void) |
| apalmieri | 0:a77f1f1f8318 | 326 | { |
| apalmieri | 0:a77f1f1f8318 | 327 | char c; |
| apalmieri | 0:a77f1f1f8318 | 328 | |
| apalmieri | 0:a77f1f1f8318 | 329 | struct _teseoMsg *msg = mpool.alloc(); |
| apalmieri | 0:a77f1f1f8318 | 330 | msg->len = 0; |
| apalmieri | 0:a77f1f1f8318 | 331 | |
| apalmieri | 0:a77f1f1f8318 | 332 | while(true) { |
| apalmieri | 0:a77f1f1f8318 | 333 | if (_uart->readable()) { |
| apalmieri | 0:a77f1f1f8318 | 334 | c = _uart->getc(); |
| apalmieri | 0:a77f1f1f8318 | 335 | |
| apalmieri | 0:a77f1f1f8318 | 336 | if (c == '$') { |
| apalmieri | 0:a77f1f1f8318 | 337 | queue.put(msg); |
| apalmieri | 0:a77f1f1f8318 | 338 | msg = mpool.alloc(); |
| apalmieri | 0:a77f1f1f8318 | 339 | msg->len = 0; |
| apalmieri | 0:a77f1f1f8318 | 340 | } |
| apalmieri | 0:a77f1f1f8318 | 341 | msg->buf[msg->len++] = c; |
| apalmieri | 0:a77f1f1f8318 | 342 | |
| apalmieri | 0:a77f1f1f8318 | 343 | } else { |
| apalmieri | 0:a77f1f1f8318 | 344 | Thread::yield(); //wait_us(100); Allow other threads to run |
| apalmieri | 0:a77f1f1f8318 | 345 | } |
| apalmieri | 0:a77f1f1f8318 | 346 | } |
| apalmieri | 0:a77f1f1f8318 | 347 | } |
| apalmieri | 0:a77f1f1f8318 | 348 | |
| apalmieri | 0:a77f1f1f8318 | 349 | static void |
| apalmieri | 0:a77f1f1f8318 | 350 | _UARTStreamProcess(Teseo *gnss) |
| apalmieri | 0:a77f1f1f8318 | 351 | { |
| apalmieri | 0:a77f1f1f8318 | 352 | gnss->ReadProcess(); |
| apalmieri | 0:a77f1f1f8318 | 353 | } |
| apalmieri | 0:a77f1f1f8318 | 354 | |
| apalmieri | 0:a77f1f1f8318 | 355 | void |
| apalmieri | 0:a77f1f1f8318 | 356 | Teseo::start(void) |
| apalmieri | 0:a77f1f1f8318 | 357 | { |
| apalmieri | 0:a77f1f1f8318 | 358 | if(_locState == TESEO_LOC_STATE_IDLE) { |
| apalmieri | 0:a77f1f1f8318 | 359 | |
| apalmieri | 0:a77f1f1f8318 | 360 | _InitUART(); |
| apalmieri | 0:a77f1f1f8318 | 361 | _locState = TESEO_LOC_STATE_RUN; |
| apalmieri | 0:a77f1f1f8318 | 362 | |
| apalmieri | 0:a77f1f1f8318 | 363 | // Start thread for UART listener and set the highest priority |
| apalmieri | 0:a77f1f1f8318 | 364 | serialStreamThread = new (std::nothrow) Thread(); |
| apalmieri | 0:a77f1f1f8318 | 365 | if(serialStreamThread != NULL) { |
| apalmieri | 0:a77f1f1f8318 | 366 | serialStreamThread->set_priority(osPriorityRealtime); |
| apalmieri | 0:a77f1f1f8318 | 367 | serialStreamThread->start(callback(_UARTStreamProcess, this)); |
| apalmieri | 0:a77f1f1f8318 | 368 | } else { |
| apalmieri | 0:a77f1f1f8318 | 369 | TESEO_LOG_INFO("Error allocating serialStreamThread\r\n"); |
| apalmieri | 0:a77f1f1f8318 | 370 | } |
| apalmieri | 0:a77f1f1f8318 | 371 | |
| apalmieri | 0:a77f1f1f8318 | 372 | eventHandler(TESEO_LOC_EVENT_START_RESULT, 0); |
| apalmieri | 0:a77f1f1f8318 | 373 | } else { |
| apalmieri | 0:a77f1f1f8318 | 374 | TESEO_LOG_INFO("Already started\r\n"); |
| apalmieri | 0:a77f1f1f8318 | 375 | } |
| apalmieri | 0:a77f1f1f8318 | 376 | } |
| apalmieri | 0:a77f1f1f8318 | 377 | |
| apalmieri | 0:a77f1f1f8318 | 378 | void |
| apalmieri | 0:a77f1f1f8318 | 379 | Teseo::stop(void) |
| apalmieri | 0:a77f1f1f8318 | 380 | { |
| apalmieri | 0:a77f1f1f8318 | 381 | if(_locState == TESEO_LOC_STATE_IDLE) { |
| apalmieri | 0:a77f1f1f8318 | 382 | return; |
| apalmieri | 0:a77f1f1f8318 | 383 | } |
| apalmieri | 0:a77f1f1f8318 | 384 | // Stop thread for UART listener |
| apalmieri | 0:a77f1f1f8318 | 385 | if(serialStreamThread != NULL) { |
| apalmieri | 0:a77f1f1f8318 | 386 | serialStreamThread->terminate(); |
| apalmieri | 0:a77f1f1f8318 | 387 | delete serialStreamThread; |
| apalmieri | 0:a77f1f1f8318 | 388 | } |
| apalmieri | 0:a77f1f1f8318 | 389 | |
| apalmieri | 0:a77f1f1f8318 | 390 | if(_uart != NULL) { |
| apalmieri | 0:a77f1f1f8318 | 391 | delete _uart; |
| apalmieri | 0:a77f1f1f8318 | 392 | _uart = NULL; |
| apalmieri | 0:a77f1f1f8318 | 393 | } |
| apalmieri | 0:a77f1f1f8318 | 394 | |
| apalmieri | 0:a77f1f1f8318 | 395 | _locState = TESEO_LOC_STATE_IDLE; |
| apalmieri | 0:a77f1f1f8318 | 396 | eventHandler(TESEO_LOC_EVENT_STOP_RESULT, 0); |
| apalmieri | 0:a77f1f1f8318 | 397 | } |
| apalmieri | 0:a77f1f1f8318 | 398 | |
| apalmieri | 0:a77f1f1f8318 | 399 | void |
| apalmieri | 0:a77f1f1f8318 | 400 | Teseo::outputHandler(uint32_t msgId, uint32_t msgType, tTeseoData *pData) |
| apalmieri | 0:a77f1f1f8318 | 401 | { |
| apalmieri | 0:a77f1f1f8318 | 402 | switch(msgId) { |
| apalmieri | 0:a77f1f1f8318 | 403 | case LOC_OUTPUT_LOCATION: |
| apalmieri | 0:a77f1f1f8318 | 404 | |
| apalmieri | 0:a77f1f1f8318 | 405 | if(pData->gpgga_data.valid == VALID) { |
| apalmieri | 0:a77f1f1f8318 | 406 | lastLocation.valid = true; |
| apalmieri | 0:a77f1f1f8318 | 407 | lastLocation.lat = pData->gpgga_data.xyz.lat; |
| apalmieri | 0:a77f1f1f8318 | 408 | lastLocation.lon = pData->gpgga_data.xyz.lon; |
| apalmieri | 0:a77f1f1f8318 | 409 | lastLocation.altitude = pData->gpgga_data.xyz.alt; |
| apalmieri | 0:a77f1f1f8318 | 410 | lastLocation.numGPSSVs = pData->gpgga_data.sats; |
| apalmieri | 0:a77f1f1f8318 | 411 | lastLocation.utcTime = pData->gpgga_data.utc.utc; |
| apalmieri | 0:a77f1f1f8318 | 412 | } else { |
| apalmieri | 0:a77f1f1f8318 | 413 | lastLocation.valid = false; |
| apalmieri | 0:a77f1f1f8318 | 414 | } |
| apalmieri | 0:a77f1f1f8318 | 415 | |
| apalmieri | 0:a77f1f1f8318 | 416 | if (locationCallback) { |
| apalmieri | 0:a77f1f1f8318 | 417 | locationCallback(&lastLocation); |
| apalmieri | 0:a77f1f1f8318 | 418 | } |
| apalmieri | 0:a77f1f1f8318 | 419 | |
| apalmieri | 0:a77f1f1f8318 | 420 | break; |
| apalmieri | 0:a77f1f1f8318 | 421 | |
| apalmieri | 0:a77f1f1f8318 | 422 | case LOC_OUTPUT_PSTM: { |
| apalmieri | 0:a77f1f1f8318 | 423 | Teseo::ePSTMsg msg = (Teseo::ePSTMsg)msgType; |
| apalmieri | 0:a77f1f1f8318 | 424 | int code; |
| apalmieri | 0:a77f1f1f8318 | 425 | |
| apalmieri | 0:a77f1f1f8318 | 426 | switch(msg) { |
| apalmieri | 0:a77f1f1f8318 | 427 | case PSTMGEOFENCE: { |
| apalmieri | 0:a77f1f1f8318 | 428 | |
| apalmieri | 0:a77f1f1f8318 | 429 | /* Geofence enabling */ |
| apalmieri | 0:a77f1f1f8318 | 430 | if(pData->geofence_data.op == GNSS_FEATURE_EN_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 431 | code = pData->geofence_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 432 | GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 433 | |
| apalmieri | 0:a77f1f1f8318 | 434 | if (code == GPS_ERROR_NONE) { |
| apalmieri | 0:a77f1f1f8318 | 435 | saveConfigParams(); |
| apalmieri | 0:a77f1f1f8318 | 436 | } |
| apalmieri | 0:a77f1f1f8318 | 437 | } |
| apalmieri | 0:a77f1f1f8318 | 438 | |
| apalmieri | 0:a77f1f1f8318 | 439 | /* Geofence configuration */ |
| apalmieri | 0:a77f1f1f8318 | 440 | if(pData->geofence_data.op == GNSS_GEOFENCE_CFG_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 441 | code = pData->geofence_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 442 | GPS_ERROR_GEOFENCE_CFG : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 443 | if (geofenceCfgMessageCallback) { |
| apalmieri | 0:a77f1f1f8318 | 444 | geofenceCfgMessageCallback(code); |
| apalmieri | 0:a77f1f1f8318 | 445 | } |
| apalmieri | 0:a77f1f1f8318 | 446 | } |
| apalmieri | 0:a77f1f1f8318 | 447 | |
| apalmieri | 0:a77f1f1f8318 | 448 | /* Geofence Status */ |
| apalmieri | 0:a77f1f1f8318 | 449 | if(pData->geofence_data.op == GNSS_GEOFENCE_STATUS_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 450 | code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 451 | if(code == GPS_ERROR_NONE) { |
| apalmieri | 0:a77f1f1f8318 | 452 | geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh; |
| apalmieri | 0:a77f1f1f8318 | 453 | geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm; |
| apalmieri | 0:a77f1f1f8318 | 454 | geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss; |
| apalmieri | 0:a77f1f1f8318 | 455 | geofenceStatus.timestamp.day = pData->geofence_data.timestamp.day; |
| apalmieri | 0:a77f1f1f8318 | 456 | geofenceStatus.timestamp.month = pData->geofence_data.timestamp.month; |
| apalmieri | 0:a77f1f1f8318 | 457 | geofenceStatus.timestamp.year = pData->geofence_data.timestamp.year; |
| apalmieri | 0:a77f1f1f8318 | 458 | geofenceStatus.currentStatus = pData->geofence_data.status; |
| apalmieri | 0:a77f1f1f8318 | 459 | geofenceStatus.numGeofences = MAX_GEOFENCES_NUM; |
| 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 | /* Geofence Alarm */ |
| apalmieri | 0:a77f1f1f8318 | 467 | if(pData->geofence_data.op == GNSS_GEOFENCE_ALARM_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 468 | code = pData->geofence_data.result ? GPS_ERROR_GEOFENCE_STATUS : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 469 | if(code == GPS_ERROR_NONE) { |
| apalmieri | 0:a77f1f1f8318 | 470 | geofenceStatus.timestamp.hh = pData->geofence_data.timestamp.hh; |
| apalmieri | 0:a77f1f1f8318 | 471 | geofenceStatus.timestamp.mm = pData->geofence_data.timestamp.mm; |
| apalmieri | 0:a77f1f1f8318 | 472 | geofenceStatus.timestamp.ss = pData->geofence_data.timestamp.ss; |
| apalmieri | 0:a77f1f1f8318 | 473 | geofenceStatus.currentStatus = pData->geofence_data.status; |
| apalmieri | 0:a77f1f1f8318 | 474 | geofenceStatus.idAlarm = pData->geofence_data.idAlarm; |
| apalmieri | 0:a77f1f1f8318 | 475 | } |
| apalmieri | 0:a77f1f1f8318 | 476 | if (geofenceStatusMessageCallback) { |
| apalmieri | 0:a77f1f1f8318 | 477 | geofenceStatusMessageCallback(&geofenceStatus, code); |
| apalmieri | 0:a77f1f1f8318 | 478 | } |
| apalmieri | 0:a77f1f1f8318 | 479 | } |
| apalmieri | 0:a77f1f1f8318 | 480 | } |
| apalmieri | 0:a77f1f1f8318 | 481 | break; |
| apalmieri | 0:a77f1f1f8318 | 482 | |
| apalmieri | 0:a77f1f1f8318 | 483 | case PSTMODO: { |
| apalmieri | 0:a77f1f1f8318 | 484 | /* Odometer enabling */ |
| apalmieri | 0:a77f1f1f8318 | 485 | if(pData->odo_data.op == GNSS_FEATURE_EN_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 486 | |
| apalmieri | 0:a77f1f1f8318 | 487 | code = pData->odo_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 488 | GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 489 | |
| apalmieri | 0:a77f1f1f8318 | 490 | if (code == GPS_ERROR_NONE) { |
| apalmieri | 0:a77f1f1f8318 | 491 | saveConfigParams(); |
| apalmieri | 0:a77f1f1f8318 | 492 | } |
| apalmieri | 0:a77f1f1f8318 | 493 | } |
| apalmieri | 0:a77f1f1f8318 | 494 | |
| apalmieri | 0:a77f1f1f8318 | 495 | /* Odometer start */ |
| apalmieri | 0:a77f1f1f8318 | 496 | if(pData->odo_data.op == GNSS_ODO_START_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 497 | code = pData->odo_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 498 | GPS_ERROR_ODO_START : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 499 | } |
| apalmieri | 0:a77f1f1f8318 | 500 | /* Odometer stop */ |
| apalmieri | 0:a77f1f1f8318 | 501 | if(pData->odo_data.op == GNSS_ODO_STOP_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 502 | code = pData->odo_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 503 | GPS_ERROR_ODO_STOP : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 504 | } |
| apalmieri | 0:a77f1f1f8318 | 505 | } |
| apalmieri | 0:a77f1f1f8318 | 506 | break; |
| apalmieri | 0:a77f1f1f8318 | 507 | |
| apalmieri | 0:a77f1f1f8318 | 508 | case PSTMDATALOG: { |
| apalmieri | 0:a77f1f1f8318 | 509 | /* Datalog enabling */ |
| apalmieri | 0:a77f1f1f8318 | 510 | if(pData->datalog_data.op == GNSS_FEATURE_EN_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 511 | |
| apalmieri | 0:a77f1f1f8318 | 512 | code = pData->datalog_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 513 | GPS_ERROR_FEATURE_ENABLING : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 514 | |
| apalmieri | 0:a77f1f1f8318 | 515 | if (code == GPS_ERROR_NONE) { |
| apalmieri | 0:a77f1f1f8318 | 516 | saveConfigParams(); |
| apalmieri | 0:a77f1f1f8318 | 517 | } |
| apalmieri | 0:a77f1f1f8318 | 518 | } |
| apalmieri | 0:a77f1f1f8318 | 519 | /* Datalog create */ |
| apalmieri | 0:a77f1f1f8318 | 520 | if(pData->datalog_data.op == GNSS_DATALOG_CFG_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 521 | code = pData->datalog_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 522 | GPS_ERROR_DATALOG_CFG : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 523 | } |
| apalmieri | 0:a77f1f1f8318 | 524 | /* Datalog start */ |
| apalmieri | 0:a77f1f1f8318 | 525 | if(pData->datalog_data.op == GNSS_DATALOG_START_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 526 | code = pData->datalog_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 527 | GPS_ERROR_DATALOG_START : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 528 | } |
| apalmieri | 0:a77f1f1f8318 | 529 | /* Datalog stop */ |
| apalmieri | 0:a77f1f1f8318 | 530 | if(pData->datalog_data.op == GNSS_DATALOG_STOP_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 531 | code = pData->datalog_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 532 | GPS_ERROR_DATALOG_STOP : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 533 | } |
| apalmieri | 0:a77f1f1f8318 | 534 | /* Datalog erase */ |
| apalmieri | 0:a77f1f1f8318 | 535 | if(pData->datalog_data.op == GNSS_DATALOG_ERASE_MSG) { |
| apalmieri | 0:a77f1f1f8318 | 536 | code = pData->datalog_data.result ? |
| apalmieri | 0:a77f1f1f8318 | 537 | GPS_ERROR_DATALOG_ERASE : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 538 | } |
| apalmieri | 0:a77f1f1f8318 | 539 | } |
| apalmieri | 0:a77f1f1f8318 | 540 | break; |
| apalmieri | 0:a77f1f1f8318 | 541 | |
| apalmieri | 0:a77f1f1f8318 | 542 | case PSTMSGL: { |
| apalmieri | 0:a77f1f1f8318 | 543 | /* Msg List cfg */ |
| apalmieri | 0:a77f1f1f8318 | 544 | code = pData->ack ? |
| apalmieri | 0:a77f1f1f8318 | 545 | GPS_ERROR_MSGLIST_CFG : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 546 | |
| apalmieri | 0:a77f1f1f8318 | 547 | if (code == GPS_ERROR_NONE) { |
| apalmieri | 0:a77f1f1f8318 | 548 | saveConfigParams(); |
| apalmieri | 0:a77f1f1f8318 | 549 | } |
| apalmieri | 0:a77f1f1f8318 | 550 | } |
| apalmieri | 0:a77f1f1f8318 | 551 | break; |
| apalmieri | 0:a77f1f1f8318 | 552 | |
| apalmieri | 0:a77f1f1f8318 | 553 | case PSTMSAVEPAR: { |
| apalmieri | 0:a77f1f1f8318 | 554 | code = pData->ack ? |
| apalmieri | 0:a77f1f1f8318 | 555 | GPS_ERROR_SAVEPAR : GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 556 | |
| apalmieri | 0:a77f1f1f8318 | 557 | if (code == GPS_ERROR_NONE) { |
| apalmieri | 0:a77f1f1f8318 | 558 | reset(); |
| apalmieri | 0:a77f1f1f8318 | 559 | } |
| apalmieri | 0:a77f1f1f8318 | 560 | } |
| apalmieri | 0:a77f1f1f8318 | 561 | break; |
| apalmieri | 0:a77f1f1f8318 | 562 | |
| apalmieri | 0:a77f1f1f8318 | 563 | } /* end switch */ |
| apalmieri | 0:a77f1f1f8318 | 564 | } /* end case LOC_OUTPUT_PSTM */ |
| apalmieri | 0:a77f1f1f8318 | 565 | |
| apalmieri | 0:a77f1f1f8318 | 566 | break; |
| apalmieri | 0:a77f1f1f8318 | 567 | |
| apalmieri | 0:a77f1f1f8318 | 568 | default: |
| apalmieri | 0:a77f1f1f8318 | 569 | break; |
| apalmieri | 0:a77f1f1f8318 | 570 | } |
| apalmieri | 0:a77f1f1f8318 | 571 | |
| apalmieri | 0:a77f1f1f8318 | 572 | if (appOutCb) { |
| apalmieri | 0:a77f1f1f8318 | 573 | appOutCb(msgId, msgType, pData); |
| apalmieri | 0:a77f1f1f8318 | 574 | } |
| apalmieri | 0:a77f1f1f8318 | 575 | |
| apalmieri | 0:a77f1f1f8318 | 576 | } |
| apalmieri | 0:a77f1f1f8318 | 577 | |
| apalmieri | 0:a77f1f1f8318 | 578 | |
| apalmieri | 0:a77f1f1f8318 | 579 | eStatus |
| apalmieri | 0:a77f1f1f8318 | 580 | Teseo::_GetMsg(Teseo::eMsg msg, uint8_t *buffer) |
| apalmieri | 0:a77f1f1f8318 | 581 | { |
| apalmieri | 0:a77f1f1f8318 | 582 | eStatus status = TESEO_STATUS_FAILURE; |
| apalmieri | 0:a77f1f1f8318 | 583 | |
| apalmieri | 0:a77f1f1f8318 | 584 | switch(msg) { |
| apalmieri | 0:a77f1f1f8318 | 585 | |
| apalmieri | 0:a77f1f1f8318 | 586 | case GPGGA: |
| apalmieri | 0:a77f1f1f8318 | 587 | status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 588 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 589 | outputHandler(LOC_OUTPUT_LOCATION, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 590 | } |
| apalmieri | 0:a77f1f1f8318 | 591 | break; |
| apalmieri | 0:a77f1f1f8318 | 592 | |
| apalmieri | 0:a77f1f1f8318 | 593 | case GNS: |
| apalmieri | 0:a77f1f1f8318 | 594 | status = (eStatus)parse_gnsmsg(&pData.gns_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 595 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 596 | outputHandler(LOC_OUTPUT_NMEA, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 597 | } |
| apalmieri | 0:a77f1f1f8318 | 598 | break; |
| apalmieri | 0:a77f1f1f8318 | 599 | |
| apalmieri | 0:a77f1f1f8318 | 600 | case GPGST: |
| apalmieri | 0:a77f1f1f8318 | 601 | status = (eStatus)parse_gpgst(&pData.gpgst_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 602 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 603 | outputHandler(LOC_OUTPUT_NMEA, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 604 | } |
| apalmieri | 0:a77f1f1f8318 | 605 | break; |
| apalmieri | 0:a77f1f1f8318 | 606 | |
| apalmieri | 0:a77f1f1f8318 | 607 | case GPRMC: |
| apalmieri | 0:a77f1f1f8318 | 608 | status = (eStatus)parse_gprmc(&pData.gprmc_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 609 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 610 | outputHandler(LOC_OUTPUT_NMEA, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 611 | } |
| apalmieri | 0:a77f1f1f8318 | 612 | break; |
| apalmieri | 0:a77f1f1f8318 | 613 | |
| apalmieri | 0:a77f1f1f8318 | 614 | case GSA: |
| apalmieri | 0:a77f1f1f8318 | 615 | status = (eStatus)parse_gsamsg(&pData.gsa_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 616 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 617 | outputHandler(LOC_OUTPUT_NMEA, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 618 | } |
| apalmieri | 0:a77f1f1f8318 | 619 | break; |
| apalmieri | 0:a77f1f1f8318 | 620 | |
| apalmieri | 0:a77f1f1f8318 | 621 | case GSV: |
| apalmieri | 0:a77f1f1f8318 | 622 | status = (eStatus)parse_gsvmsg(&pData.gsv_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 623 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 624 | outputHandler(LOC_OUTPUT_NMEA, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 625 | } |
| apalmieri | 0:a77f1f1f8318 | 626 | break; |
| apalmieri | 0:a77f1f1f8318 | 627 | |
| apalmieri | 0:a77f1f1f8318 | 628 | default: |
| apalmieri | 0:a77f1f1f8318 | 629 | break; |
| apalmieri | 0:a77f1f1f8318 | 630 | |
| apalmieri | 0:a77f1f1f8318 | 631 | } |
| apalmieri | 0:a77f1f1f8318 | 632 | |
| apalmieri | 0:a77f1f1f8318 | 633 | return status; |
| apalmieri | 0:a77f1f1f8318 | 634 | } |
| apalmieri | 0:a77f1f1f8318 | 635 | |
| apalmieri | 0:a77f1f1f8318 | 636 | void |
| apalmieri | 0:a77f1f1f8318 | 637 | Teseo::setVerboseMode(int level) |
| apalmieri | 0:a77f1f1f8318 | 638 | { |
| apalmieri | 0:a77f1f1f8318 | 639 | cfgMessageList(level); |
| apalmieri | 0:a77f1f1f8318 | 640 | } |
| apalmieri | 0:a77f1f1f8318 | 641 | |
| apalmieri | 0:a77f1f1f8318 | 642 | void |
| apalmieri | 0:a77f1f1f8318 | 643 | Teseo::_GetLocationMsg(Teseo::eMsg msg, uint8_t *buffer) |
| apalmieri | 0:a77f1f1f8318 | 644 | { |
| apalmieri | 0:a77f1f1f8318 | 645 | eStatus status; |
| apalmieri | 0:a77f1f1f8318 | 646 | |
| apalmieri | 0:a77f1f1f8318 | 647 | #if 0 |
| apalmieri | 0:a77f1f1f8318 | 648 | _serialDebug->printf("\n\r --------------------->\n\r"); |
| apalmieri | 0:a77f1f1f8318 | 649 | for (int i = 0; i < TESEO_RXBUF_LEN ; ++i) { |
| apalmieri | 0:a77f1f1f8318 | 650 | _serialDebug->putc((int)buffer[i]); |
| apalmieri | 0:a77f1f1f8318 | 651 | } |
| apalmieri | 0:a77f1f1f8318 | 652 | _serialDebug->printf("\n\r<---------------------\n\r"); |
| apalmieri | 0:a77f1f1f8318 | 653 | #endif |
| apalmieri | 0:a77f1f1f8318 | 654 | |
| apalmieri | 0:a77f1f1f8318 | 655 | status = (eStatus)parse_gpgga(&pData.gpgga_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 656 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 657 | outputHandler(LOC_OUTPUT_LOCATION, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 658 | } |
| apalmieri | 0:a77f1f1f8318 | 659 | } |
| apalmieri | 0:a77f1f1f8318 | 660 | |
| apalmieri | 0:a77f1f1f8318 | 661 | void |
| apalmieri | 0:a77f1f1f8318 | 662 | Teseo::_GetPSTMsg(Teseo::ePSTMsg msg, uint8_t *buffer) |
| apalmieri | 0:a77f1f1f8318 | 663 | { |
| apalmieri | 0:a77f1f1f8318 | 664 | eStatus status; |
| apalmieri | 0:a77f1f1f8318 | 665 | |
| apalmieri | 0:a77f1f1f8318 | 666 | #if 0 |
| apalmieri | 0:a77f1f1f8318 | 667 | _serialDebug->printf("\n\r --------------------->\n\r"); |
| apalmieri | 0:a77f1f1f8318 | 668 | for (int i = 0; i < TESEO_RXBUF_LEN ; ++i) { |
| apalmieri | 0:a77f1f1f8318 | 669 | _serialDebug->putc((int)buffer[i]); |
| apalmieri | 0:a77f1f1f8318 | 670 | } |
| apalmieri | 0:a77f1f1f8318 | 671 | _serialDebug->printf("\n\r<---------------------\n\r"); |
| apalmieri | 0:a77f1f1f8318 | 672 | #endif |
| apalmieri | 0:a77f1f1f8318 | 673 | |
| apalmieri | 0:a77f1f1f8318 | 674 | switch(msg) { |
| apalmieri | 0:a77f1f1f8318 | 675 | |
| apalmieri | 0:a77f1f1f8318 | 676 | case PSTMGEOFENCE: |
| apalmieri | 0:a77f1f1f8318 | 677 | status = (eStatus)parse_pstmgeofence(&pData.geofence_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 678 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 679 | outputHandler(LOC_OUTPUT_PSTM, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 680 | } |
| apalmieri | 0:a77f1f1f8318 | 681 | break; |
| apalmieri | 0:a77f1f1f8318 | 682 | case PSTMODO: |
| apalmieri | 0:a77f1f1f8318 | 683 | status = (eStatus)parse_pstmodo(&pData.odo_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 684 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 685 | outputHandler(LOC_OUTPUT_PSTM, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 686 | } |
| apalmieri | 0:a77f1f1f8318 | 687 | break; |
| apalmieri | 0:a77f1f1f8318 | 688 | case PSTMDATALOG: |
| apalmieri | 0:a77f1f1f8318 | 689 | status = (eStatus)parse_pstmdatalog(&pData.datalog_data, buffer); |
| apalmieri | 0:a77f1f1f8318 | 690 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 691 | outputHandler(LOC_OUTPUT_PSTM, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 692 | } |
| apalmieri | 0:a77f1f1f8318 | 693 | break; |
| apalmieri | 0:a77f1f1f8318 | 694 | case PSTMSGL: |
| apalmieri | 0:a77f1f1f8318 | 695 | status = (eStatus)parse_pstmsgl(&pData.ack, buffer); |
| apalmieri | 0:a77f1f1f8318 | 696 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 697 | outputHandler(LOC_OUTPUT_PSTM, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 698 | } |
| apalmieri | 0:a77f1f1f8318 | 699 | break; |
| apalmieri | 0:a77f1f1f8318 | 700 | case PSTMSAVEPAR: |
| apalmieri | 0:a77f1f1f8318 | 701 | status = (eStatus)parse_pstmsavepar(&pData.ack, buffer); |
| apalmieri | 0:a77f1f1f8318 | 702 | if(status == TESEO_STATUS_SUCCESS) { |
| apalmieri | 0:a77f1f1f8318 | 703 | outputHandler(LOC_OUTPUT_PSTM, msg, &pData); |
| apalmieri | 0:a77f1f1f8318 | 704 | } |
| apalmieri | 0:a77f1f1f8318 | 705 | break; |
| apalmieri | 0:a77f1f1f8318 | 706 | |
| apalmieri | 0:a77f1f1f8318 | 707 | default: |
| apalmieri | 0:a77f1f1f8318 | 708 | break; |
| apalmieri | 0:a77f1f1f8318 | 709 | } |
| apalmieri | 0:a77f1f1f8318 | 710 | /* Recover the normal state */ |
| apalmieri | 0:a77f1f1f8318 | 711 | |
| apalmieri | 0:a77f1f1f8318 | 712 | } |
| apalmieri | 0:a77f1f1f8318 | 713 | |
| apalmieri | 0:a77f1f1f8318 | 714 | void |
| apalmieri | 0:a77f1f1f8318 | 715 | Teseo::process(void) |
| apalmieri | 0:a77f1f1f8318 | 716 | { |
| apalmieri | 0:a77f1f1f8318 | 717 | osEvent evt = queue.get(); |
| apalmieri | 0:a77f1f1f8318 | 718 | if (evt.status == osEventMessage) { |
| apalmieri | 0:a77f1f1f8318 | 719 | struct _teseoMsg *message = (struct _teseoMsg *)evt.value.p; |
| apalmieri | 0:a77f1f1f8318 | 720 | if (message->len > 0) { |
| apalmieri | 0:a77f1f1f8318 | 721 | |
| apalmieri | 0:a77f1f1f8318 | 722 | for(int m = 0; m < PSTM_NMEA_MSGS_NUM; m++) { |
| apalmieri | 0:a77f1f1f8318 | 723 | _GetPSTMsg((ePSTMsg)m, message->buf); |
| apalmieri | 0:a77f1f1f8318 | 724 | } |
| apalmieri | 0:a77f1f1f8318 | 725 | for(int m = 0; m < NMEA_MSGS_NUM; m++) { |
| apalmieri | 0:a77f1f1f8318 | 726 | _GetMsg((eMsg)m, message->buf); |
| apalmieri | 0:a77f1f1f8318 | 727 | } |
| apalmieri | 0:a77f1f1f8318 | 728 | } |
| apalmieri | 0:a77f1f1f8318 | 729 | |
| apalmieri | 0:a77f1f1f8318 | 730 | mpool.free(message); |
| apalmieri | 0:a77f1f1f8318 | 731 | } |
| apalmieri | 0:a77f1f1f8318 | 732 | } |
| apalmieri | 0:a77f1f1f8318 | 733 | |
| apalmieri | 0:a77f1f1f8318 | 734 | uint32_t |
| apalmieri | 0:a77f1f1f8318 | 735 | Teseo::ioctl(uint32_t command, void *arg) |
| apalmieri | 0:a77f1f1f8318 | 736 | { |
| apalmieri | 0:a77f1f1f8318 | 737 | /* TBI */ |
| apalmieri | 0:a77f1f1f8318 | 738 | return 0; |
| apalmieri | 0:a77f1f1f8318 | 739 | } |
| apalmieri | 0:a77f1f1f8318 | 740 | |
| apalmieri | 0:a77f1f1f8318 | 741 | void |
| apalmieri | 0:a77f1f1f8318 | 742 | Teseo::lpmGetImmediateLocation(void) |
| apalmieri | 0:a77f1f1f8318 | 743 | { |
| apalmieri | 0:a77f1f1f8318 | 744 | /* TBI */ |
| apalmieri | 0:a77f1f1f8318 | 745 | } |
| apalmieri | 0:a77f1f1f8318 | 746 | |
| apalmieri | 0:a77f1f1f8318 | 747 | const GPSProvider::LocationUpdateParams_t * |
| apalmieri | 0:a77f1f1f8318 | 748 | Teseo::getLastLocation(void) const |
| apalmieri | 0:a77f1f1f8318 | 749 | { |
| apalmieri | 0:a77f1f1f8318 | 750 | return &lastLocation; |
| apalmieri | 0:a77f1f1f8318 | 751 | } |
| apalmieri | 0:a77f1f1f8318 | 752 | |
| apalmieri | 0:a77f1f1f8318 | 753 | void |
| apalmieri | 0:a77f1f1f8318 | 754 | Teseo::reset(void) |
| apalmieri | 0:a77f1f1f8318 | 755 | { |
| apalmieri | 0:a77f1f1f8318 | 756 | _ResetFast(_serialDebug); |
| apalmieri | 0:a77f1f1f8318 | 757 | } |
| apalmieri | 0:a77f1f1f8318 | 758 | |
| apalmieri | 0:a77f1f1f8318 | 759 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 760 | Teseo::cfgMessageList(int level) |
| apalmieri | 0:a77f1f1f8318 | 761 | { |
| apalmieri | 0:a77f1f1f8318 | 762 | int lowMask = 0x2; |
| apalmieri | 0:a77f1f1f8318 | 763 | int highMask = 0x80000; |
| apalmieri | 0:a77f1f1f8318 | 764 | |
| apalmieri | 0:a77f1f1f8318 | 765 | if(level == 2) { |
| apalmieri | 0:a77f1f1f8318 | 766 | lowMask = 0x18004F; |
| apalmieri | 0:a77f1f1f8318 | 767 | } |
| apalmieri | 0:a77f1f1f8318 | 768 | |
| apalmieri | 0:a77f1f1f8318 | 769 | sprintf(_teseoCmd, "$PSTMCFGMSGL,%d,%d,%x,%x", |
| apalmieri | 0:a77f1f1f8318 | 770 | 0, /*NMEA 0*/ |
| apalmieri | 0:a77f1f1f8318 | 771 | 1, /*Rate*/ |
| apalmieri | 0:a77f1f1f8318 | 772 | lowMask, |
| apalmieri | 0:a77f1f1f8318 | 773 | highMask); |
| apalmieri | 0:a77f1f1f8318 | 774 | |
| apalmieri | 0:a77f1f1f8318 | 775 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 776 | |
| apalmieri | 0:a77f1f1f8318 | 777 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 778 | } |
| apalmieri | 0:a77f1f1f8318 | 779 | |
| apalmieri | 0:a77f1f1f8318 | 780 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 781 | Teseo::saveConfigParams(void) |
| apalmieri | 0:a77f1f1f8318 | 782 | { |
| apalmieri | 0:a77f1f1f8318 | 783 | sprintf(_teseoCmd, "$PSTMSAVEPAR"); |
| apalmieri | 0:a77f1f1f8318 | 784 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 785 | |
| apalmieri | 0:a77f1f1f8318 | 786 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 787 | } |
| apalmieri | 0:a77f1f1f8318 | 788 | |
| apalmieri | 0:a77f1f1f8318 | 789 | bool |
| apalmieri | 0:a77f1f1f8318 | 790 | Teseo::isGeofencingSupported(void) |
| apalmieri | 0:a77f1f1f8318 | 791 | { |
| apalmieri | 0:a77f1f1f8318 | 792 | return true; |
| apalmieri | 0:a77f1f1f8318 | 793 | } |
| apalmieri | 0:a77f1f1f8318 | 794 | |
| apalmieri | 0:a77f1f1f8318 | 795 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 796 | Teseo::enableGeofence(void) |
| apalmieri | 0:a77f1f1f8318 | 797 | { |
| apalmieri | 0:a77f1f1f8318 | 798 | //$PSTMCFGGEOFENCE,<en>,<tol>*<checksum><cr><lf> |
| apalmieri | 0:a77f1f1f8318 | 799 | sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d",1,1); |
| apalmieri | 0:a77f1f1f8318 | 800 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 801 | |
| apalmieri | 0:a77f1f1f8318 | 802 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 803 | } |
| apalmieri | 0:a77f1f1f8318 | 804 | |
| apalmieri | 0:a77f1f1f8318 | 805 | //FIXME! |
| apalmieri | 0:a77f1f1f8318 | 806 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 807 | Teseo::cfgGeofenceCircle(void) |
| apalmieri | 0:a77f1f1f8318 | 808 | { |
| apalmieri | 0:a77f1f1f8318 | 809 | GPSGeofence::GeofenceCircle_t circle = { |
| apalmieri | 0:a77f1f1f8318 | 810 | .id = 0, |
| apalmieri | 0:a77f1f1f8318 | 811 | .enabled = 1, |
| apalmieri | 0:a77f1f1f8318 | 812 | .tolerance = 1, |
| apalmieri | 0:a77f1f1f8318 | 813 | .lat = 40.336055, |
| apalmieri | 0:a77f1f1f8318 | 814 | .lon = 18.120611, |
| apalmieri | 0:a77f1f1f8318 | 815 | .radius = 200 |
| apalmieri | 0:a77f1f1f8318 | 816 | }; |
| apalmieri | 0:a77f1f1f8318 | 817 | |
| apalmieri | 0:a77f1f1f8318 | 818 | sprintf(_teseoCmd, "$PSTMCFGGEOCIR,%d,%d,%lf,%lf,%lf", |
| apalmieri | 0:a77f1f1f8318 | 819 | circle.id, |
| apalmieri | 0:a77f1f1f8318 | 820 | circle.enabled, |
| apalmieri | 0:a77f1f1f8318 | 821 | circle.lat, |
| apalmieri | 0:a77f1f1f8318 | 822 | circle.lon, |
| apalmieri | 0:a77f1f1f8318 | 823 | circle.radius); |
| apalmieri | 0:a77f1f1f8318 | 824 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 825 | |
| apalmieri | 0:a77f1f1f8318 | 826 | sprintf(_teseoCmd, "$PSTMSAVEPAR"); |
| apalmieri | 0:a77f1f1f8318 | 827 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 828 | |
| apalmieri | 0:a77f1f1f8318 | 829 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 830 | } |
| apalmieri | 0:a77f1f1f8318 | 831 | |
| apalmieri | 0:a77f1f1f8318 | 832 | |
| apalmieri | 0:a77f1f1f8318 | 833 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 834 | Teseo::configGeofences(GPSGeofence *geofences[], unsigned geofenceCount) |
| apalmieri | 0:a77f1f1f8318 | 835 | { |
| apalmieri | 0:a77f1f1f8318 | 836 | uint8_t trials; |
| apalmieri | 0:a77f1f1f8318 | 837 | |
| apalmieri | 0:a77f1f1f8318 | 838 | if(geofenceCount > MAX_GEOFENCES_NUM) { |
| apalmieri | 0:a77f1f1f8318 | 839 | return GPS_ERROR_GEOFENCE_MAX_EXCEEDED; |
| apalmieri | 0:a77f1f1f8318 | 840 | } |
| apalmieri | 0:a77f1f1f8318 | 841 | |
| apalmieri | 0:a77f1f1f8318 | 842 | for(uint8_t i = 0; i < geofenceCount; i++) { |
| apalmieri | 0:a77f1f1f8318 | 843 | trials = 1; |
| apalmieri | 0:a77f1f1f8318 | 844 | //printf("Teseo::configGeofences id=%d\r\n", (geofences[i]->getGeofenceCircle()).id); |
| apalmieri | 0:a77f1f1f8318 | 845 | /* |
| apalmieri | 0:a77f1f1f8318 | 846 | printf("Teseo::configGeofences en=%d\r\n", en); |
| apalmieri | 0:a77f1f1f8318 | 847 | printf("Teseo::configGeofences tol=%d\r\n", (geofences[i]->getGeofenceCircle()).tolerance); |
| apalmieri | 0:a77f1f1f8318 | 848 | printf("Teseo::configGeofences lat=%02f\r\n", (geofences[i]->getGeofenceCircle()).lat); |
| apalmieri | 0:a77f1f1f8318 | 849 | printf("Teseo::configGeofences lon=%02f\r\n", (geofences[i]->getGeofenceCircle()).lon); |
| apalmieri | 0:a77f1f1f8318 | 850 | printf("Teseo::configGeofences radius=%02f\r\n", (geofences[i]->getGeofenceCircle()).radius); |
| apalmieri | 0:a77f1f1f8318 | 851 | */ |
| apalmieri | 0:a77f1f1f8318 | 852 | sprintf(_teseoCmd, "$PSTMGEOFENCECFG,%d,%d,%d,%lf,%lf,%lf", |
| apalmieri | 0:a77f1f1f8318 | 853 | (geofences[i]->getGeofenceCircle()).id, |
| apalmieri | 0:a77f1f1f8318 | 854 | (geofences[i]->getGeofenceCircle()).enabled, |
| apalmieri | 0:a77f1f1f8318 | 855 | (geofences[i]->getGeofenceCircle()).tolerance, |
| apalmieri | 0:a77f1f1f8318 | 856 | (geofences[i]->getGeofenceCircle()).lat, |
| apalmieri | 0:a77f1f1f8318 | 857 | (geofences[i]->getGeofenceCircle()).lon, |
| apalmieri | 0:a77f1f1f8318 | 858 | (geofences[i]->getGeofenceCircle()).radius); |
| apalmieri | 0:a77f1f1f8318 | 859 | |
| apalmieri | 0:a77f1f1f8318 | 860 | do{ |
| apalmieri | 0:a77f1f1f8318 | 861 | trials--; |
| apalmieri | 0:a77f1f1f8318 | 862 | |
| apalmieri | 0:a77f1f1f8318 | 863 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 864 | /* |
| apalmieri | 0:a77f1f1f8318 | 865 | sprintf(_teseoCmd, "$PSTMCFGGEOFENCE,%d,%d\n\r",1,1); |
| apalmieri | 0:a77f1f1f8318 | 866 | |
| apalmieri | 0:a77f1f1f8318 | 867 | //sprintf(_teseoCmd, "$PSTMGETSWVER,6"); |
| apalmieri | 0:a77f1f1f8318 | 868 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 869 | |
| apalmieri | 0:a77f1f1f8318 | 870 | sprintf(_teseoCmd, "$PSTMSAVEPAR\n\r"); |
| apalmieri | 0:a77f1f1f8318 | 871 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 872 | */ |
| apalmieri | 0:a77f1f1f8318 | 873 | //printf("Teseo::configGeofences _teseoCmd=%s\r\n", _teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 874 | } while (trials > 0); |
| apalmieri | 0:a77f1f1f8318 | 875 | } |
| apalmieri | 0:a77f1f1f8318 | 876 | //printf("Teseo::configGeofences sizeof(geofences)=%d numGeofences=%d strlen(_teseoCmd)=%d\r\n", sizeof(geofences), numGeofences, strlen(_teseoCmd)); |
| apalmieri | 0:a77f1f1f8318 | 877 | |
| apalmieri | 0:a77f1f1f8318 | 878 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 879 | } |
| apalmieri | 0:a77f1f1f8318 | 880 | |
| apalmieri | 0:a77f1f1f8318 | 881 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 882 | Teseo::geofenceReq(void) |
| apalmieri | 0:a77f1f1f8318 | 883 | { |
| apalmieri | 0:a77f1f1f8318 | 884 | sprintf(_teseoCmd, "$PSTMGEOFENCEREQ"); |
| apalmieri | 0:a77f1f1f8318 | 885 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 886 | |
| apalmieri | 0:a77f1f1f8318 | 887 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 888 | } |
| apalmieri | 0:a77f1f1f8318 | 889 | |
| apalmieri | 0:a77f1f1f8318 | 890 | bool |
| apalmieri | 0:a77f1f1f8318 | 891 | Teseo::isDataloggingSupported(void) |
| apalmieri | 0:a77f1f1f8318 | 892 | { |
| apalmieri | 0:a77f1f1f8318 | 893 | return true; |
| apalmieri | 0:a77f1f1f8318 | 894 | } |
| apalmieri | 0:a77f1f1f8318 | 895 | |
| apalmieri | 0:a77f1f1f8318 | 896 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 897 | Teseo::enableDatalog(void) |
| apalmieri | 0:a77f1f1f8318 | 898 | { |
| apalmieri | 0:a77f1f1f8318 | 899 | //$PSTMCFGLOG,<en>,<circ>,<rectype>,<oneshot>,<rate>,<speed>,<dist>*<checksum><cr><lf> |
| apalmieri | 0:a77f1f1f8318 | 900 | sprintf(_teseoCmd, "$PSTMCFGLOG,%d,%d,%d,%d,%u,%u,%u", |
| apalmieri | 0:a77f1f1f8318 | 901 | 1, //Enable/Disable the log |
| apalmieri | 0:a77f1f1f8318 | 902 | 1, //Enable/Disable circular mode |
| apalmieri | 0:a77f1f1f8318 | 903 | 1, //Record type |
| apalmieri | 0:a77f1f1f8318 | 904 | 0, //Enable/Disable one shot mode |
| apalmieri | 0:a77f1f1f8318 | 905 | 5, //time interval in seconds between two consecutive logged records |
| apalmieri | 0:a77f1f1f8318 | 906 | 0, //minimum speed threshold |
| apalmieri | 0:a77f1f1f8318 | 907 | 0 //distance threshold |
| apalmieri | 0:a77f1f1f8318 | 908 | ); |
| apalmieri | 0:a77f1f1f8318 | 909 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 910 | |
| apalmieri | 0:a77f1f1f8318 | 911 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 912 | } |
| apalmieri | 0:a77f1f1f8318 | 913 | |
| apalmieri | 0:a77f1f1f8318 | 914 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 915 | Teseo::configDatalog(GPSDatalog *datalog) |
| apalmieri | 0:a77f1f1f8318 | 916 | { |
| apalmieri | 0:a77f1f1f8318 | 917 | //printf("Teseo::configDatalog 0x%03x\r\n", (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer())); |
| apalmieri | 0:a77f1f1f8318 | 918 | //$PSTMLOGCREATE,<cfg>,<min-rate>,<min-speed>,<min-position>,<logmask>*<checksum><cr><lf> |
| apalmieri | 0:a77f1f1f8318 | 919 | sprintf(_teseoCmd, "$PSTMLOGCREATE,%03x,%u,%u,%u,%d", |
| apalmieri | 0:a77f1f1f8318 | 920 | (datalog->getEnableBufferFullAlarm())<<1|(datalog->getEnableCircularBuffer()), |
| apalmieri | 0:a77f1f1f8318 | 921 | datalog->getMinRate(), |
| apalmieri | 0:a77f1f1f8318 | 922 | datalog->getMinSpeed(), |
| apalmieri | 0:a77f1f1f8318 | 923 | datalog->getMinPosition(), |
| apalmieri | 0:a77f1f1f8318 | 924 | datalog->getLogMask() |
| apalmieri | 0:a77f1f1f8318 | 925 | ); |
| apalmieri | 0:a77f1f1f8318 | 926 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 927 | |
| apalmieri | 0:a77f1f1f8318 | 928 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 929 | } |
| apalmieri | 0:a77f1f1f8318 | 930 | |
| apalmieri | 0:a77f1f1f8318 | 931 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 932 | Teseo::startDatalog(void) |
| apalmieri | 0:a77f1f1f8318 | 933 | { |
| apalmieri | 0:a77f1f1f8318 | 934 | sprintf(_teseoCmd, "$PSTMLOGSTART"); |
| apalmieri | 0:a77f1f1f8318 | 935 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 936 | |
| apalmieri | 0:a77f1f1f8318 | 937 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 938 | } |
| apalmieri | 0:a77f1f1f8318 | 939 | |
| apalmieri | 0:a77f1f1f8318 | 940 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 941 | Teseo::stopDatalog(void) |
| apalmieri | 0:a77f1f1f8318 | 942 | { |
| apalmieri | 0:a77f1f1f8318 | 943 | sprintf(_teseoCmd, "$PSTMLOGSTOP"); |
| apalmieri | 0:a77f1f1f8318 | 944 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 945 | |
| apalmieri | 0:a77f1f1f8318 | 946 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 947 | } |
| apalmieri | 0:a77f1f1f8318 | 948 | |
| apalmieri | 0:a77f1f1f8318 | 949 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 950 | Teseo::eraseDatalog(void) |
| apalmieri | 0:a77f1f1f8318 | 951 | { |
| apalmieri | 0:a77f1f1f8318 | 952 | sprintf(_teseoCmd, "$PSTMLOGERASE"); |
| apalmieri | 0:a77f1f1f8318 | 953 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 954 | |
| apalmieri | 0:a77f1f1f8318 | 955 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 956 | } |
| apalmieri | 0:a77f1f1f8318 | 957 | |
| apalmieri | 0:a77f1f1f8318 | 958 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 959 | Teseo::logReqStatus(void) |
| apalmieri | 0:a77f1f1f8318 | 960 | { |
| apalmieri | 0:a77f1f1f8318 | 961 | /* TBI */ |
| apalmieri | 0:a77f1f1f8318 | 962 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 963 | } |
| apalmieri | 0:a77f1f1f8318 | 964 | |
| apalmieri | 0:a77f1f1f8318 | 965 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 966 | Teseo::logReqQuery(GPSProvider::LogQueryParams_t &logReqQuery) |
| apalmieri | 0:a77f1f1f8318 | 967 | { |
| apalmieri | 0:a77f1f1f8318 | 968 | /* TBI */ |
| apalmieri | 0:a77f1f1f8318 | 969 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 970 | } |
| apalmieri | 0:a77f1f1f8318 | 971 | |
| apalmieri | 0:a77f1f1f8318 | 972 | bool |
| apalmieri | 0:a77f1f1f8318 | 973 | Teseo::isOdometerSupported(void) |
| apalmieri | 0:a77f1f1f8318 | 974 | { |
| apalmieri | 0:a77f1f1f8318 | 975 | return true; |
| apalmieri | 0:a77f1f1f8318 | 976 | } |
| apalmieri | 0:a77f1f1f8318 | 977 | |
| apalmieri | 0:a77f1f1f8318 | 978 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 979 | Teseo::enableOdo(void) |
| apalmieri | 0:a77f1f1f8318 | 980 | { |
| apalmieri | 0:a77f1f1f8318 | 981 | //$PSTMCFGODO,<en>,<enmsg>,<alarm>*<checksum><cr><lf> |
| apalmieri | 0:a77f1f1f8318 | 982 | sprintf(_teseoCmd, "$PSTMCFGODO,1,1,1"); |
| apalmieri | 0:a77f1f1f8318 | 983 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 984 | |
| apalmieri | 0:a77f1f1f8318 | 985 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 986 | } |
| apalmieri | 0:a77f1f1f8318 | 987 | |
| apalmieri | 0:a77f1f1f8318 | 988 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 989 | Teseo::startOdo(unsigned alarmDistance) |
| apalmieri | 0:a77f1f1f8318 | 990 | { |
| apalmieri | 0:a77f1f1f8318 | 991 | sprintf(_teseoCmd, "$PSTMODOSTART,%08x", alarmDistance); |
| apalmieri | 0:a77f1f1f8318 | 992 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 993 | |
| apalmieri | 0:a77f1f1f8318 | 994 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 995 | } |
| apalmieri | 0:a77f1f1f8318 | 996 | |
| apalmieri | 0:a77f1f1f8318 | 997 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 998 | Teseo::stopOdo(void) |
| apalmieri | 0:a77f1f1f8318 | 999 | { |
| apalmieri | 0:a77f1f1f8318 | 1000 | sprintf(_teseoCmd, "$PSTMODOSTOP"); |
| apalmieri | 0:a77f1f1f8318 | 1001 | SendCommand(_teseoCmd); |
| apalmieri | 0:a77f1f1f8318 | 1002 | |
| apalmieri | 0:a77f1f1f8318 | 1003 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 1004 | } |
| apalmieri | 0:a77f1f1f8318 | 1005 | gps_provider_error_t |
| apalmieri | 0:a77f1f1f8318 | 1006 | Teseo::resetOdo(void) |
| apalmieri | 0:a77f1f1f8318 | 1007 | { |
| apalmieri | 0:a77f1f1f8318 | 1008 | /* TBI */ |
| apalmieri | 0:a77f1f1f8318 | 1009 | return GPS_ERROR_NONE; |
| apalmieri | 0:a77f1f1f8318 | 1010 | } |