From 3342f9d217b2479ee262e9883bafd593b314144b Mon Sep 17 00:00:00 2001 From: Bernardo Gabriel Date: Fri, 1 Sep 2023 16:48:46 +0100 Subject: [PATCH 1/6] Drivers/CrawlerSerial: converted this task into BasicDeviceDriver, not using CheckSum at the moment. --- src/Drivers/CrawlerSerial/Driver.hpp | 165 ------------ src/Drivers/CrawlerSerial/Reader.hpp | 124 +++++++++ src/Drivers/CrawlerSerial/Task.cpp | 379 ++++++++++++++++++--------- 3 files changed, 384 insertions(+), 284 deletions(-) delete mode 100644 src/Drivers/CrawlerSerial/Driver.hpp create mode 100644 src/Drivers/CrawlerSerial/Reader.hpp diff --git a/src/Drivers/CrawlerSerial/Driver.hpp b/src/Drivers/CrawlerSerial/Driver.hpp deleted file mode 100644 index bd6936d8a8..0000000000 --- a/src/Drivers/CrawlerSerial/Driver.hpp +++ /dev/null @@ -1,165 +0,0 @@ -#ifndef CRAWLERSERIAL_HPP_INCLUDED_ -#define CRAWLERSERIAL_HPP_INCLUDED_ - -#include - -namespace Drivers -{ - using DUNE_NAMESPACES; - class DriverCrawlerSerial - { - public: - struct CrawlerData - { - std::string firmVersion; - uint16_t pressure; - - bool state_new_data[1]; - }; - - SerialPort *m_uart; - Poll m_poll; - - DriverCrawlerSerial(DUNE::Tasks::Task *task, SerialPort *uart, Poll poll) : m_task(task) - { - m_uart = uart; - m_poll = poll; - m_timeout_uart = 1.0f; - resetStateNewData(); - } - - ~DriverCrawlerSerial() {} - - void - resetStateNewData(void) - { - for (uint8_t t = 0; t < 1; t++) - m_crawlerData.state_new_data[t] = false; - } - - bool - getVersionFirmware() - { - if (sendCommand("@VERS,*", "$VERS,")) - return true; - - return false; - } - - bool - startAcquisition() - { - if (sendCommand("@START,*", "$RSP,ACK,*")) - return true; - - return false; - } - - bool - stopAcquisition() - { - if (sendCommand("@STOP,*", "$STOP,*")) - return true; - - return false; - } - - bool - haveNewData() - { - std::size_t rv = m_uart->readString(bfr, sizeof(bfr)); - - if (rv == 0) - { - m_task->err(DTR("I/O error")); - return false; - } - - bfr[strlen(bfr) - 3] = '\0'; - - char *param = std::strtok(bfr, ","); - if (std::strcmp(param, "$PRESS") == 0) - { - param = std::strtok(NULL, ","); - uint16_t pressure = atoi(param); - if (pressure == 0) - m_task->war("Pressure sensor is disconected."); - else - { - m_crawlerData.pressure = pressure; - m_task->debug("Pressure: %d mBar", m_crawlerData.pressure); - m_crawlerData.state_new_data[0] = true; - } - } - - bool result = true; - for (uint8_t t = 0; t < 1; t++) - { - if (m_crawlerData.state_new_data[t] == false) - result = false; - } - - return result; - } - - void - sendCommandNoRsp(const char *cmd) - { - char cmdText[32]; - std::sprintf(cmdText, "%s%c\n", cmd, (Algorithms::XORChecksum::compute((uint8_t *)cmd, strlen(cmd) - 1) | 0x80)); - // std::sprintf(cmdText, "%s\n", cmd); - m_task->inf("Command (no rsp): %s", cmdText); - m_uart->writeString(cmdText); - } - - bool - sendCommand(const char *send, const char *reply) - { - char cmdText[32]; - char cmdReplyText[32]; - std::sprintf(cmdText, "%s%c\n", send, (Algorithms::XORChecksum::compute((uint8_t *)send, strlen(send) - 1) | 0x80)); - std::sprintf(cmdReplyText, "%s%c\n", reply, (Algorithms::XORChecksum::compute((uint8_t *)reply, strlen(reply) - 1) | 0x80)); - /* std::sprintf(cmdText, "%s\n", send); - std::sprintf(cmdReplyText, "%s\n", reply); */ - char bfrUart[128]; - m_task->inf("Command: %s", cmdText); - - m_uart->writeString(cmdText); - - if (Poll::poll(*m_uart, m_timeout_uart)) - { - m_uart->readString(bfrUart, sizeof(bfrUart)); - m_task->inf("Reply: %s", bfrUart); - if (std::strcmp(bfrUart, cmdReplyText) == 0) - { - return true; - } - else if (std::strcmp(reply, "$VERS,") == 0) - { - char *vrs = std::strtok(bfrUart, ","); - vrs = std::strtok(NULL, ","); - m_crawlerData.firmVersion = vrs; - return true; - } - } - - return false; - } - - std::string - getFirmwareVersion(void) - { - return m_crawlerData.firmVersion; - } - - CrawlerData m_crawlerData; - - private: - DUNE::Tasks::Task *m_task; - float m_timeout_uart; - char bfr[64]; - }; - -} - -#endif \ No newline at end of file diff --git a/src/Drivers/CrawlerSerial/Reader.hpp b/src/Drivers/CrawlerSerial/Reader.hpp new file mode 100644 index 0000000000..4221e12f9a --- /dev/null +++ b/src/Drivers/CrawlerSerial/Reader.hpp @@ -0,0 +1,124 @@ +//*************************************************************************** +// Copyright 2007-2022 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Ricardo Martins * +//*************************************************************************** + +#ifndef DRIVERS_CRAWLERSERIAL_READER_HPP_INCLUDED_ +#define DRIVERS_CRAWLERSERIAL_READER_HPP_INCLUDED_ + +// DUNE headers. +#include + +namespace Drivers +{ + namespace CrawlerSerial + { + using DUNE_NAMESPACES; + + //! Read buffer size. + static const size_t c_read_buffer_size = 4096; + //! Line termination character. + static const char c_line_term = '\n'; + + class Reader: public Concurrency::Thread + { + public: + //! Constructor. + //! @param[in] task parent task. + //! @param[in] handle I/O handle. + Reader(Tasks::Task* task, IO::Handle* handle): + m_task(task), + m_handle(handle) + { + m_buffer.resize(c_read_buffer_size); + } + + private: + //! Parent task. + Tasks::Task* m_task; + //! I/O handle. + IO::Handle* m_handle; + //! Internal read buffer. + std::vector m_buffer; + //! Current line. + std::string m_line; + + void + dispatch(IMC::Message& msg) + { + msg.setDestination(m_task->getSystemId()); + msg.setDestinationEntity(m_task->getEntityId()); + m_task->dispatch(msg, DF_LOOP_BACK); + } + + void + read(void) + { + if (!Poll::poll(*m_handle, 1.0)) + return; + + size_t rv = m_handle->read(&m_buffer[0], m_buffer.size()); + if (rv == 0) + throw std::runtime_error(DTR("invalid read size")); + + for (size_t i = 0; i < rv; ++i) + { + m_line.push_back(m_buffer[i]); + if (m_buffer[i] == c_line_term) + { + IMC::DevDataText line; + line.value = m_line; + dispatch(line); + m_line.clear(); + } + } + } + + void + run(void) + { + while (!isStopping()) + { + try + { + read(); + } + catch (std::runtime_error& e) + { + IMC::IoEvent evt; + evt.type = IMC::IoEvent::IOV_TYPE_INPUT_ERROR; + evt.error = e.what(); + dispatch(evt); + break; + } + } + } + }; + } +} + +#endif diff --git a/src/Drivers/CrawlerSerial/Task.cpp b/src/Drivers/CrawlerSerial/Task.cpp index 6aa16038f0..a2fa8d8534 100644 --- a/src/Drivers/CrawlerSerial/Task.cpp +++ b/src/Drivers/CrawlerSerial/Task.cpp @@ -1,5 +1,5 @@ //*************************************************************************** -// Copyright 2007-2022 Universidade do Porto - Faculdade de Engenharia * +// Copyright 2007-2023 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * @@ -24,163 +24,262 @@ // https://github.com/LSTS/dune/blob/master/LICENCE.md and * // http://ec.europa.eu/idabc/eupl.html. * //*************************************************************************** -// Author: João Bogas * +// Author: Ricardo Martins (GPS driver) * +// Author: Luis Venancio (BasicDeviceDriver compatibility) * +// Author: João Bogas (adaptation to Devices/CrawlerSerial) * +// Author: Bernardo Gabriel (adaptation to Devices/CrawlerSerial) * //*************************************************************************** +// ISO C++ 98 headers. +#include +#include +#include + // DUNE headers. #include -// Local headers -#include "Driver.hpp" +// Local headers. +#include "Reader.hpp" namespace Drivers { - //! Insert short task description here. + //! The task will receive Pressure data and request a PWM signal. //! - //! Insert explanation on task behaviour here. + //! The task will communicate with a Raspberry Pi Pico via SerialPort. //! @author João Bogas + //! @author Bernardo Gabriel namespace CrawlerSerial { using DUNE_NAMESPACES; + //! Maximum number of initialization commands. + static const unsigned c_max_init_cmds = 5; + //! Timeout for waitReply() function. + static const float c_wait_reply_tout = 4.0; + struct Arguments { - //! Serial port device. - std::string uart_dev; - //! Serial port baud rate. - unsigned uart_baud; - //! Input timeout. - double input_timeout; - //! Number of attempts before error - int number_attempts; + //! IO device (URI). + std::string io_dev; + //! Input timeout in seconds. + float inp_tout; + //! Initialization commands. + std::string init_cmds[c_max_init_cmds]; + //! Initialization replies. + std::string init_rpls[c_max_init_cmds]; }; - struct Task: public DUNE::Tasks::Task + struct Task : public Hardware::BasicDeviceDriver { - //! Serial port handle - SerialPort *m_uart; - //! I/O Multiplexer - Poll m_poll; - //! Task arguments + //! Serial port handle. + IO::Handle *m_handle; + //! Task arguments. Arguments m_args; - //! Driver for CrawlerSerial - DriverCrawlerSerial *m_driver; - //! Timer - Counter m_wdog; + //! Input watchdog. + Time::Counter m_wdog; + //! Reader thread. + Reader *m_reader; //! IMC msg IMC::Pressure m_press; - //! Read timestamp. - double m_tstamp; - //! Count for attempts - int m_count_attempts; - //! Flag to control reset of board - bool m_is_first_reset; + //! Buffer forEntityState + char m_bufer_entity[64]; + //! Last line while waiting for reply. + std::string m_reply_line; + //! Wait for reply + bool m_wait_reply = false; //! Constructor. //! @param[in] name task name. //! @param[in] ctx context. - Task(const std::string& name, Tasks::Context& ctx): - DUNE::Tasks::Task(name, ctx) + Task(const std::string &name, Tasks::Context &ctx) : Hardware::BasicDeviceDriver(name, ctx), + m_handle(NULL), + m_reader(NULL) { - param("Serial Port - Device", m_args.uart_dev) - .defaultValue("") - .description("Serial port device"); + // Define configuration parameters. + paramActive(Tasks::Parameter::SCOPE_GLOBAL, + Tasks::Parameter::VISIBILITY_DEVELOPER, + true); - param("Serial Port - Baud Rate", m_args.uart_baud) + param("IO Port - Device", m_args.io_dev) .defaultValue("") - .description("Serial port baud rate"); + .description("IO device URI in the form \"uart://DEVICE:BAUD\""); - param("Input Timeout", m_args.input_timeout) - .defaultValue("3.0") - .minimumValue("2.0") - .maximumValue("4.0") + param("Input Timeout", m_args.inp_tout) .units(Units::Second) - .description("Amount of seconds to wait for data before reporting an error"); + .defaultValue("4.0") + .minimumValue("0.0") + .description("Input timeout"); + for (unsigned i = 0; i < c_max_init_cmds; ++i) + { + std::string cmd_label = String::str("Initialization String %u - Command", i); + param(cmd_label, m_args.init_cmds[i]) + .defaultValue(""); + + std::string rpl_label = String::str("Initialization String %u - Reply", i); + param(rpl_label, m_args.init_rpls[i]) + .defaultValue(""); + } + + // Use wait for messages + setWaitForMessages(1.0); + + // Initialize messages. + clearMessages(); + + bind(this); + bind(this); bind(this); } + ~Task() override + { + onDisconnect(); + } + //! Update internal state with new parameter values. void onUpdateParameters(void) { } - //! Reserve entity identifiers. - void - onEntityReservation(void) + //! Try to connect to the device. + //! @return true if connection was established, false otherwise. + bool + onConnect() override { + try + { + m_handle = openDeviceHandle(m_args.io_dev); + m_reader = new Reader(this, m_handle); + m_reader->start(); + setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_ACTIVATING); + return true; + } + catch (...) + { + throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); + } + + return false; } - //! Resolve entity names. + //! Disconnect from device. void - onEntityResolution(void) + onDisconnect() override { + if (m_reader != NULL) + { + m_reader->stopAndJoin(); + Memory::clear(m_reader); + } + + Memory::clear(m_handle); } - //! Acquire resources. + //! Initialize device. void - onResourceAcquisition(void) + onInitializeDevice() override { - setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT); - try - { - m_uart = new SerialPort(m_args.uart_dev, m_args.uart_baud); - m_uart->setCanonicalInput(true); - m_uart->flush(); - m_poll.add(*m_uart); - m_driver = new DriverCrawlerSerial(this, m_uart, m_poll); - } - catch (const std::runtime_error &e) + for (unsigned i = 0; i < c_max_init_cmds; ++i) { - throw RestartNeeded(e.what(), 10); + if (m_args.init_cmds[i].empty()) + continue; + + if (m_args.init_rpls[i].empty()) + sendCommand(m_args.init_cmds[i].c_str()); + else if (!sendCommand(String::unescape(m_args.init_cmds[i]).c_str(), String::unescape(m_args.init_rpls[i]).c_str())) + { + err("%s: %s", DTR("no reply to command"), String::unescape(m_args.init_cmds[i]).c_str()); + throw std::runtime_error(DTR("failed to setup device")); + } } + + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); + + // m_wdog.setTop(m_args.inp_tout); } - //! Initialize resources. void - onResourceInitialization(void) + clearMessages(void) { - m_driver->stopAcquisition(); - m_uart->flush(); - Delay::wait(1.0f); - initBoard(); - m_wdog.setTop(m_args.input_timeout); - m_wdog.reset(); + m_press.clear(); } - //! Release resources. - void - onResourceRelease(void) + //! Wait reply to command. + //! @param[in] stn string to compare. + //! @return true on successful match, false otherwise. + bool + waitReply(const std::string &stn) { - if (m_uart != nullptr) + Counter counter(c_wait_reply_tout); + while (!stopping() && !counter.overflow()) { - m_poll.remove(*m_uart); - Memory::clear(m_driver); - Memory::clear(m_uart); + waitForMessages(counter.getRemaining()); + + if (m_reply_line == stn) + { + m_reply_line.clear(); + m_wait_reply = false; + return true; + } } + + return false; } void - initBoard() + consume(const IMC::DevDataText *msg) { - m_driver->stopAcquisition(); + if (msg->getDestination() != getSystemId()) + return; - if (!m_driver->getVersionFirmware()) - { - setEntityState(IMC::EntityState::ESTA_NORMAL, Utils::String::str(DTR("trying connecting to board"))); - war(DTR("failed to get firmware version")); - } + if (msg->getDestinationEntity() != getEntityId()) + return; - if (!m_driver->startAcquisition()) - { - setEntityState(IMC::EntityState::ESTA_NORMAL, Utils::String::str(DTR("trying connecting to board"))); - war(DTR("failed to start")); - } + trace("%s", sanitize(msg->value).c_str()); - debug("Init OK"); - m_wdog.setTop(m_args.input_timeout); - m_wdog.reset(); + if (m_wait_reply) + m_reply_line = msg->value; + else + processSentence(msg->value); + } + + void + consume(const IMC::IoEvent *msg) + { + if (msg->getDestination() != getSystemId()) + return; + + if (msg->getDestinationEntity() != getEntityId()) + return; + + if (msg->type == IMC::IoEvent::IOV_TYPE_INPUT_ERROR) + throw RestartNeeded(msg->error, 5); + } + + //! Send a command via Serial Port. + //! @param cmd command to send. + void + sendCommand(const char *cmd) + { + char cmdText[32]; + // std::sprintf(cmdText, "%s%c\n", cmd, (Algorithms::XORChecksum::compute((uint8_t *)cmd, strlen(cmd) - 1) | 0x80)); + std::sprintf(cmdText, "%s", cmd); + inf("Command: %s", String::unescape(cmdText).c_str()); + m_handle->writeString(cmdText); + } + + //! Send a command via Serial Port and wait for a specific reply. + //! @param cmd command to send. + //! @param reply message to wait for. + //! @return true if succes, false otherwise + bool + sendCommand(const char *cmd, const char *reply) + { + sendCommand(cmd); + m_wait_reply = true; + return waitReply(reply); } void @@ -197,49 +296,91 @@ namespace Drivers set_light.id = 0; set_light.value = ((light_val - (-127.0)) / (127.0 - (-127.0)) * (100)); - std::string send = String::str("@PWM,%d,*", (int) set_light.value); - m_driver->sendCommandNoRsp(send.c_str()); + std::string send = String::str("@PWM,%d,*", (int)set_light.value); + sendCommand(send.c_str()); } } + //! Process sentence. + //! @param[in] line line. void - dispatchData() + processSentence(const std::string &line) { - m_tstamp = Clock::getSinceEpoch(); + // Discard leading noise. + size_t sidx = 0; + for (sidx = 0; sidx < line.size(); ++sidx) + { + if (line[sidx] == '$') + break; + } - m_press.setTimeStamp(m_tstamp); - m_press.value = m_driver->m_crawlerData.pressure; - dispatch(m_press, DF_KEEP_TIME); + // Discard trailing noise. + size_t eidx = 0; + for (eidx = line.size() - 1; eidx > sidx; --eidx) + { + if (line[eidx] == '*') + break; + } + + if (sidx >= eidx) + return; + + // // Compute checksum. + // uint8_t ccsum = 0; + // for (size_t i = sidx + 1; i < eidx; ++i) + // ccsum ^= line[i]; + + // // Validate checksum. + // unsigned rcsum = 0; + // if (std::sscanf(&line[0] + eidx + 1, "%02X", &rcsum) != 1) + // { + // trace("No checksum found, will not parse sentence."); + // return; + // } + + // if (ccsum != rcsum) + // { + // trace("Checksum field does not match computed checksum, will not " + // "parse sentence."); + // return; + // } + + // Split sentence + std::vector parts; + String::split(line.substr(sidx + 1, eidx - sidx - 1), ",", parts); + + interpretSentence(parts); } - //! Main loop. + //! Interpret given sentence. + //! @param[in] parts vector of strings from sentence. void - onMain(void) + interpretSentence(std::vector &parts) { - while (!stopping()) + if (parts[0] == "$PWM") { - waitForMessages(1.0); - - if (m_wdog.overflow()) - { - inf("Timer overflow"); - throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 10); - initBoard(); - } - - if (!Poll::poll(*m_uart, m_args.input_timeout)) - continue; + clearMessages(); + m_press.setTimeStamp(); + m_press.value = atof(parts[1].c_str()); + dispatch(m_press, DF_KEEP_TIME); + } - if (m_driver->haveNewData()) - { - dispatchData(); - m_wdog.reset(); - } + m_wdog.reset(); + } - m_uart->flush(); - } + //! Check for input timeout. + //! Data is read in the DevDataText consume. + //! @return true. + bool + onReadData() override + { + // if (m_wdog.overflow()) + // { + // setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); + // throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); + // } - m_driver->stopAcquisition(); + return true; } }; } From b4f4cd272c48dfbad1dcbc44c6116d1057dd95d0 Mon Sep 17 00:00:00 2001 From: Bernardo Gabriel Date: Mon, 4 Sep 2023 11:45:57 +0100 Subject: [PATCH 2/6] Drivers/CrawlerSerial: added Checksum validation. --- src/Drivers/CrawlerSerial/Task.cpp | 71 ++++++++++++++++++------------ 1 file changed, 42 insertions(+), 29 deletions(-) diff --git a/src/Drivers/CrawlerSerial/Task.cpp b/src/Drivers/CrawlerSerial/Task.cpp index a2fa8d8534..d31d9ec7fa 100644 --- a/src/Drivers/CrawlerSerial/Task.cpp +++ b/src/Drivers/CrawlerSerial/Task.cpp @@ -168,6 +168,8 @@ namespace Drivers void onDisconnect() override { + sendCommand("@STOP,*"); + if (m_reader != NULL) { m_reader->stopAndJoin(); @@ -197,7 +199,7 @@ namespace Drivers setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); - // m_wdog.setTop(m_args.inp_tout); + m_wdog.setTop(m_args.inp_tout); } void @@ -217,7 +219,17 @@ namespace Drivers { waitForMessages(counter.getRemaining()); - if (m_reply_line == stn) + std::string line = m_reply_line; + unsigned line_len = line.length(); + + //! + if (line_len < 2) + continue; + + //! Remove Checksum and '\n' characters + line.erase(line.length() - 2); + + if (line == stn) { m_reply_line.clear(); m_wait_reply = false; @@ -264,9 +276,9 @@ namespace Drivers sendCommand(const char *cmd) { char cmdText[32]; - // std::sprintf(cmdText, "%s%c\n", cmd, (Algorithms::XORChecksum::compute((uint8_t *)cmd, strlen(cmd) - 1) | 0x80)); - std::sprintf(cmdText, "%s", cmd); - inf("Command: %s", String::unescape(cmdText).c_str()); + std::sprintf(cmdText, "%s%c\n", cmd, (Algorithms::XORChecksum::compute((uint8_t *)cmd, strlen(cmd) - 1) | 0x80)); + // std::sprintf(cmdText, "%s\n", cmd); + trace("Command: %s", sanitize(cmdText).c_str()); m_handle->writeString(cmdText); } @@ -325,25 +337,26 @@ namespace Drivers if (sidx >= eidx) return; - // // Compute checksum. - // uint8_t ccsum = 0; - // for (size_t i = sidx + 1; i < eidx; ++i) - // ccsum ^= line[i]; - - // // Validate checksum. - // unsigned rcsum = 0; - // if (std::sscanf(&line[0] + eidx + 1, "%02X", &rcsum) != 1) - // { - // trace("No checksum found, will not parse sentence."); - // return; - // } - - // if (ccsum != rcsum) - // { - // trace("Checksum field does not match computed checksum, will not " - // "parse sentence."); - // return; - // } + unsigned line_len = line.length(); + if (line_len < 3) + { + trace("No checksum found, will not parse sentence."); + return; + } + + // Compute checksum. + unsigned ccsum = Algorithms::XORChecksum::compute((uint8_t *)&line[0], line_len - 3) | 0x80; + + // Validate checksum. + unsigned rcsum = 0; + rcsum = (uint8_t) line[line_len-2]; + + if (ccsum != rcsum) + { + trace("Checksum field does not match computed checksum, will not " + "parse sentence."); + return; + } // Split sentence std::vector parts; @@ -374,11 +387,11 @@ namespace Drivers bool onReadData() override { - // if (m_wdog.overflow()) - // { - // setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); - // throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); - // } + if (m_wdog.overflow()) + { + setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); + throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); + } return true; } From f5eff258ce64d047f2ca17789f4431ba8779d15d Mon Sep 17 00:00:00 2001 From: Bernardo Gabriel Date: Mon, 4 Sep 2023 11:47:16 +0100 Subject: [PATCH 3/6] Config: changes for the new Drivers/CrawlerSerial task. --- etc/crawler.ini | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/etc/crawler.ini b/etc/crawler.ini index d01142aa5b..bf7e633e46 100644 --- a/etc/crawler.ini +++ b/etc/crawler.ini @@ -55,10 +55,16 @@ Underwater Depth Threshold = 0.3 #TO BE DONE [Drivers.CrawlerSerial] Enabled = Always Entity Label = CrawlerSerial -Debug Level = Spew -Serial Port - Device = /dev/ttyPICO -Serial Port - Baud Rate = 115200 -Input Timeout = 3.0 +Debug Level = None +IO Port - Device = uart:///dev/ttyPICO:115200 +Input Timeout = 5.0 +Activation Time = 20.0 +Initialization String 0 - Command = @STOP,* +Initialization String 1 - Command = @VERS,* +Initialization String 2 - Command = @START,* +Initialization String 0 - Reply = $STOP,* +Initialization String 1 - Reply = $VERS,1.0.0,* +Initialization String 2 - Reply = $RSP,ACK,* [Transports.Announce] #Ignored Interfaces = eth0:prv From 946238d8fdf7df36468c9f3ffa9bdb0af453120e Mon Sep 17 00:00:00 2001 From: Bernardo Gabriel Date: Thu, 9 Nov 2023 14:48:01 +0000 Subject: [PATCH 4/6] Drivers/CLP: update task name and fixed error in parser. --- src/Drivers/{CrawlerSerial => CLP}/Reader.hpp | 6 +++--- src/Drivers/{CrawlerSerial => CLP}/Task.cmake | 0 src/Drivers/{CrawlerSerial => CLP}/Task.cpp | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) rename src/Drivers/{CrawlerSerial => CLP}/Reader.hpp (97%) rename src/Drivers/{CrawlerSerial => CLP}/Task.cmake (100%) rename src/Drivers/{CrawlerSerial => CLP}/Task.cpp (99%) diff --git a/src/Drivers/CrawlerSerial/Reader.hpp b/src/Drivers/CLP/Reader.hpp similarity index 97% rename from src/Drivers/CrawlerSerial/Reader.hpp rename to src/Drivers/CLP/Reader.hpp index 4221e12f9a..19ea6eacdf 100644 --- a/src/Drivers/CrawlerSerial/Reader.hpp +++ b/src/Drivers/CLP/Reader.hpp @@ -27,15 +27,15 @@ // Author: Ricardo Martins * //*************************************************************************** -#ifndef DRIVERS_CRAWLERSERIAL_READER_HPP_INCLUDED_ -#define DRIVERS_CRAWLERSERIAL_READER_HPP_INCLUDED_ +#ifndef DRIVERS_CLP_READER_HPP_INCLUDED_ +#define DRIVERS_CLP_READER_HPP_INCLUDED_ // DUNE headers. #include namespace Drivers { - namespace CrawlerSerial + namespace CLP { using DUNE_NAMESPACES; diff --git a/src/Drivers/CrawlerSerial/Task.cmake b/src/Drivers/CLP/Task.cmake similarity index 100% rename from src/Drivers/CrawlerSerial/Task.cmake rename to src/Drivers/CLP/Task.cmake diff --git a/src/Drivers/CrawlerSerial/Task.cpp b/src/Drivers/CLP/Task.cpp similarity index 99% rename from src/Drivers/CrawlerSerial/Task.cpp rename to src/Drivers/CLP/Task.cpp index d31d9ec7fa..42c101434e 100644 --- a/src/Drivers/CrawlerSerial/Task.cpp +++ b/src/Drivers/CLP/Task.cpp @@ -43,12 +43,14 @@ namespace Drivers { + //! Driver for CLP - Crawler's Light control and Pressure sensor. + //! //! The task will receive Pressure data and request a PWM signal. //! //! The task will communicate with a Raspberry Pi Pico via SerialPort. //! @author João Bogas //! @author Bernardo Gabriel - namespace CrawlerSerial + namespace CLP { using DUNE_NAMESPACES; @@ -370,7 +372,7 @@ namespace Drivers void interpretSentence(std::vector &parts) { - if (parts[0] == "$PWM") + if (parts[0] == "PRESS") { clearMessages(); m_press.setTimeStamp(); From ceacd0494b8dacc969f99dcbef809c26ef52eec2 Mon Sep 17 00:00:00 2001 From: Bernardo Gabriel Date: Thu, 9 Nov 2023 14:48:27 +0000 Subject: [PATCH 5/6] Config: crawler: update CLP task name. --- etc/crawler.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/etc/crawler.ini b/etc/crawler.ini index bf7e633e46..5a4ed10e6e 100644 --- a/etc/crawler.ini +++ b/etc/crawler.ini @@ -52,9 +52,9 @@ Absolute Minimum Altitude = 0 #?? Time Of Arrival Factor = 5.0 #TO BE DONE Underwater Depth Threshold = 0.3 #TO BE DONE -[Drivers.CrawlerSerial] +[Drivers.CLP] Enabled = Always -Entity Label = CrawlerSerial +Entity Label = CLP Debug Level = None IO Port - Device = uart:///dev/ttyPICO:115200 Input Timeout = 5.0 From ee6f9af455194b15169ab0d26834d50daa83e0ff Mon Sep 17 00:00:00 2001 From: Bernardo Gabriel Date: Tue, 16 Jan 2024 11:31:55 +0000 Subject: [PATCH 6/6] Drivers/CLP: only send STOP command in onDisconnect() if UART correctly opened. --- src/Drivers/CLP/Task.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Drivers/CLP/Task.cpp b/src/Drivers/CLP/Task.cpp index 42c101434e..70e5457603 100644 --- a/src/Drivers/CLP/Task.cpp +++ b/src/Drivers/CLP/Task.cpp @@ -170,7 +170,8 @@ namespace Drivers void onDisconnect() override { - sendCommand("@STOP,*"); + if (m_handle != nullptr) + sendCommand("@STOP,*"); if (m_reader != NULL) {