diff --git a/etc/crawler.ini b/etc/crawler.ini index d01142aa5b..5a4ed10e6e 100644 --- a/etc/crawler.ini +++ b/etc/crawler.ini @@ -52,13 +52,19 @@ 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 -Debug Level = Spew -Serial Port - Device = /dev/ttyPICO -Serial Port - Baud Rate = 115200 -Input Timeout = 3.0 +Entity Label = CLP +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 diff --git a/src/Drivers/CLP/Reader.hpp b/src/Drivers/CLP/Reader.hpp new file mode 100644 index 0000000000..19ea6eacdf --- /dev/null +++ b/src/Drivers/CLP/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_CLP_READER_HPP_INCLUDED_ +#define DRIVERS_CLP_READER_HPP_INCLUDED_ + +// DUNE headers. +#include + +namespace Drivers +{ + namespace CLP + { + 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.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/CLP/Task.cpp b/src/Drivers/CLP/Task.cpp new file mode 100644 index 0000000000..70e5457603 --- /dev/null +++ b/src/Drivers/CLP/Task.cpp @@ -0,0 +1,405 @@ +//*************************************************************************** +// 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. * +// * +// 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 (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 "Reader.hpp" + +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 CLP + { + 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 + { + //! 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 Hardware::BasicDeviceDriver + { + //! Serial port handle. + IO::Handle *m_handle; + //! Task arguments. + Arguments m_args; + //! Input watchdog. + Time::Counter m_wdog; + //! Reader thread. + Reader *m_reader; + //! IMC msg + IMC::Pressure m_press; + //! 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) : Hardware::BasicDeviceDriver(name, ctx), + m_handle(NULL), + m_reader(NULL) + { + // Define configuration parameters. + paramActive(Tasks::Parameter::SCOPE_GLOBAL, + Tasks::Parameter::VISIBILITY_DEVELOPER, + true); + + param("IO Port - Device", m_args.io_dev) + .defaultValue("") + .description("IO device URI in the form \"uart://DEVICE:BAUD\""); + + param("Input Timeout", m_args.inp_tout) + .units(Units::Second) + .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) + { + } + + //! 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; + } + + //! Disconnect from device. + void + onDisconnect() override + { + if (m_handle != nullptr) + sendCommand("@STOP,*"); + + if (m_reader != NULL) + { + m_reader->stopAndJoin(); + Memory::clear(m_reader); + } + + Memory::clear(m_handle); + } + + //! Initialize device. + void + onInitializeDevice() override + { + for (unsigned i = 0; i < c_max_init_cmds; ++i) + { + 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); + } + + void + clearMessages(void) + { + m_press.clear(); + } + + //! Wait reply to command. + //! @param[in] stn string to compare. + //! @return true on successful match, false otherwise. + bool + waitReply(const std::string &stn) + { + Counter counter(c_wait_reply_tout); + while (!stopping() && !counter.overflow()) + { + waitForMessages(counter.getRemaining()); + + 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; + return true; + } + } + + return false; + } + + void + consume(const IMC::DevDataText *msg) + { + if (msg->getDestination() != getSystemId()) + return; + + if (msg->getDestinationEntity() != getEntityId()) + return; + + trace("%s", sanitize(msg->value).c_str()); + + 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\n", cmd); + trace("Command: %s", sanitize(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 + consume(const IMC::RemoteActions *msg) + { + if (msg->getDestination() != getSystemId()) + return; + + Utils::TupleList tuples(msg->actions); + int light_val = tuples.get("Lights", -127); + if (light_val != 0) + { + IMC::SetServoPosition set_light; + 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); + sendCommand(send.c_str()); + } + } + + //! Process sentence. + //! @param[in] line line. + void + processSentence(const std::string &line) + { + // Discard leading noise. + size_t sidx = 0; + for (sidx = 0; sidx < line.size(); ++sidx) + { + if (line[sidx] == '$') + break; + } + + // Discard trailing noise. + size_t eidx = 0; + for (eidx = line.size() - 1; eidx > sidx; --eidx) + { + if (line[eidx] == '*') + break; + } + + if (sidx >= eidx) + 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; + String::split(line.substr(sidx + 1, eidx - sidx - 1), ",", parts); + + interpretSentence(parts); + } + + //! Interpret given sentence. + //! @param[in] parts vector of strings from sentence. + void + interpretSentence(std::vector &parts) + { + if (parts[0] == "PRESS") + { + clearMessages(); + m_press.setTimeStamp(); + m_press.value = atof(parts[1].c_str()); + dispatch(m_press, DF_KEEP_TIME); + } + + m_wdog.reset(); + } + + //! 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); + } + + return true; + } + }; + } +} + +DUNE_TASK \ No newline at end of file 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/Task.cpp b/src/Drivers/CrawlerSerial/Task.cpp deleted file mode 100644 index 6aa16038f0..0000000000 --- a/src/Drivers/CrawlerSerial/Task.cpp +++ /dev/null @@ -1,248 +0,0 @@ -//*************************************************************************** -// 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: João Bogas * -//*************************************************************************** - -// DUNE headers. -#include - -// Local headers -#include "Driver.hpp" - -namespace Drivers -{ - //! Insert short task description here. - //! - //! Insert explanation on task behaviour here. - //! @author João Bogas - namespace CrawlerSerial - { - using DUNE_NAMESPACES; - - 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; - }; - - struct Task: public DUNE::Tasks::Task - { - //! Serial port handle - SerialPort *m_uart; - //! I/O Multiplexer - Poll m_poll; - //! Task arguments - Arguments m_args; - //! Driver for CrawlerSerial - DriverCrawlerSerial *m_driver; - //! Timer - Counter m_wdog; - //! 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; - - //! Constructor. - //! @param[in] name task name. - //! @param[in] ctx context. - Task(const std::string& name, Tasks::Context& ctx): - DUNE::Tasks::Task(name, ctx) - { - param("Serial Port - Device", m_args.uart_dev) - .defaultValue("") - .description("Serial port device"); - - param("Serial Port - Baud Rate", m_args.uart_baud) - .defaultValue("") - .description("Serial port baud rate"); - - param("Input Timeout", m_args.input_timeout) - .defaultValue("3.0") - .minimumValue("2.0") - .maximumValue("4.0") - .units(Units::Second) - .description("Amount of seconds to wait for data before reporting an error"); - - bind(this); - } - - //! Update internal state with new parameter values. - void - onUpdateParameters(void) - { - } - - //! Reserve entity identifiers. - void - onEntityReservation(void) - { - } - - //! Resolve entity names. - void - onEntityResolution(void) - { - } - - //! Acquire resources. - void - onResourceAcquisition(void) - { - 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) - { - throw RestartNeeded(e.what(), 10); - } - } - - //! Initialize resources. - void - onResourceInitialization(void) - { - m_driver->stopAcquisition(); - m_uart->flush(); - Delay::wait(1.0f); - initBoard(); - m_wdog.setTop(m_args.input_timeout); - m_wdog.reset(); - } - - //! Release resources. - void - onResourceRelease(void) - { - if (m_uart != nullptr) - { - m_poll.remove(*m_uart); - Memory::clear(m_driver); - Memory::clear(m_uart); - } - } - - void - initBoard() - { - m_driver->stopAcquisition(); - - 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 (!m_driver->startAcquisition()) - { - setEntityState(IMC::EntityState::ESTA_NORMAL, Utils::String::str(DTR("trying connecting to board"))); - war(DTR("failed to start")); - } - - debug("Init OK"); - m_wdog.setTop(m_args.input_timeout); - m_wdog.reset(); - } - - void - consume(const IMC::RemoteActions *msg) - { - if (msg->getDestination() != getSystemId()) - return; - - Utils::TupleList tuples(msg->actions); - int light_val = tuples.get("Lights", -127); - if (light_val != 0) - { - IMC::SetServoPosition set_light; - 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()); - } - } - - void - dispatchData() - { - m_tstamp = Clock::getSinceEpoch(); - - m_press.setTimeStamp(m_tstamp); - m_press.value = m_driver->m_crawlerData.pressure; - dispatch(m_press, DF_KEEP_TIME); - } - - //! Main loop. - void - onMain(void) - { - while (!stopping()) - { - 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; - - if (m_driver->haveNewData()) - { - dispatchData(); - m_wdog.reset(); - } - - m_uart->flush(); - } - - m_driver->stopAcquisition(); - } - }; - } -} - -DUNE_TASK \ No newline at end of file