Simone Mentasti / Mbed 2 deprecated AcquisizioneGPS

Acquisition for GNSS1A1

Dependencies:   F7_Ethernet X_NUCLEO_IKS01A2 mbed-rtos mbed

Fork of Test2Boards by Simone Mentasti

Teseo-LIV3F/Teseo-LIV3F.cpp@3:0fb5321d73fb, 2018-10-01 (annotated)

Committer:
nirnakern
Date:
Mon Oct 01 14:33:56 2018 +0000
Revision:
3:0fb5321d73fb
Parent:
0:4e088cbb2dbf 
First Commit;

Who changed what in which revision?

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