John Lam
F742ZG
Dependencies: mbed X_NUCLEO_IKS01A2 mbed-rtos
Dependents: ros_button_2021 test test2
Diff: Teseo-LIV3F/Teseo-LIV3F.cpp
- Revision:
- 0:4e088cbb2dbf
diff -r 000000000000 -r 4e088cbb2dbf Teseo-LIV3F/Teseo-LIV3F.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Teseo-LIV3F/Teseo-LIV3F.cpp Mon Oct 23 07:11:35 2017 +0000
@@ -0,0 +1,654 @@
+/*
+ * -------------------------------------------------------------------------
+ * Copyright (C) 2017 STMicroelectronics
+ * Author: Francesco M. Virlinzi <francesco.virlinzi@st.com>
+ *
+ * May be copied or modified under the terms of the GNU General Public
+ * License V.2 ONLY. See linux/COPYING for more information.
+ *
+ * -------------------------------------------------------------------------
+ */
+#include "string.h"
+#include "Teseo-LIV3F.h"
+
+
+static char T3_name[] = "Teseo-LIV3F";
+
+char _OK[] = "OK";
+char _Failed[] = "Failed";
+char X_Nucleo_name[] = "X-Nucleo-GNSS1A1";
+
+struct teseo_cmd {
+ char *cmd;
+};
+
+static struct teseo_cmd teseo_cmds[] = {
+ [Teseo_LIV3F::GETSWVER] = {
+ .cmd = "$PSTMGETSWVER,7",
+ },
+ [Teseo_LIV3F::FORCESTANDBY] = {
+ .cmd = "$PSTMFORCESTANDBY,00007",
+ },
+ [Teseo_LIV3F::RFTESTON] = {
+ .cmd = "$PSTMRFTESTON,16",
+ },
+ [Teseo_LIV3F::RFTESTOFF] = {
+ .cmd = "$PSTMRFTESTOFF\n\r",
+ },
+ [Teseo_LIV3F::LOWPOWER] = {
+ .cmd = "$PSTMLOWPOWERONOFF,1,0,000,05,0,1,000,1,00010,01,0,0,1,01",
+ },
+ [Teseo_LIV3F::FWUPDATE] = {
+ .cmd = "$PSTMFWUPGRADE",
+ },
+};
+
+
+Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin,
+ PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin,
+ Serial *serial_debug):
+ _reset(reset_pin, 1),
+ _pps(pps_pin),
+ _wakeup(wakeup_pin, 0),
+ _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD),
+ _serial_debug(serial_debug)
+{
+ wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS);
+ _uart.baud(SDT_UART_BAUD);
+ _uart.format(8, SerialBase::None, 1);
+ _i2c = NULL;
+ _uart_interleaded = false;
+ _uart_discard = false;
+}
+
+Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin,
+ PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin,
+ I2C *bus, Serial *serial_debug):
+ _reset(reset_pin, 1),
+ _pps(pps_pin),
+ _wakeup(wakeup_pin, 0),
+ _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD),
+ _i2c(bus),
+ _serial_debug(serial_debug)
+{
+ wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS);
+ _uart.baud(SDT_UART_BAUD);
+ _uart.format(8, SerialBase::None, 1);
+ _uart_interleaded = false;
+ _uart_discard = false;
+}
+
+int Teseo_LIV3F::EnableLowPower()
+{
+ SendCommand(LOWPOWER);
+ return 0;
+}
+
+void Teseo_LIV3F::Reset(Serial *serial_debug)
+{
+ if (serial_debug)
+ serial_debug->printf("%s: Resetting...", T3_name);
+
+ _reset.write(0);
+
+ wait_ms(50);
+
+ _reset.write(1);
+
+ wait_ms(70);
+
+ if (serial_debug)
+ serial_debug->printf("Done...\n\r");
+}
+
+enum {
+ TESEO_FLASHER_IDENTIFIER = 0, // 0xBCD501F4
+ TESEO_FLASHER_SYNC, // 0x83984073
+ DEVICE_START_COMMUNICATION, // 0xA3
+ FLASHER_READY, // 0x4A
+ ACK, // 0xCC
+ NACK,
+};
+
+struct firmware_ctrl {
+ char *cmd;
+ unsigned char len;
+ char *n;
+} ;
+
+/*
+ * #define FWUPG_IDENTIFIER 0xBC D5 01 F4
+ * #define FWUPG_SYNC 0x83 98 40 73
+ */
+static struct firmware_ctrl fw_data[] = {
+ [TESEO_FLASHER_IDENTIFIER] = {
+ .cmd = (char *)(char[]){ 0xF4, 0x01, 0xD5, 0xBC},
+ .len = 4,
+ .n = "TESEO_FLASHER_IDENTIFIER",
+ },
+ [TESEO_FLASHER_SYNC] = {
+ .cmd =(char *)(char[]){ 0x73, 0x40, 0x98, 0x83 },
+ .len = 4,
+ .n = "TESEO_FLASHER_SYNC",
+ },
+ [DEVICE_START_COMMUNICATION] = {
+ .cmd = (char *)(char[]){0xA3},
+ .len = 1,
+ .n = "DEVICE_START_COMMUNICATION",
+ },
+ [FLASHER_READY] = {
+ .cmd = (char *)(char[]){0x4A},
+ .len = 1,
+ .n = "FLASHER_READY",
+ },
+ [ACK] = {
+ .cmd = (char *)(char[]){0xCC},
+ .len = 1,
+ .n = "ACK",
+ },
+ [NACK] = {
+ .cmd = (char *)(char[]){0xDD},
+ .len = 1,
+ .n = "NACK",
+ },
+};
+
+int Teseo_LIV3F::SendString(char *buf, int len)
+{
+ for (int i = 0; i < len; ++i) {
+ while (!_uart.writeable());
+ _uart.putc(buf[i]);
+ }
+}
+
+struct ImageOptions
+{
+ unsigned char eraseNVM;
+ unsigned char programOnly;
+ unsigned char reserved;
+ unsigned char baudRate;
+ unsigned int firmwareSize;
+ unsigned int firmwareCRC;
+ unsigned int nvmAddressOffset;
+ unsigned int nvmSize;
+} liv3f_img_option = {
+ .eraseNVM = 1,
+ .programOnly = 0,
+ .reserved = 0,
+ .baudRate = 1,
+ .firmwareSize = 0,
+ .firmwareCRC = 0,
+ .nvmAddressOffset = 0x00100000,
+ .nvmSize = 0x00100000,
+
+};
+
+int Teseo_LIV3F::FwWaitAck()
+{
+ while (!_uart.readable());
+
+ char c = _uart.getc();
+
+ if (fw_data[ACK].cmd[0] == c) {
+ if (_serial_debug)
+ _serial_debug->printf("%s (0x%x)\n\r", _OK, c);
+ return 0;
+ }
+
+ if (fw_data[NACK].cmd[0] == c) {
+ if (_serial_debug)
+ _serial_debug->printf("%s (%x)\n\r", _Failed, c);
+ return -1;
+ }
+
+ if (_serial_debug)
+ _serial_debug->printf("%s - Char not allowed (%x)\n\r", _Failed, c);
+ return -1;
+}
+
+bool Teseo_LIV3F::FirmwareUpdate(bool is_recovery, char *data,
+ unsigned int data_len,
+ unsigned long crc,
+ Serial *serial_debug)
+{
+ unsigned int i;
+
+ char _buf[4] = { 0xff, 0xff, 0xff, 0xff };
+
+ liv3f_img_option.firmwareSize = data_len;
+ liv3f_img_option.firmwareCRC = crc;
+
+ if (data == NULL || !data_len)
+ return false;
+
+ if (is_recovery)
+ Reset();
+
+ {
+
+ _uart.baud(FWU_UART_BAUD);
+
+#if 1
+ while (1) {
+ /* send TESEO_FLASHER_IDENTIFIER */
+/*
+ * Device is under reset. Host sends continuously “TESEO2_FLASHER_IDENTIFIER� word.
+ */
+ SendString(fw_data[TESEO_FLASHER_IDENTIFIER].cmd, fw_data[TESEO_FLASHER_IDENTIFIER].len);
+
+ /* try to read... TESEO_FLASHER_SYNC */
+ if (_uart.readable())
+ for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len; ) {
+
+ while (!_uart.readable());
+
+ _buf[i] = _uart.getc();
+
+ if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == _buf[i]) {
+ if (serial_debug)
+ serial_debug->printf("-- %d -- 0x%x -- ok--\n\r", i, _buf[i]);
+ i++;
+ goto exit_step_1; /* FMV: WA to have firmware update working.... */
+ } else {
+ i = 0;
+ }
+ }
+ if (i == fw_data[TESEO_FLASHER_SYNC].len)
+ goto exit_step_1;
+ }
+
+exit_step_1:
+
+ _uart.abort_read();
+
+ if (serial_debug)
+ serial_debug->printf("Got: %s from %s\n\r", fw_data[TESEO_FLASHER_SYNC].n, T3_name);
+
+/*
+ * Host sends “DEVICE_START_COMMUNICATION� word.
+ */
+ serial_debug->printf("\n\r%s Step: %s ",T3_name, fw_data[DEVICE_START_COMMUNICATION].n);
+ SendString(fw_data[DEVICE_START_COMMUNICATION].cmd, fw_data[DEVICE_START_COMMUNICATION].len);
+
+ FwWaitAck();
+
+/*
+ * Host sends the binary image options. Both host and
+ * device change UART baud rates. Host sends continuously
+ * the “FLASHER_READY� word.
+ */
+ if (serial_debug)
+ serial_debug->printf("%s Step: Send ImageOption\n\r",T3_name);
+
+ SendString((char*)&liv3f_img_option, sizeof(ImageOptions));
+
+ if (serial_debug)
+ serial_debug->printf("%s Step: Send %s\n\r",T3_name, fw_data[FLASHER_READY].n);
+
+ while (1) {
+ SendString(fw_data[FLASHER_READY].cmd, fw_data[FLASHER_READY].len);
+ if (_uart.readable())
+ goto exit_step_3;
+ }
+
+ exit_step_3:
+ FwWaitAck();
+
+ if (serial_debug)
+ serial_debug->printf("%s Step: Erasing flash area ",T3_name);
+
+
+ FwWaitAck();
+
+/*
+ * Device is erasing flash program. Host is waiting for an “ACK�.
+ */
+
+ if (serial_debug)
+ serial_debug->printf("%s Step: Erasing NVM ",T3_name);
+
+ while (!_uart.readable());
+
+ FwWaitAck();
+
+ if (serial_debug)
+ serial_debug->printf("%s Step: Sending data... ",T3_name);
+
+ for (i = 0; i < (data_len / (16*1024)); ++i) {
+ SendString(&data[i], 16*1024);
+
+ FwWaitAck();
+ }
+
+ serial_debug->printf("\n\r");
+ /*
+ * send remaining data...
+ */
+ if (data_len != (i * 16*1024)) {
+ SendString(&data[i*16*1024], data_len-i*16*1024);
+ FwWaitAck();
+ }
+ /*
+ * wait CRC ack
+ */
+ FwWaitAck();
+ }
+
+#else
+ //_uart.format(8, SerialBase::Forced0, 1);
+ while (1)
+ {
+ /* send TESEO_FLASHER_IDENTIFIER */
+ for (i = 0; i < fw_data[TESEO_FLASHER_IDENTIFIER].len; ++i) {
+ while (!_uart.writeable());
+ _uart.putc(fw_data[TESEO_FLASHER_IDENTIFIER].cmd[i]);
+ }
+
+
+ /* try to read... TESEO_FLASHER_SYNC */
+ //while (!_uart.readable());
+ for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len && _uart.readable(); )
+ if (_uart.readable())
+ {
+ char c = _uart.getc();
+ /*
+ if (!c)
+ break;
+ */
+ if (serial_debug)
+ serial_debug->printf("%x vs %x\n\r", c, fw_data[TESEO_FLASHER_SYNC].cmd[i]);
+
+ if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == c)
+ i++;
+ else
+ i = 0;
+ }
+
+ if (i == fw_data[TESEO_FLASHER_SYNC].len && serial_debug)
+ serial_debug->printf("Got %s from %s\n\r",fw_data[TESEO_FLASHER_SYNC].n, T3_name);
+ }
+ }
+#endif
+ if (serial_debug)
+ serial_debug->printf("END\n\r");
+
+ return true;
+}
+
+int Teseo_LIV3F::WakeUp()
+{
+ wait_ms(100);
+
+ _wakeup.write(1);
+
+ wait_ms(500);
+
+ _wakeup.write(0);
+
+ return 0;
+}
+
+bool Teseo_LIV3F::CheckPPSWorking()
+{
+ int val_0, val_1;
+
+ wait_ms(500);
+
+ val_0 = _pps.read();
+
+ wait_ms(500);
+ val_1 = _pps.read();
+
+ if (val_0 != val_1)
+ return true;
+
+ return false;
+}
+
+int Teseo_LIV3F::CRC_(char *buf, int size)
+{
+ int i = 0, ch = 0;
+
+ if (buf[0] == '$')
+ ++i;
+
+ if (size)
+ for (; i < size; ++i)
+ ch ^= buf[i];
+ else
+ for (; buf[i] != 0; ++i)
+ ch ^= buf[i];
+
+ return ch;
+}
+
+bool Teseo_LIV3F::WaitBooting(Timer *t, float timeout)
+{
+ unsigned int now = t->read_ms();;
+ while (1) {
+ if (CheckPPSWorking() == true)
+ return true;
+
+ if ((now + timeout*1000) < t->read_ms())
+ break;
+ }
+
+ return false;
+}
+
+void Teseo_LIV3F::SendCommand(enum Teseo_LIV3F::cmd_enum c)
+{
+ char crc[5];
+
+ sprintf(crc, "*%02X\n\r", CRC_(teseo_cmds[c].cmd, 0));
+
+ _uart_mutex_lock();
+ _uart_interleaded = true;
+ SendString(teseo_cmds[c].cmd, strlen(teseo_cmds[c].cmd));
+ SendString(crc, 5);
+ _uart_mutex_unlock();
+}
+
+char *Teseo_LIV3F::DetectSentence(const char *cmd, char *buf, unsigned long len)
+{
+ char *result = NULL;
+ unsigned int i = 0;
+ const unsigned long cmd_len = strlen(cmd);
+ len -= strlen(cmd);
+
+ while (!result && i < len) {
+ for (; buf[i] != '$' && i < len; ++i); /* 1. check '$' char */
+ if (i == len)
+ break; /* no more char.... */
+
+ ++i; /* to point to the char after '$' */
+
+ if (strncmp(&buf[i], cmd, cmd_len) == 0) {
+ result = &buf[i];
+ }
+ }
+
+ if (result) {
+ for (i = 0; result[i] != '*'; ++i);
+ result[i] = 0;
+ }
+#if 0
+ if (_serial_debug)
+ _serial_debug->printf("%s: %s: %s %s FOUND\n\r", T3_name, __FUNCTION__, cmd, result ? " " : "NOT");
+#endif
+ return result;
+}
+
+
+int Teseo_LIV3F::CheckI2C()
+{
+
+ if (!_i2c)
+ return -1;
+
+ _i2c->start();
+ int res = _i2c->write((TESEO_I2C_ADDRESS << 1) | 1);
+ _i2c->stop();
+ /*
+ * @returns
+ * '0' - NAK was received
+ * '1' - ACK was received,
+ * '2' - timeout
+ */
+ return res == 1 ? 0 : -1;
+}
+
+ int Teseo_LIV3F::ReadMessage(char *buf, unsigned long len, Timer *t, float timeout)
+{
+ memset(buf, 0, len);
+
+ for (unsigned int i = 0; i < len; ++i){
+ if (t) {
+ unsigned int now = t->read_ms();;
+ while (!_uart.readable() && (now + timeout*1000) > t->read_ms());
+ } else
+ while (!_uart.readable());
+ if (_uart.readable())
+ buf[i] = _uart.getc();;
+ }
+#if 0
+ if (_serial_debug) {
+ unsigned int i;
+ _serial_debug->printf("\n\r---------------------\n\r");
+ for (i = 0; i < len ; ++i)
+ _serial_debug->putc((int)buf[i]);
+ _serial_debug->printf("\n\r---------------------\n\r");
+ }
+#endif
+ return 0;
+}
+
+ void Teseo_LIV3F::RFTest(bool enable)
+ {
+ if (enable)
+ SendCommand(Teseo_LIV3F::RFTESTON);
+ else
+ SendCommand(Teseo_LIV3F::RFTESTOFF);
+ }
+
+void Teseo_LIV3F::ReadLoop(Serial *serial_debug)
+{
+ while (1)
+ if (_uart.readable()) {
+ int c = _uart.getc();
+ serial_debug->putc(c);
+ }
+}
+
+char *Teseo_LIV3F::ReadSentence(const char *cmd, char *buf, unsigned long len)
+{
+ int ret = ReadMessage(buf, len);
+ if (ret)
+ return NULL;
+ return DetectSentence(cmd, buf, len);
+}
+
+struct ___msg {
+ unsigned char len;
+ char *str;
+};
+
+
+static const struct ___msg teseo_msgs[] = {
+ [ NMEA_GPGGA ] = { .len = 5, .str = "GPGGA", },
+ [ NMEA_GPGLL ] = { .len = 5, .str = "GPGLL", },
+ [ NMEA_GNGSA ] = { .len = 5, .str = "GNGSA", },
+ [ NMEA_GPTXT ] = { .len = 5, .str = "GPTXT", },
+ [ NMEA_GPVTG ] = { .len = 5, .str = "GPVTG", },
+ [ NMEA_GPRMC ] = { .len = 5, .str = "GPRMC", },
+ [ NMEA_PSTMCPU ] = { .len = 7, .str = "PSTMCPU", },
+ [ NMEA_PSTMVER ] = { .len = 7, .str = "PSTMVER", },
+};
+
+
+enum nmea_msg_id Teseo_LIV3F::MsgDetect(char *buf, int buf_len, Serial *serial_debug)
+{
+ int i;
+
+ if (buf[0] == '$')
+ ++buf;
+
+ for (i = 0; i < NMEA_END__; ++i)
+ if (memcmp((void*)teseo_msgs[i].str, (void*)buf, teseo_msgs[i].len) == 0)
+ return (enum nmea_msg_id) i;
+#if 0
+ if (serial_debug) {
+ serial_debug->puts("MESSAGE NOT FOUND: ");
+ for (int i = 0; i < 5; ++i)
+ serial_debug->putc(lbuf[i]);
+ serial_debug->puts("\n\r");
+ }
+#endif
+ return NMEA_END__;
+}
+
+void Teseo_LIV3F::UARTStreamProcess(Serial *serial_debug)
+{
+ enum nmea_msg_id id;
+ char c;
+
+ struct teseo_msg *msg = mpool.alloc();
+ msg->len = 0;
+
+ while (true) {
+ _uart_mutex_lock();
+#if 0
+ if (_uart_interleaded == true) {
+ msg->len = 0;
+ _uart_interleaded = false;
+ _uart_discard = true;
+ }
+#endif
+ if (_uart.readable()) {
+ c = _uart.getc();
+ _uart_mutex_unlock();
+ if (c == '$') {
+ queue.put(msg);
+ msg = mpool.alloc();
+ msg->len = 0;
+ _uart_discard = false;
+ }
+ if (!_uart_discard)
+ msg->buf[msg->len++] = c;
+ } else {
+ _uart_mutex_unlock();
+ wait_us(100);
+ }
+ }
+}
+
+struct thr_data {
+ Teseo_LIV3F *gnss;
+ Serial *serial_debug;
+};
+
+static void Teseo_LIV3F_UARTStreamProcess(struct thr_data *data)
+{
+ data->gnss->UARTStreamProcess(data->serial_debug);
+}
+
+void Teseo_LIV3F::startListener(Serial *serial_debug)
+{
+ if (serialStreamThread.get_state() == Thread::Running)
+ return;
+
+ static struct thr_data data = {
+ .gnss = this,
+ .serial_debug = serial_debug,
+ };
+
+ serialStreamThread.start(Teseo_LIV3F_UARTStreamProcess, &data);
+}
+
+void Teseo_LIV3F::stopListener(Serial *serial_debug)
+{
+ if (serialStreamThread.get_state() != Thread::Running)
+ return;
+ serialStreamThread.terminate();
+}
+