![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
hue
Teseo-LIV3F/Teseo-LIV3F.cpp@1:d6e2c8220523, 2019-02-19 (annotated)
- Committer:
- etiene32
- Date:
- Tue Feb 19 13:12:21 2019 +0000
- Revision:
- 1:d6e2c8220523
- Parent:
- 0:4e088cbb2dbf
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nirnakern | 0:4e088cbb2dbf | 1 | /* |
nirnakern | 0:4e088cbb2dbf | 2 | * ------------------------------------------------------------------------- |
nirnakern | 0:4e088cbb2dbf | 3 | * Copyright (C) 2017 STMicroelectronics |
nirnakern | 0:4e088cbb2dbf | 4 | * Author: Francesco M. Virlinzi <francesco.virlinzi@st.com> |
nirnakern | 0:4e088cbb2dbf | 5 | * |
nirnakern | 0:4e088cbb2dbf | 6 | * May be copied or modified under the terms of the GNU General Public |
nirnakern | 0:4e088cbb2dbf | 7 | * License V.2 ONLY. See linux/COPYING for more information. |
nirnakern | 0:4e088cbb2dbf | 8 | * |
nirnakern | 0:4e088cbb2dbf | 9 | * ------------------------------------------------------------------------- |
nirnakern | 0:4e088cbb2dbf | 10 | */ |
nirnakern | 0:4e088cbb2dbf | 11 | #include "string.h" |
nirnakern | 0:4e088cbb2dbf | 12 | #include "Teseo-LIV3F.h" |
nirnakern | 0:4e088cbb2dbf | 13 | |
nirnakern | 0:4e088cbb2dbf | 14 | |
nirnakern | 0:4e088cbb2dbf | 15 | static char T3_name[] = "Teseo-LIV3F"; |
nirnakern | 0:4e088cbb2dbf | 16 | |
nirnakern | 0:4e088cbb2dbf | 17 | char _OK[] = "OK"; |
nirnakern | 0:4e088cbb2dbf | 18 | char _Failed[] = "Failed"; |
nirnakern | 0:4e088cbb2dbf | 19 | char X_Nucleo_name[] = "X-Nucleo-GNSS1A1"; |
nirnakern | 0:4e088cbb2dbf | 20 | |
nirnakern | 0:4e088cbb2dbf | 21 | struct teseo_cmd { |
nirnakern | 0:4e088cbb2dbf | 22 | char *cmd; |
nirnakern | 0:4e088cbb2dbf | 23 | }; |
nirnakern | 0:4e088cbb2dbf | 24 | |
nirnakern | 0:4e088cbb2dbf | 25 | static struct teseo_cmd teseo_cmds[] = { |
nirnakern | 0:4e088cbb2dbf | 26 | [Teseo_LIV3F::GETSWVER] = { |
nirnakern | 0:4e088cbb2dbf | 27 | .cmd = "$PSTMGETSWVER,7", |
nirnakern | 0:4e088cbb2dbf | 28 | }, |
nirnakern | 0:4e088cbb2dbf | 29 | [Teseo_LIV3F::FORCESTANDBY] = { |
nirnakern | 0:4e088cbb2dbf | 30 | .cmd = "$PSTMFORCESTANDBY,00007", |
nirnakern | 0:4e088cbb2dbf | 31 | }, |
nirnakern | 0:4e088cbb2dbf | 32 | [Teseo_LIV3F::RFTESTON] = { |
nirnakern | 0:4e088cbb2dbf | 33 | .cmd = "$PSTMRFTESTON,16", |
nirnakern | 0:4e088cbb2dbf | 34 | }, |
nirnakern | 0:4e088cbb2dbf | 35 | [Teseo_LIV3F::RFTESTOFF] = { |
nirnakern | 0:4e088cbb2dbf | 36 | .cmd = "$PSTMRFTESTOFF\n\r", |
nirnakern | 0:4e088cbb2dbf | 37 | }, |
nirnakern | 0:4e088cbb2dbf | 38 | [Teseo_LIV3F::LOWPOWER] = { |
nirnakern | 0:4e088cbb2dbf | 39 | .cmd = "$PSTMLOWPOWERONOFF,1,0,000,05,0,1,000,1,00010,01,0,0,1,01", |
nirnakern | 0:4e088cbb2dbf | 40 | }, |
nirnakern | 0:4e088cbb2dbf | 41 | [Teseo_LIV3F::FWUPDATE] = { |
nirnakern | 0:4e088cbb2dbf | 42 | .cmd = "$PSTMFWUPGRADE", |
nirnakern | 0:4e088cbb2dbf | 43 | }, |
nirnakern | 0:4e088cbb2dbf | 44 | }; |
nirnakern | 0:4e088cbb2dbf | 45 | |
nirnakern | 0:4e088cbb2dbf | 46 | |
nirnakern | 0:4e088cbb2dbf | 47 | Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin, |
nirnakern | 0:4e088cbb2dbf | 48 | PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin, |
nirnakern | 0:4e088cbb2dbf | 49 | Serial *serial_debug): |
nirnakern | 0:4e088cbb2dbf | 50 | _reset(reset_pin, 1), |
nirnakern | 0:4e088cbb2dbf | 51 | _pps(pps_pin), |
nirnakern | 0:4e088cbb2dbf | 52 | _wakeup(wakeup_pin, 0), |
nirnakern | 0:4e088cbb2dbf | 53 | _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD), |
nirnakern | 0:4e088cbb2dbf | 54 | _serial_debug(serial_debug) |
nirnakern | 0:4e088cbb2dbf | 55 | { |
nirnakern | 0:4e088cbb2dbf | 56 | wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS); |
nirnakern | 0:4e088cbb2dbf | 57 | _uart.baud(SDT_UART_BAUD); |
nirnakern | 0:4e088cbb2dbf | 58 | _uart.format(8, SerialBase::None, 1); |
nirnakern | 0:4e088cbb2dbf | 59 | _i2c = NULL; |
nirnakern | 0:4e088cbb2dbf | 60 | _uart_interleaded = false; |
nirnakern | 0:4e088cbb2dbf | 61 | _uart_discard = false; |
nirnakern | 0:4e088cbb2dbf | 62 | } |
nirnakern | 0:4e088cbb2dbf | 63 | |
nirnakern | 0:4e088cbb2dbf | 64 | Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin, |
nirnakern | 0:4e088cbb2dbf | 65 | PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin, |
nirnakern | 0:4e088cbb2dbf | 66 | I2C *bus, Serial *serial_debug): |
nirnakern | 0:4e088cbb2dbf | 67 | _reset(reset_pin, 1), |
nirnakern | 0:4e088cbb2dbf | 68 | _pps(pps_pin), |
nirnakern | 0:4e088cbb2dbf | 69 | _wakeup(wakeup_pin, 0), |
nirnakern | 0:4e088cbb2dbf | 70 | _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD), |
nirnakern | 0:4e088cbb2dbf | 71 | _i2c(bus), |
nirnakern | 0:4e088cbb2dbf | 72 | _serial_debug(serial_debug) |
nirnakern | 0:4e088cbb2dbf | 73 | { |
nirnakern | 0:4e088cbb2dbf | 74 | wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS); |
nirnakern | 0:4e088cbb2dbf | 75 | _uart.baud(SDT_UART_BAUD); |
nirnakern | 0:4e088cbb2dbf | 76 | _uart.format(8, SerialBase::None, 1); |
nirnakern | 0:4e088cbb2dbf | 77 | _uart_interleaded = false; |
nirnakern | 0:4e088cbb2dbf | 78 | _uart_discard = false; |
nirnakern | 0:4e088cbb2dbf | 79 | } |
nirnakern | 0:4e088cbb2dbf | 80 | |
nirnakern | 0:4e088cbb2dbf | 81 | int Teseo_LIV3F::EnableLowPower() |
nirnakern | 0:4e088cbb2dbf | 82 | { |
nirnakern | 0:4e088cbb2dbf | 83 | SendCommand(LOWPOWER); |
nirnakern | 0:4e088cbb2dbf | 84 | return 0; |
nirnakern | 0:4e088cbb2dbf | 85 | } |
nirnakern | 0:4e088cbb2dbf | 86 | |
nirnakern | 0:4e088cbb2dbf | 87 | void Teseo_LIV3F::Reset(Serial *serial_debug) |
nirnakern | 0:4e088cbb2dbf | 88 | { |
nirnakern | 0:4e088cbb2dbf | 89 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 90 | serial_debug->printf("%s: Resetting...", T3_name); |
nirnakern | 0:4e088cbb2dbf | 91 | |
nirnakern | 0:4e088cbb2dbf | 92 | _reset.write(0); |
nirnakern | 0:4e088cbb2dbf | 93 | |
nirnakern | 0:4e088cbb2dbf | 94 | wait_ms(50); |
nirnakern | 0:4e088cbb2dbf | 95 | |
nirnakern | 0:4e088cbb2dbf | 96 | _reset.write(1); |
nirnakern | 0:4e088cbb2dbf | 97 | |
nirnakern | 0:4e088cbb2dbf | 98 | wait_ms(70); |
nirnakern | 0:4e088cbb2dbf | 99 | |
nirnakern | 0:4e088cbb2dbf | 100 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 101 | serial_debug->printf("Done...\n\r"); |
nirnakern | 0:4e088cbb2dbf | 102 | } |
nirnakern | 0:4e088cbb2dbf | 103 | |
nirnakern | 0:4e088cbb2dbf | 104 | enum { |
nirnakern | 0:4e088cbb2dbf | 105 | TESEO_FLASHER_IDENTIFIER = 0, // 0xBCD501F4 |
nirnakern | 0:4e088cbb2dbf | 106 | TESEO_FLASHER_SYNC, // 0x83984073 |
nirnakern | 0:4e088cbb2dbf | 107 | DEVICE_START_COMMUNICATION, // 0xA3 |
nirnakern | 0:4e088cbb2dbf | 108 | FLASHER_READY, // 0x4A |
nirnakern | 0:4e088cbb2dbf | 109 | ACK, // 0xCC |
nirnakern | 0:4e088cbb2dbf | 110 | NACK, |
nirnakern | 0:4e088cbb2dbf | 111 | }; |
nirnakern | 0:4e088cbb2dbf | 112 | |
nirnakern | 0:4e088cbb2dbf | 113 | struct firmware_ctrl { |
nirnakern | 0:4e088cbb2dbf | 114 | char *cmd; |
nirnakern | 0:4e088cbb2dbf | 115 | unsigned char len; |
nirnakern | 0:4e088cbb2dbf | 116 | char *n; |
nirnakern | 0:4e088cbb2dbf | 117 | } ; |
nirnakern | 0:4e088cbb2dbf | 118 | |
nirnakern | 0:4e088cbb2dbf | 119 | /* |
nirnakern | 0:4e088cbb2dbf | 120 | * #define FWUPG_IDENTIFIER 0xBC D5 01 F4 |
nirnakern | 0:4e088cbb2dbf | 121 | * #define FWUPG_SYNC 0x83 98 40 73 |
nirnakern | 0:4e088cbb2dbf | 122 | */ |
nirnakern | 0:4e088cbb2dbf | 123 | static struct firmware_ctrl fw_data[] = { |
nirnakern | 0:4e088cbb2dbf | 124 | [TESEO_FLASHER_IDENTIFIER] = { |
nirnakern | 0:4e088cbb2dbf | 125 | .cmd = (char *)(char[]){ 0xF4, 0x01, 0xD5, 0xBC}, |
nirnakern | 0:4e088cbb2dbf | 126 | .len = 4, |
nirnakern | 0:4e088cbb2dbf | 127 | .n = "TESEO_FLASHER_IDENTIFIER", |
nirnakern | 0:4e088cbb2dbf | 128 | }, |
nirnakern | 0:4e088cbb2dbf | 129 | [TESEO_FLASHER_SYNC] = { |
nirnakern | 0:4e088cbb2dbf | 130 | .cmd =(char *)(char[]){ 0x73, 0x40, 0x98, 0x83 }, |
nirnakern | 0:4e088cbb2dbf | 131 | .len = 4, |
nirnakern | 0:4e088cbb2dbf | 132 | .n = "TESEO_FLASHER_SYNC", |
nirnakern | 0:4e088cbb2dbf | 133 | }, |
nirnakern | 0:4e088cbb2dbf | 134 | [DEVICE_START_COMMUNICATION] = { |
nirnakern | 0:4e088cbb2dbf | 135 | .cmd = (char *)(char[]){0xA3}, |
nirnakern | 0:4e088cbb2dbf | 136 | .len = 1, |
nirnakern | 0:4e088cbb2dbf | 137 | .n = "DEVICE_START_COMMUNICATION", |
nirnakern | 0:4e088cbb2dbf | 138 | }, |
nirnakern | 0:4e088cbb2dbf | 139 | [FLASHER_READY] = { |
nirnakern | 0:4e088cbb2dbf | 140 | .cmd = (char *)(char[]){0x4A}, |
nirnakern | 0:4e088cbb2dbf | 141 | .len = 1, |
nirnakern | 0:4e088cbb2dbf | 142 | .n = "FLASHER_READY", |
nirnakern | 0:4e088cbb2dbf | 143 | }, |
nirnakern | 0:4e088cbb2dbf | 144 | [ACK] = { |
nirnakern | 0:4e088cbb2dbf | 145 | .cmd = (char *)(char[]){0xCC}, |
nirnakern | 0:4e088cbb2dbf | 146 | .len = 1, |
nirnakern | 0:4e088cbb2dbf | 147 | .n = "ACK", |
nirnakern | 0:4e088cbb2dbf | 148 | }, |
nirnakern | 0:4e088cbb2dbf | 149 | [NACK] = { |
nirnakern | 0:4e088cbb2dbf | 150 | .cmd = (char *)(char[]){0xDD}, |
nirnakern | 0:4e088cbb2dbf | 151 | .len = 1, |
nirnakern | 0:4e088cbb2dbf | 152 | .n = "NACK", |
nirnakern | 0:4e088cbb2dbf | 153 | }, |
nirnakern | 0:4e088cbb2dbf | 154 | }; |
nirnakern | 0:4e088cbb2dbf | 155 | |
nirnakern | 0:4e088cbb2dbf | 156 | int Teseo_LIV3F::SendString(char *buf, int len) |
nirnakern | 0:4e088cbb2dbf | 157 | { |
nirnakern | 0:4e088cbb2dbf | 158 | for (int i = 0; i < len; ++i) { |
nirnakern | 0:4e088cbb2dbf | 159 | while (!_uart.writeable()); |
nirnakern | 0:4e088cbb2dbf | 160 | _uart.putc(buf[i]); |
nirnakern | 0:4e088cbb2dbf | 161 | } |
nirnakern | 0:4e088cbb2dbf | 162 | } |
nirnakern | 0:4e088cbb2dbf | 163 | |
nirnakern | 0:4e088cbb2dbf | 164 | struct ImageOptions |
nirnakern | 0:4e088cbb2dbf | 165 | { |
nirnakern | 0:4e088cbb2dbf | 166 | unsigned char eraseNVM; |
nirnakern | 0:4e088cbb2dbf | 167 | unsigned char programOnly; |
nirnakern | 0:4e088cbb2dbf | 168 | unsigned char reserved; |
nirnakern | 0:4e088cbb2dbf | 169 | unsigned char baudRate; |
nirnakern | 0:4e088cbb2dbf | 170 | unsigned int firmwareSize; |
nirnakern | 0:4e088cbb2dbf | 171 | unsigned int firmwareCRC; |
nirnakern | 0:4e088cbb2dbf | 172 | unsigned int nvmAddressOffset; |
nirnakern | 0:4e088cbb2dbf | 173 | unsigned int nvmSize; |
nirnakern | 0:4e088cbb2dbf | 174 | } liv3f_img_option = { |
nirnakern | 0:4e088cbb2dbf | 175 | .eraseNVM = 1, |
nirnakern | 0:4e088cbb2dbf | 176 | .programOnly = 0, |
nirnakern | 0:4e088cbb2dbf | 177 | .reserved = 0, |
nirnakern | 0:4e088cbb2dbf | 178 | .baudRate = 1, |
nirnakern | 0:4e088cbb2dbf | 179 | .firmwareSize = 0, |
nirnakern | 0:4e088cbb2dbf | 180 | .firmwareCRC = 0, |
nirnakern | 0:4e088cbb2dbf | 181 | .nvmAddressOffset = 0x00100000, |
nirnakern | 0:4e088cbb2dbf | 182 | .nvmSize = 0x00100000, |
nirnakern | 0:4e088cbb2dbf | 183 | |
nirnakern | 0:4e088cbb2dbf | 184 | }; |
nirnakern | 0:4e088cbb2dbf | 185 | |
nirnakern | 0:4e088cbb2dbf | 186 | int Teseo_LIV3F::FwWaitAck() |
nirnakern | 0:4e088cbb2dbf | 187 | { |
nirnakern | 0:4e088cbb2dbf | 188 | while (!_uart.readable()); |
nirnakern | 0:4e088cbb2dbf | 189 | |
nirnakern | 0:4e088cbb2dbf | 190 | char c = _uart.getc(); |
nirnakern | 0:4e088cbb2dbf | 191 | |
nirnakern | 0:4e088cbb2dbf | 192 | if (fw_data[ACK].cmd[0] == c) { |
nirnakern | 0:4e088cbb2dbf | 193 | if (_serial_debug) |
nirnakern | 0:4e088cbb2dbf | 194 | _serial_debug->printf("%s (0x%x)\n\r", _OK, c); |
nirnakern | 0:4e088cbb2dbf | 195 | return 0; |
nirnakern | 0:4e088cbb2dbf | 196 | } |
nirnakern | 0:4e088cbb2dbf | 197 | |
nirnakern | 0:4e088cbb2dbf | 198 | if (fw_data[NACK].cmd[0] == c) { |
nirnakern | 0:4e088cbb2dbf | 199 | if (_serial_debug) |
nirnakern | 0:4e088cbb2dbf | 200 | _serial_debug->printf("%s (%x)\n\r", _Failed, c); |
nirnakern | 0:4e088cbb2dbf | 201 | return -1; |
nirnakern | 0:4e088cbb2dbf | 202 | } |
nirnakern | 0:4e088cbb2dbf | 203 | |
nirnakern | 0:4e088cbb2dbf | 204 | if (_serial_debug) |
nirnakern | 0:4e088cbb2dbf | 205 | _serial_debug->printf("%s - Char not allowed (%x)\n\r", _Failed, c); |
nirnakern | 0:4e088cbb2dbf | 206 | return -1; |
nirnakern | 0:4e088cbb2dbf | 207 | } |
nirnakern | 0:4e088cbb2dbf | 208 | |
nirnakern | 0:4e088cbb2dbf | 209 | bool Teseo_LIV3F::FirmwareUpdate(bool is_recovery, char *data, |
nirnakern | 0:4e088cbb2dbf | 210 | unsigned int data_len, |
nirnakern | 0:4e088cbb2dbf | 211 | unsigned long crc, |
nirnakern | 0:4e088cbb2dbf | 212 | Serial *serial_debug) |
nirnakern | 0:4e088cbb2dbf | 213 | { |
nirnakern | 0:4e088cbb2dbf | 214 | unsigned int i; |
nirnakern | 0:4e088cbb2dbf | 215 | |
nirnakern | 0:4e088cbb2dbf | 216 | char _buf[4] = { 0xff, 0xff, 0xff, 0xff }; |
nirnakern | 0:4e088cbb2dbf | 217 | |
nirnakern | 0:4e088cbb2dbf | 218 | liv3f_img_option.firmwareSize = data_len; |
nirnakern | 0:4e088cbb2dbf | 219 | liv3f_img_option.firmwareCRC = crc; |
nirnakern | 0:4e088cbb2dbf | 220 | |
nirnakern | 0:4e088cbb2dbf | 221 | if (data == NULL || !data_len) |
nirnakern | 0:4e088cbb2dbf | 222 | return false; |
nirnakern | 0:4e088cbb2dbf | 223 | |
nirnakern | 0:4e088cbb2dbf | 224 | if (is_recovery) |
nirnakern | 0:4e088cbb2dbf | 225 | Reset(); |
nirnakern | 0:4e088cbb2dbf | 226 | |
nirnakern | 0:4e088cbb2dbf | 227 | { |
nirnakern | 0:4e088cbb2dbf | 228 | |
nirnakern | 0:4e088cbb2dbf | 229 | _uart.baud(FWU_UART_BAUD); |
nirnakern | 0:4e088cbb2dbf | 230 | |
nirnakern | 0:4e088cbb2dbf | 231 | #if 1 |
nirnakern | 0:4e088cbb2dbf | 232 | while (1) { |
nirnakern | 0:4e088cbb2dbf | 233 | /* send TESEO_FLASHER_IDENTIFIER */ |
nirnakern | 0:4e088cbb2dbf | 234 | /* |
nirnakern | 0:4e088cbb2dbf | 235 | * Device is under reset. Host sends continuously “TESEO2_FLASHER_IDENTIFIER� word. |
nirnakern | 0:4e088cbb2dbf | 236 | */ |
nirnakern | 0:4e088cbb2dbf | 237 | SendString(fw_data[TESEO_FLASHER_IDENTIFIER].cmd, fw_data[TESEO_FLASHER_IDENTIFIER].len); |
nirnakern | 0:4e088cbb2dbf | 238 | |
nirnakern | 0:4e088cbb2dbf | 239 | /* try to read... TESEO_FLASHER_SYNC */ |
nirnakern | 0:4e088cbb2dbf | 240 | if (_uart.readable()) |
nirnakern | 0:4e088cbb2dbf | 241 | for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len; ) { |
nirnakern | 0:4e088cbb2dbf | 242 | |
nirnakern | 0:4e088cbb2dbf | 243 | while (!_uart.readable()); |
nirnakern | 0:4e088cbb2dbf | 244 | |
nirnakern | 0:4e088cbb2dbf | 245 | _buf[i] = _uart.getc(); |
nirnakern | 0:4e088cbb2dbf | 246 | |
nirnakern | 0:4e088cbb2dbf | 247 | if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == _buf[i]) { |
nirnakern | 0:4e088cbb2dbf | 248 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 249 | serial_debug->printf("-- %d -- 0x%x -- ok--\n\r", i, _buf[i]); |
nirnakern | 0:4e088cbb2dbf | 250 | i++; |
nirnakern | 0:4e088cbb2dbf | 251 | goto exit_step_1; /* FMV: WA to have firmware update working.... */ |
nirnakern | 0:4e088cbb2dbf | 252 | } else { |
nirnakern | 0:4e088cbb2dbf | 253 | i = 0; |
nirnakern | 0:4e088cbb2dbf | 254 | } |
nirnakern | 0:4e088cbb2dbf | 255 | } |
nirnakern | 0:4e088cbb2dbf | 256 | if (i == fw_data[TESEO_FLASHER_SYNC].len) |
nirnakern | 0:4e088cbb2dbf | 257 | goto exit_step_1; |
nirnakern | 0:4e088cbb2dbf | 258 | } |
nirnakern | 0:4e088cbb2dbf | 259 | |
nirnakern | 0:4e088cbb2dbf | 260 | exit_step_1: |
nirnakern | 0:4e088cbb2dbf | 261 | |
nirnakern | 0:4e088cbb2dbf | 262 | _uart.abort_read(); |
nirnakern | 0:4e088cbb2dbf | 263 | |
nirnakern | 0:4e088cbb2dbf | 264 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 265 | serial_debug->printf("Got: %s from %s\n\r", fw_data[TESEO_FLASHER_SYNC].n, T3_name); |
nirnakern | 0:4e088cbb2dbf | 266 | |
nirnakern | 0:4e088cbb2dbf | 267 | /* |
nirnakern | 0:4e088cbb2dbf | 268 | * Host sends “DEVICE_START_COMMUNICATION� word. |
nirnakern | 0:4e088cbb2dbf | 269 | */ |
nirnakern | 0:4e088cbb2dbf | 270 | serial_debug->printf("\n\r%s Step: %s ",T3_name, fw_data[DEVICE_START_COMMUNICATION].n); |
nirnakern | 0:4e088cbb2dbf | 271 | SendString(fw_data[DEVICE_START_COMMUNICATION].cmd, fw_data[DEVICE_START_COMMUNICATION].len); |
nirnakern | 0:4e088cbb2dbf | 272 | |
nirnakern | 0:4e088cbb2dbf | 273 | FwWaitAck(); |
nirnakern | 0:4e088cbb2dbf | 274 | |
nirnakern | 0:4e088cbb2dbf | 275 | /* |
nirnakern | 0:4e088cbb2dbf | 276 | * Host sends the binary image options. Both host and |
nirnakern | 0:4e088cbb2dbf | 277 | * device change UART baud rates. Host sends continuously |
nirnakern | 0:4e088cbb2dbf | 278 | * the “FLASHER_READY� word. |
nirnakern | 0:4e088cbb2dbf | 279 | */ |
nirnakern | 0:4e088cbb2dbf | 280 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 281 | serial_debug->printf("%s Step: Send ImageOption\n\r",T3_name); |
nirnakern | 0:4e088cbb2dbf | 282 | |
nirnakern | 0:4e088cbb2dbf | 283 | SendString((char*)&liv3f_img_option, sizeof(ImageOptions)); |
nirnakern | 0:4e088cbb2dbf | 284 | |
nirnakern | 0:4e088cbb2dbf | 285 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 286 | serial_debug->printf("%s Step: Send %s\n\r",T3_name, fw_data[FLASHER_READY].n); |
nirnakern | 0:4e088cbb2dbf | 287 | |
nirnakern | 0:4e088cbb2dbf | 288 | while (1) { |
nirnakern | 0:4e088cbb2dbf | 289 | SendString(fw_data[FLASHER_READY].cmd, fw_data[FLASHER_READY].len); |
nirnakern | 0:4e088cbb2dbf | 290 | if (_uart.readable()) |
nirnakern | 0:4e088cbb2dbf | 291 | goto exit_step_3; |
nirnakern | 0:4e088cbb2dbf | 292 | } |
nirnakern | 0:4e088cbb2dbf | 293 | |
nirnakern | 0:4e088cbb2dbf | 294 | exit_step_3: |
nirnakern | 0:4e088cbb2dbf | 295 | FwWaitAck(); |
nirnakern | 0:4e088cbb2dbf | 296 | |
nirnakern | 0:4e088cbb2dbf | 297 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 298 | serial_debug->printf("%s Step: Erasing flash area ",T3_name); |
nirnakern | 0:4e088cbb2dbf | 299 | |
nirnakern | 0:4e088cbb2dbf | 300 | |
nirnakern | 0:4e088cbb2dbf | 301 | FwWaitAck(); |
nirnakern | 0:4e088cbb2dbf | 302 | |
nirnakern | 0:4e088cbb2dbf | 303 | /* |
nirnakern | 0:4e088cbb2dbf | 304 | * Device is erasing flash program. Host is waiting for an “ACK�. |
nirnakern | 0:4e088cbb2dbf | 305 | */ |
nirnakern | 0:4e088cbb2dbf | 306 | |
nirnakern | 0:4e088cbb2dbf | 307 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 308 | serial_debug->printf("%s Step: Erasing NVM ",T3_name); |
nirnakern | 0:4e088cbb2dbf | 309 | |
nirnakern | 0:4e088cbb2dbf | 310 | while (!_uart.readable()); |
nirnakern | 0:4e088cbb2dbf | 311 | |
nirnakern | 0:4e088cbb2dbf | 312 | FwWaitAck(); |
nirnakern | 0:4e088cbb2dbf | 313 | |
nirnakern | 0:4e088cbb2dbf | 314 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 315 | serial_debug->printf("%s Step: Sending data... ",T3_name); |
nirnakern | 0:4e088cbb2dbf | 316 | |
nirnakern | 0:4e088cbb2dbf | 317 | for (i = 0; i < (data_len / (16*1024)); ++i) { |
nirnakern | 0:4e088cbb2dbf | 318 | SendString(&data[i], 16*1024); |
nirnakern | 0:4e088cbb2dbf | 319 | |
nirnakern | 0:4e088cbb2dbf | 320 | FwWaitAck(); |
nirnakern | 0:4e088cbb2dbf | 321 | } |
nirnakern | 0:4e088cbb2dbf | 322 | |
nirnakern | 0:4e088cbb2dbf | 323 | serial_debug->printf("\n\r"); |
nirnakern | 0:4e088cbb2dbf | 324 | /* |
nirnakern | 0:4e088cbb2dbf | 325 | * send remaining data... |
nirnakern | 0:4e088cbb2dbf | 326 | */ |
nirnakern | 0:4e088cbb2dbf | 327 | if (data_len != (i * 16*1024)) { |
nirnakern | 0:4e088cbb2dbf | 328 | SendString(&data[i*16*1024], data_len-i*16*1024); |
nirnakern | 0:4e088cbb2dbf | 329 | FwWaitAck(); |
nirnakern | 0:4e088cbb2dbf | 330 | } |
nirnakern | 0:4e088cbb2dbf | 331 | /* |
nirnakern | 0:4e088cbb2dbf | 332 | * wait CRC ack |
nirnakern | 0:4e088cbb2dbf | 333 | */ |
nirnakern | 0:4e088cbb2dbf | 334 | FwWaitAck(); |
nirnakern | 0:4e088cbb2dbf | 335 | } |
nirnakern | 0:4e088cbb2dbf | 336 | |
nirnakern | 0:4e088cbb2dbf | 337 | #else |
nirnakern | 0:4e088cbb2dbf | 338 | //_uart.format(8, SerialBase::Forced0, 1); |
nirnakern | 0:4e088cbb2dbf | 339 | while (1) |
nirnakern | 0:4e088cbb2dbf | 340 | { |
nirnakern | 0:4e088cbb2dbf | 341 | /* send TESEO_FLASHER_IDENTIFIER */ |
nirnakern | 0:4e088cbb2dbf | 342 | for (i = 0; i < fw_data[TESEO_FLASHER_IDENTIFIER].len; ++i) { |
nirnakern | 0:4e088cbb2dbf | 343 | while (!_uart.writeable()); |
nirnakern | 0:4e088cbb2dbf | 344 | _uart.putc(fw_data[TESEO_FLASHER_IDENTIFIER].cmd[i]); |
nirnakern | 0:4e088cbb2dbf | 345 | } |
nirnakern | 0:4e088cbb2dbf | 346 | |
nirnakern | 0:4e088cbb2dbf | 347 | |
nirnakern | 0:4e088cbb2dbf | 348 | /* try to read... TESEO_FLASHER_SYNC */ |
nirnakern | 0:4e088cbb2dbf | 349 | //while (!_uart.readable()); |
nirnakern | 0:4e088cbb2dbf | 350 | for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len && _uart.readable(); ) |
nirnakern | 0:4e088cbb2dbf | 351 | if (_uart.readable()) |
nirnakern | 0:4e088cbb2dbf | 352 | { |
nirnakern | 0:4e088cbb2dbf | 353 | char c = _uart.getc(); |
nirnakern | 0:4e088cbb2dbf | 354 | /* |
nirnakern | 0:4e088cbb2dbf | 355 | if (!c) |
nirnakern | 0:4e088cbb2dbf | 356 | break; |
nirnakern | 0:4e088cbb2dbf | 357 | */ |
nirnakern | 0:4e088cbb2dbf | 358 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 359 | serial_debug->printf("%x vs %x\n\r", c, fw_data[TESEO_FLASHER_SYNC].cmd[i]); |
nirnakern | 0:4e088cbb2dbf | 360 | |
nirnakern | 0:4e088cbb2dbf | 361 | if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == c) |
nirnakern | 0:4e088cbb2dbf | 362 | i++; |
nirnakern | 0:4e088cbb2dbf | 363 | else |
nirnakern | 0:4e088cbb2dbf | 364 | i = 0; |
nirnakern | 0:4e088cbb2dbf | 365 | } |
nirnakern | 0:4e088cbb2dbf | 366 | |
nirnakern | 0:4e088cbb2dbf | 367 | if (i == fw_data[TESEO_FLASHER_SYNC].len && serial_debug) |
nirnakern | 0:4e088cbb2dbf | 368 | serial_debug->printf("Got %s from %s\n\r",fw_data[TESEO_FLASHER_SYNC].n, T3_name); |
nirnakern | 0:4e088cbb2dbf | 369 | } |
nirnakern | 0:4e088cbb2dbf | 370 | } |
nirnakern | 0:4e088cbb2dbf | 371 | #endif |
nirnakern | 0:4e088cbb2dbf | 372 | if (serial_debug) |
nirnakern | 0:4e088cbb2dbf | 373 | serial_debug->printf("END\n\r"); |
nirnakern | 0:4e088cbb2dbf | 374 | |
nirnakern | 0:4e088cbb2dbf | 375 | return true; |
nirnakern | 0:4e088cbb2dbf | 376 | } |
nirnakern | 0:4e088cbb2dbf | 377 | |
nirnakern | 0:4e088cbb2dbf | 378 | int Teseo_LIV3F::WakeUp() |
nirnakern | 0:4e088cbb2dbf | 379 | { |
nirnakern | 0:4e088cbb2dbf | 380 | wait_ms(100); |
nirnakern | 0:4e088cbb2dbf | 381 | |
nirnakern | 0:4e088cbb2dbf | 382 | _wakeup.write(1); |
nirnakern | 0:4e088cbb2dbf | 383 | |
nirnakern | 0:4e088cbb2dbf | 384 | wait_ms(500); |
nirnakern | 0:4e088cbb2dbf | 385 | |
nirnakern | 0:4e088cbb2dbf | 386 | _wakeup.write(0); |
nirnakern | 0:4e088cbb2dbf | 387 | |
nirnakern | 0:4e088cbb2dbf | 388 | return 0; |
nirnakern | 0:4e088cbb2dbf | 389 | } |
nirnakern | 0:4e088cbb2dbf | 390 | |
nirnakern | 0:4e088cbb2dbf | 391 | bool Teseo_LIV3F::CheckPPSWorking() |
nirnakern | 0:4e088cbb2dbf | 392 | { |
nirnakern | 0:4e088cbb2dbf | 393 | int val_0, val_1; |
nirnakern | 0:4e088cbb2dbf | 394 | |
nirnakern | 0:4e088cbb2dbf | 395 | wait_ms(500); |
nirnakern | 0:4e088cbb2dbf | 396 | |
nirnakern | 0:4e088cbb2dbf | 397 | val_0 = _pps.read(); |
nirnakern | 0:4e088cbb2dbf | 398 | |
nirnakern | 0:4e088cbb2dbf | 399 | wait_ms(500); |
nirnakern | 0:4e088cbb2dbf | 400 | val_1 = _pps.read(); |
nirnakern | 0:4e088cbb2dbf | 401 | |
nirnakern | 0:4e088cbb2dbf | 402 | if (val_0 != val_1) |
nirnakern | 0:4e088cbb2dbf | 403 | return true; |
nirnakern | 0:4e088cbb2dbf | 404 | |
nirnakern | 0:4e088cbb2dbf | 405 | return false; |
nirnakern | 0:4e088cbb2dbf | 406 | } |
nirnakern | 0:4e088cbb2dbf | 407 | |
nirnakern | 0:4e088cbb2dbf | 408 | int Teseo_LIV3F::CRC_(char *buf, int size) |
nirnakern | 0:4e088cbb2dbf | 409 | { |
nirnakern | 0:4e088cbb2dbf | 410 | int i = 0, ch = 0; |
nirnakern | 0:4e088cbb2dbf | 411 | |
nirnakern | 0:4e088cbb2dbf | 412 | if (buf[0] == '$') |
nirnakern | 0:4e088cbb2dbf | 413 | ++i; |
nirnakern | 0:4e088cbb2dbf | 414 | |
nirnakern | 0:4e088cbb2dbf | 415 | if (size) |
nirnakern | 0:4e088cbb2dbf | 416 | for (; i < size; ++i) |
nirnakern | 0:4e088cbb2dbf | 417 | ch ^= buf[i]; |
nirnakern | 0:4e088cbb2dbf | 418 | else |
nirnakern | 0:4e088cbb2dbf | 419 | for (; buf[i] != 0; ++i) |
nirnakern | 0:4e088cbb2dbf | 420 | ch ^= buf[i]; |
nirnakern | 0:4e088cbb2dbf | 421 | |
nirnakern | 0:4e088cbb2dbf | 422 | return ch; |
nirnakern | 0:4e088cbb2dbf | 423 | } |
nirnakern | 0:4e088cbb2dbf | 424 | |
nirnakern | 0:4e088cbb2dbf | 425 | bool Teseo_LIV3F::WaitBooting(Timer *t, float timeout) |
nirnakern | 0:4e088cbb2dbf | 426 | { |
nirnakern | 0:4e088cbb2dbf | 427 | unsigned int now = t->read_ms();; |
nirnakern | 0:4e088cbb2dbf | 428 | while (1) { |
nirnakern | 0:4e088cbb2dbf | 429 | if (CheckPPSWorking() == true) |
nirnakern | 0:4e088cbb2dbf | 430 | return true; |
nirnakern | 0:4e088cbb2dbf | 431 | |
nirnakern | 0:4e088cbb2dbf | 432 | if ((now + timeout*1000) < t->read_ms()) |
nirnakern | 0:4e088cbb2dbf | 433 | break; |
nirnakern | 0:4e088cbb2dbf | 434 | } |
nirnakern | 0:4e088cbb2dbf | 435 | |
nirnakern | 0:4e088cbb2dbf | 436 | return false; |
nirnakern | 0:4e088cbb2dbf | 437 | } |
nirnakern | 0:4e088cbb2dbf | 438 | |
nirnakern | 0:4e088cbb2dbf | 439 | void Teseo_LIV3F::SendCommand(enum Teseo_LIV3F::cmd_enum c) |
nirnakern | 0:4e088cbb2dbf | 440 | { |
nirnakern | 0:4e088cbb2dbf | 441 | char crc[5]; |
nirnakern | 0:4e088cbb2dbf | 442 | |
nirnakern | 0:4e088cbb2dbf | 443 | sprintf(crc, "*%02X\n\r", CRC_(teseo_cmds[c].cmd, 0)); |
nirnakern | 0:4e088cbb2dbf | 444 | |
nirnakern | 0:4e088cbb2dbf | 445 | _uart_mutex_lock(); |
nirnakern | 0:4e088cbb2dbf | 446 | _uart_interleaded = true; |
nirnakern | 0:4e088cbb2dbf | 447 | SendString(teseo_cmds[c].cmd, strlen(teseo_cmds[c].cmd)); |
nirnakern | 0:4e088cbb2dbf | 448 | SendString(crc, 5); |
nirnakern | 0:4e088cbb2dbf | 449 | _uart_mutex_unlock(); |
nirnakern | 0:4e088cbb2dbf | 450 | } |
nirnakern | 0:4e088cbb2dbf | 451 | |
nirnakern | 0:4e088cbb2dbf | 452 | char *Teseo_LIV3F::DetectSentence(const char *cmd, char *buf, unsigned long len) |
nirnakern | 0:4e088cbb2dbf | 453 | { |
nirnakern | 0:4e088cbb2dbf | 454 | char *result = NULL; |
nirnakern | 0:4e088cbb2dbf | 455 | unsigned int i = 0; |
nirnakern | 0:4e088cbb2dbf | 456 | const unsigned long cmd_len = strlen(cmd); |
nirnakern | 0:4e088cbb2dbf | 457 | len -= strlen(cmd); |
nirnakern | 0:4e088cbb2dbf | 458 | |
nirnakern | 0:4e088cbb2dbf | 459 | while (!result && i < len) { |
nirnakern | 0:4e088cbb2dbf | 460 | for (; buf[i] != '$' && i < len; ++i); /* 1. check '$' char */ |
nirnakern | 0:4e088cbb2dbf | 461 | if (i == len) |
nirnakern | 0:4e088cbb2dbf | 462 | break; /* no more char.... */ |
nirnakern | 0:4e088cbb2dbf | 463 | |
nirnakern | 0:4e088cbb2dbf | 464 | ++i; /* to point to the char after '$' */ |
nirnakern | 0:4e088cbb2dbf | 465 | |
nirnakern | 0:4e088cbb2dbf | 466 | if (strncmp(&buf[i], cmd, cmd_len) == 0) { |
nirnakern | 0:4e088cbb2dbf | 467 | result = &buf[i]; |
nirnakern | 0:4e088cbb2dbf | 468 | } |
nirnakern | 0:4e088cbb2dbf | 469 | } |
nirnakern | 0:4e088cbb2dbf | 470 | |
nirnakern | 0:4e088cbb2dbf | 471 | if (result) { |
nirnakern | 0:4e088cbb2dbf | 472 | for (i = 0; result[i] != '*'; ++i); |
nirnakern | 0:4e088cbb2dbf | 473 | result[i] = 0; |
nirnakern | 0:4e088cbb2dbf | 474 | } |
nirnakern | 0:4e088cbb2dbf | 475 | #if 0 |
nirnakern | 0:4e088cbb2dbf | 476 | if (_serial_debug) |
nirnakern | 0:4e088cbb2dbf | 477 | _serial_debug->printf("%s: %s: %s %s FOUND\n\r", T3_name, __FUNCTION__, cmd, result ? " " : "NOT"); |
nirnakern | 0:4e088cbb2dbf | 478 | #endif |
nirnakern | 0:4e088cbb2dbf | 479 | return result; |
nirnakern | 0:4e088cbb2dbf | 480 | } |
nirnakern | 0:4e088cbb2dbf | 481 | |
nirnakern | 0:4e088cbb2dbf | 482 | |
nirnakern | 0:4e088cbb2dbf | 483 | int Teseo_LIV3F::CheckI2C() |
nirnakern | 0:4e088cbb2dbf | 484 | { |
nirnakern | 0:4e088cbb2dbf | 485 | |
nirnakern | 0:4e088cbb2dbf | 486 | if (!_i2c) |
nirnakern | 0:4e088cbb2dbf | 487 | return -1; |
nirnakern | 0:4e088cbb2dbf | 488 | |
nirnakern | 0:4e088cbb2dbf | 489 | _i2c->start(); |
nirnakern | 0:4e088cbb2dbf | 490 | int res = _i2c->write((TESEO_I2C_ADDRESS << 1) | 1); |
nirnakern | 0:4e088cbb2dbf | 491 | _i2c->stop(); |
nirnakern | 0:4e088cbb2dbf | 492 | /* |
nirnakern | 0:4e088cbb2dbf | 493 | * @returns |
nirnakern | 0:4e088cbb2dbf | 494 | * '0' - NAK was received |
nirnakern | 0:4e088cbb2dbf | 495 | * '1' - ACK was received, |
nirnakern | 0:4e088cbb2dbf | 496 | * '2' - timeout |
nirnakern | 0:4e088cbb2dbf | 497 | */ |
nirnakern | 0:4e088cbb2dbf | 498 | return res == 1 ? 0 : -1; |
nirnakern | 0:4e088cbb2dbf | 499 | } |
nirnakern | 0:4e088cbb2dbf | 500 | |
nirnakern | 0:4e088cbb2dbf | 501 | int Teseo_LIV3F::ReadMessage(char *buf, unsigned long len, Timer *t, float timeout) |
nirnakern | 0:4e088cbb2dbf | 502 | { |
nirnakern | 0:4e088cbb2dbf | 503 | memset(buf, 0, len); |
nirnakern | 0:4e088cbb2dbf | 504 | |
nirnakern | 0:4e088cbb2dbf | 505 | for (unsigned int i = 0; i < len; ++i){ |
nirnakern | 0:4e088cbb2dbf | 506 | if (t) { |
nirnakern | 0:4e088cbb2dbf | 507 | unsigned int now = t->read_ms();; |
nirnakern | 0:4e088cbb2dbf | 508 | while (!_uart.readable() && (now + timeout*1000) > t->read_ms()); |
nirnakern | 0:4e088cbb2dbf | 509 | } else |
nirnakern | 0:4e088cbb2dbf | 510 | while (!_uart.readable()); |
nirnakern | 0:4e088cbb2dbf | 511 | if (_uart.readable()) |
nirnakern | 0:4e088cbb2dbf | 512 | buf[i] = _uart.getc();; |
nirnakern | 0:4e088cbb2dbf | 513 | } |
nirnakern | 0:4e088cbb2dbf | 514 | #if 0 |
nirnakern | 0:4e088cbb2dbf | 515 | if (_serial_debug) { |
nirnakern | 0:4e088cbb2dbf | 516 | unsigned int i; |
nirnakern | 0:4e088cbb2dbf | 517 | _serial_debug->printf("\n\r---------------------\n\r"); |
nirnakern | 0:4e088cbb2dbf | 518 | for (i = 0; i < len ; ++i) |
nirnakern | 0:4e088cbb2dbf | 519 | _serial_debug->putc((int)buf[i]); |
nirnakern | 0:4e088cbb2dbf | 520 | _serial_debug->printf("\n\r---------------------\n\r"); |
nirnakern | 0:4e088cbb2dbf | 521 | } |
nirnakern | 0:4e088cbb2dbf | 522 | #endif |
nirnakern | 0:4e088cbb2dbf | 523 | return 0; |
nirnakern | 0:4e088cbb2dbf | 524 | } |
nirnakern | 0:4e088cbb2dbf | 525 | |
nirnakern | 0:4e088cbb2dbf | 526 | void Teseo_LIV3F::RFTest(bool enable) |
nirnakern | 0:4e088cbb2dbf | 527 | { |
nirnakern | 0:4e088cbb2dbf | 528 | if (enable) |
nirnakern | 0:4e088cbb2dbf | 529 | SendCommand(Teseo_LIV3F::RFTESTON); |
nirnakern | 0:4e088cbb2dbf | 530 | else |
nirnakern | 0:4e088cbb2dbf | 531 | SendCommand(Teseo_LIV3F::RFTESTOFF); |
nirnakern | 0:4e088cbb2dbf | 532 | } |
nirnakern | 0:4e088cbb2dbf | 533 | |
nirnakern | 0:4e088cbb2dbf | 534 | void Teseo_LIV3F::ReadLoop(Serial *serial_debug) |
nirnakern | 0:4e088cbb2dbf | 535 | { |
nirnakern | 0:4e088cbb2dbf | 536 | while (1) |
nirnakern | 0:4e088cbb2dbf | 537 | if (_uart.readable()) { |
nirnakern | 0:4e088cbb2dbf | 538 | int c = _uart.getc(); |
nirnakern | 0:4e088cbb2dbf | 539 | serial_debug->putc(c); |
nirnakern | 0:4e088cbb2dbf | 540 | } |
nirnakern | 0:4e088cbb2dbf | 541 | } |
nirnakern | 0:4e088cbb2dbf | 542 | |
nirnakern | 0:4e088cbb2dbf | 543 | char *Teseo_LIV3F::ReadSentence(const char *cmd, char *buf, unsigned long len) |
nirnakern | 0:4e088cbb2dbf | 544 | { |
nirnakern | 0:4e088cbb2dbf | 545 | int ret = ReadMessage(buf, len); |
nirnakern | 0:4e088cbb2dbf | 546 | if (ret) |
nirnakern | 0:4e088cbb2dbf | 547 | return NULL; |
nirnakern | 0:4e088cbb2dbf | 548 | return DetectSentence(cmd, buf, len); |
nirnakern | 0:4e088cbb2dbf | 549 | } |
nirnakern | 0:4e088cbb2dbf | 550 | |
nirnakern | 0:4e088cbb2dbf | 551 | struct ___msg { |
nirnakern | 0:4e088cbb2dbf | 552 | unsigned char len; |
nirnakern | 0:4e088cbb2dbf | 553 | char *str; |
nirnakern | 0:4e088cbb2dbf | 554 | }; |
nirnakern | 0:4e088cbb2dbf | 555 | |
nirnakern | 0:4e088cbb2dbf | 556 | |
nirnakern | 0:4e088cbb2dbf | 557 | static const struct ___msg teseo_msgs[] = { |
nirnakern | 0:4e088cbb2dbf | 558 | [ NMEA_GPGGA ] = { .len = 5, .str = "GPGGA", }, |
nirnakern | 0:4e088cbb2dbf | 559 | [ NMEA_GPGLL ] = { .len = 5, .str = "GPGLL", }, |
nirnakern | 0:4e088cbb2dbf | 560 | [ NMEA_GNGSA ] = { .len = 5, .str = "GNGSA", }, |
nirnakern | 0:4e088cbb2dbf | 561 | [ NMEA_GPTXT ] = { .len = 5, .str = "GPTXT", }, |
nirnakern | 0:4e088cbb2dbf | 562 | [ NMEA_GPVTG ] = { .len = 5, .str = "GPVTG", }, |
nirnakern | 0:4e088cbb2dbf | 563 | [ NMEA_GPRMC ] = { .len = 5, .str = "GPRMC", }, |
nirnakern | 0:4e088cbb2dbf | 564 | [ NMEA_PSTMCPU ] = { .len = 7, .str = "PSTMCPU", }, |
nirnakern | 0:4e088cbb2dbf | 565 | [ NMEA_PSTMVER ] = { .len = 7, .str = "PSTMVER", }, |
nirnakern | 0:4e088cbb2dbf | 566 | }; |
nirnakern | 0:4e088cbb2dbf | 567 | |
nirnakern | 0:4e088cbb2dbf | 568 | |
nirnakern | 0:4e088cbb2dbf | 569 | enum nmea_msg_id Teseo_LIV3F::MsgDetect(char *buf, int buf_len, Serial *serial_debug) |
nirnakern | 0:4e088cbb2dbf | 570 | { |
nirnakern | 0:4e088cbb2dbf | 571 | int i; |
nirnakern | 0:4e088cbb2dbf | 572 | |
nirnakern | 0:4e088cbb2dbf | 573 | if (buf[0] == '$') |
nirnakern | 0:4e088cbb2dbf | 574 | ++buf; |
nirnakern | 0:4e088cbb2dbf | 575 | |
nirnakern | 0:4e088cbb2dbf | 576 | for (i = 0; i < NMEA_END__; ++i) |
nirnakern | 0:4e088cbb2dbf | 577 | if (memcmp((void*)teseo_msgs[i].str, (void*)buf, teseo_msgs[i].len) == 0) |
nirnakern | 0:4e088cbb2dbf | 578 | return (enum nmea_msg_id) i; |
nirnakern | 0:4e088cbb2dbf | 579 | #if 0 |
nirnakern | 0:4e088cbb2dbf | 580 | if (serial_debug) { |
nirnakern | 0:4e088cbb2dbf | 581 | serial_debug->puts("MESSAGE NOT FOUND: "); |
nirnakern | 0:4e088cbb2dbf | 582 | for (int i = 0; i < 5; ++i) |
nirnakern | 0:4e088cbb2dbf | 583 | serial_debug->putc(lbuf[i]); |
nirnakern | 0:4e088cbb2dbf | 584 | serial_debug->puts("\n\r"); |
nirnakern | 0:4e088cbb2dbf | 585 | } |
nirnakern | 0:4e088cbb2dbf | 586 | #endif |
nirnakern | 0:4e088cbb2dbf | 587 | return NMEA_END__; |
nirnakern | 0:4e088cbb2dbf | 588 | } |
nirnakern | 0:4e088cbb2dbf | 589 | |
nirnakern | 0:4e088cbb2dbf | 590 | void Teseo_LIV3F::UARTStreamProcess(Serial *serial_debug) |
nirnakern | 0:4e088cbb2dbf | 591 | { |
nirnakern | 0:4e088cbb2dbf | 592 | enum nmea_msg_id id; |
nirnakern | 0:4e088cbb2dbf | 593 | char c; |
nirnakern | 0:4e088cbb2dbf | 594 | |
nirnakern | 0:4e088cbb2dbf | 595 | struct teseo_msg *msg = mpool.alloc(); |
nirnakern | 0:4e088cbb2dbf | 596 | msg->len = 0; |
nirnakern | 0:4e088cbb2dbf | 597 | |
nirnakern | 0:4e088cbb2dbf | 598 | while (true) { |
nirnakern | 0:4e088cbb2dbf | 599 | _uart_mutex_lock(); |
nirnakern | 0:4e088cbb2dbf | 600 | #if 0 |
nirnakern | 0:4e088cbb2dbf | 601 | if (_uart_interleaded == true) { |
nirnakern | 0:4e088cbb2dbf | 602 | msg->len = 0; |
nirnakern | 0:4e088cbb2dbf | 603 | _uart_interleaded = false; |
nirnakern | 0:4e088cbb2dbf | 604 | _uart_discard = true; |
nirnakern | 0:4e088cbb2dbf | 605 | } |
nirnakern | 0:4e088cbb2dbf | 606 | #endif |
nirnakern | 0:4e088cbb2dbf | 607 | if (_uart.readable()) { |
nirnakern | 0:4e088cbb2dbf | 608 | c = _uart.getc(); |
nirnakern | 0:4e088cbb2dbf | 609 | _uart_mutex_unlock(); |
nirnakern | 0:4e088cbb2dbf | 610 | if (c == '$') { |
nirnakern | 0:4e088cbb2dbf | 611 | queue.put(msg); |
nirnakern | 0:4e088cbb2dbf | 612 | msg = mpool.alloc(); |
nirnakern | 0:4e088cbb2dbf | 613 | msg->len = 0; |
nirnakern | 0:4e088cbb2dbf | 614 | _uart_discard = false; |
nirnakern | 0:4e088cbb2dbf | 615 | } |
nirnakern | 0:4e088cbb2dbf | 616 | if (!_uart_discard) |
nirnakern | 0:4e088cbb2dbf | 617 | msg->buf[msg->len++] = c; |
nirnakern | 0:4e088cbb2dbf | 618 | } else { |
nirnakern | 0:4e088cbb2dbf | 619 | _uart_mutex_unlock(); |
nirnakern | 0:4e088cbb2dbf | 620 | wait_us(100); |
nirnakern | 0:4e088cbb2dbf | 621 | } |
nirnakern | 0:4e088cbb2dbf | 622 | } |
nirnakern | 0:4e088cbb2dbf | 623 | } |
nirnakern | 0:4e088cbb2dbf | 624 | |
nirnakern | 0:4e088cbb2dbf | 625 | struct thr_data { |
nirnakern | 0:4e088cbb2dbf | 626 | Teseo_LIV3F *gnss; |
nirnakern | 0:4e088cbb2dbf | 627 | Serial *serial_debug; |
nirnakern | 0:4e088cbb2dbf | 628 | }; |
nirnakern | 0:4e088cbb2dbf | 629 | |
nirnakern | 0:4e088cbb2dbf | 630 | static void Teseo_LIV3F_UARTStreamProcess(struct thr_data *data) |
nirnakern | 0:4e088cbb2dbf | 631 | { |
nirnakern | 0:4e088cbb2dbf | 632 | data->gnss->UARTStreamProcess(data->serial_debug); |
nirnakern | 0:4e088cbb2dbf | 633 | } |
nirnakern | 0:4e088cbb2dbf | 634 | |
nirnakern | 0:4e088cbb2dbf | 635 | void Teseo_LIV3F::startListener(Serial *serial_debug) |
nirnakern | 0:4e088cbb2dbf | 636 | { |
nirnakern | 0:4e088cbb2dbf | 637 | if (serialStreamThread.get_state() == Thread::Running) |
nirnakern | 0:4e088cbb2dbf | 638 | return; |
nirnakern | 0:4e088cbb2dbf | 639 | |
nirnakern | 0:4e088cbb2dbf | 640 | static struct thr_data data = { |
nirnakern | 0:4e088cbb2dbf | 641 | .gnss = this, |
nirnakern | 0:4e088cbb2dbf | 642 | .serial_debug = serial_debug, |
nirnakern | 0:4e088cbb2dbf | 643 | }; |
nirnakern | 0:4e088cbb2dbf | 644 | |
nirnakern | 0:4e088cbb2dbf | 645 | serialStreamThread.start(Teseo_LIV3F_UARTStreamProcess, &data); |
nirnakern | 0:4e088cbb2dbf | 646 | } |
nirnakern | 0:4e088cbb2dbf | 647 | |
nirnakern | 0:4e088cbb2dbf | 648 | void Teseo_LIV3F::stopListener(Serial *serial_debug) |
nirnakern | 0:4e088cbb2dbf | 649 | { |
nirnakern | 0:4e088cbb2dbf | 650 | if (serialStreamThread.get_state() != Thread::Running) |
nirnakern | 0:4e088cbb2dbf | 651 | return; |
nirnakern | 0:4e088cbb2dbf | 652 | serialStreamThread.terminate(); |
nirnakern | 0:4e088cbb2dbf | 653 | } |