From fc7d64c8883ee991e76f7225ab92343876d5ae02 Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Thu, 6 Aug 2015 19:54:16 +0200 Subject: [PATCH 01/19] NortekDVL: task added --- src/Sensors/NortekDVL/Reader.hpp | 122 +++++++++ src/Sensors/NortekDVL/Task.cmake | 0 src/Sensors/NortekDVL/Task.cpp | 451 +++++++++++++++++++++++++++++++ 3 files changed, 573 insertions(+) create mode 100644 src/Sensors/NortekDVL/Reader.hpp create mode 100644 src/Sensors/NortekDVL/Task.cmake create mode 100644 src/Sensors/NortekDVL/Task.cpp diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp new file mode 100644 index 0000000000..70faa4b06d --- /dev/null +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -0,0 +1,122 @@ +//*************************************************************************** +// Copyright 2007-2014 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 Universidade do Porto. For licensing * +// terms, conditions, and further information contact lsts@fe.up.pt. * +// * +// European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the 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://www.lsts.pt/dune/licence. * +//*************************************************************************** +// Author: Ricardo Martins * +//*************************************************************************** + +#ifndef SENSORS_NORTEKDVL_READER_HPP_INCLUDED_ +#define SENSORS_NORTEKDVL_READER_HPP_INCLUDED_ + +// DUNE headers. +#include + +namespace Sensors +{ + namespace NortekDVL + { + 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/Sensors/NortekDVL/Task.cmake b/src/Sensors/NortekDVL/Task.cmake new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp new file mode 100644 index 0000000000..2424175f7b --- /dev/null +++ b/src/Sensors/NortekDVL/Task.cpp @@ -0,0 +1,451 @@ +//*************************************************************************** +// Copyright 2007-2014 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 Universidade do Porto. For licensing * +// terms, conditions, and further information contact lsts@fe.up.pt. * +// * +// European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the 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://www.lsts.pt/dune/licence. * +//*************************************************************************** +// Author: Ricardo Martins * +//*************************************************************************** + +// ISO C++ 98 headers. +#include +#include +#include + +// DUNE headers. +#include + +// Local headers. +#include "Reader.hpp" + +namespace Sensors +{ + //! Device driver for NMEA capable %GPS devices. + namespace NortekDVL + { + using DUNE_NAMESPACES; + + //! Maximum number of initialization commands. + static const unsigned c_max_init_cmds = 14; + //! Timeout for waitReply() function. + static const float c_wait_reply_tout = 4.0; + static const unsigned c_pnorbt_fields = 10; + static const unsigned c_pnors1_fields = 15; + static const unsigned c_pnori1_fields = 7; + static const unsigned c_pnorc1_fields = 17; + //! Power on delay. + static const double c_pwr_on_delay = 5.0; + + struct Arguments + { + //! Serial port device. + std::string uart_dev; + //! Serial port baud rate. + unsigned uart_baud; + //! Order of sentences. + std::vector stn_order; + //! 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]; + //! Power channels. + std::vector pwr_channels; + }; + + struct Task: public Tasks::Task + { + //! Serial port handle. + IO::Handle* m_handle; + IMC::GroundVelocity m_gvel; + IMC::Temperature m_temp; + IMC::Pressure m_prs; + IMC::EulerAngles m_euler; + //! Task arguments. + Arguments m_args; + double m_bdist, m_cell_len; + std::string m_init_line; + //! Reader thread. + Reader* m_reader; + + Task(const std::string& name, Tasks::Context& ctx): + Tasks::Task(name, ctx), + m_handle(NULL), + m_bdist(0), + m_cell_len(0), + m_reader(NULL) + { + // Define configuration parameters. + param("Serial Port - Device", m_args.uart_dev) + .defaultValue("") + .description("Serial port device used to communicate with the sensor"); + + param("Serial Port - Baud Rate", m_args.uart_baud) + .defaultValue("4800") + .description("Serial port baud rate"); + + param("Input Timeout", m_args.inp_tout) + .units(Units::Second) + .defaultValue("4.0") + .minimumValue("0.0") + .description("Input timeout"); + + param("Power Channel - Names", m_args.pwr_channels) + .defaultValue("") + .description("Device's power channels"); + + param("Sentence Order", m_args.stn_order) + .defaultValue("") + .description("Sentence order"); + + 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(""); + } + + m_euler.setSourceEntity(getEntityId()); + m_prs.setSourceEntity(getEntityId()); + m_temp.setSourceEntity(getEntityId()); + m_gvel.setSourceEntity(getEntityId()); + + bind(this); + bind(this); + } + + void + onResourceAcquisition(void) + { + if (m_args.pwr_channels.size() > 0) + { + IMC::PowerChannelControl pcc; + pcc.op = IMC::PowerChannelControl::PCC_OP_TURN_ON; + for (size_t i = 0; i < m_args.pwr_channels.size(); ++i) + { + pcc.name = m_args.pwr_channels[i]; + dispatch(pcc); + } + } + + Counter timer(c_pwr_on_delay); + while (!stopping() && !timer.overflow()) + waitForMessages(timer.getRemaining()); + + try + { + if (!openSocket()) + m_handle = new SerialPort(m_args.uart_dev, m_args.uart_baud); + + m_reader = new Reader(this, m_handle); + m_reader->start(); + } + catch (...) + { + throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); + } + } + + bool + openSocket(void) + { + char addr[128] = {0}; + unsigned port = 0; + + if (std::sscanf(m_args.uart_dev.c_str(), "tcp://%[^:]:%u", addr, &port) != 2) + return false; + + TCPSocket* sock = new TCPSocket; + sock->connect(addr, port); + m_handle = sock; + return true; + } + + void + onResourceRelease(void) + { + if (m_reader != NULL) + { + m_reader->stopAndJoin(); + delete m_reader; + m_reader = NULL; + } + + Memory::clear(m_handle); + } + + void + onResourceInitialization(void) + { + for (unsigned i = 0; i < c_max_init_cmds; ++i) + { + if (m_args.init_cmds[i].empty()) + continue; + + std::string cmd = String::unescape(m_args.init_cmds[i]); + m_handle->writeString(cmd.c_str()); + + if (!m_args.init_rpls[i].empty()) + { + std::string rpl = String::unescape(m_args.init_rpls[i]); + if (!waitInitReply(rpl)) + { + err("%s: %s", DTR("no reply to command"), m_args.init_cmds[i].c_str()); + throw std::runtime_error(DTR("failed to setup device")); + } + } + } + + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); + } + + void + consume(const IMC::DevDataText* msg) + { + if (msg->getDestination() != getSystemId()) + return; + + if (msg->getDestinationEntity() != getEntityId()) + return; + + spew("inp: %s", sanitize(msg->value).c_str()); + + // if (getEntityState() == IMC::EntityState::ESTA_BOOT) + // m_init_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); + } + + //! Wait reply to initialization command. + //! @param[in] stn string to compare. + //! @return true on successful match, false otherwise. + bool + waitInitReply(const std::string& stn) + { + Counter counter(c_wait_reply_tout); + while (!stopping() && !counter.overflow()) + { + waitForMessages(counter.getRemaining()); + if (m_init_line == stn) + { + m_init_line.clear(); + return true; + } + } + + return false; + } + + //! Read decimal from input string. + //! @param[in] str input string. + //! @param[out] dst decimal. + //! @return true if successful, false otherwise. + template + bool + readDecimal(const std::string& str, T& dst) + { + unsigned idx = 0; + while (str[idx] == '0') + ++idx; + + return castLexical(std::string(str, idx), dst); + } + + //! Read number from input string. + //! @param[in] str input string. + //! @param[out] dst number. + //! @return true if successful, false otherwise. + template + bool + readNumber(const std::string& str, T& dst) + { + return castLexical(str, dst); + } + + //! 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; + + // 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) + { + 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] == "PNORBT") + { + interpretPNORBT(parts); + } + else if (parts[0] == "PNORS1") + { + interpretPNORS1(parts); + } + else if (parts[0] == "PNORI1") + { + interpretPNORI1(parts); + } + else if (parts[0] == "PNORC1") + { + interpretPNORC1(parts); + } + } + + void + interpretPNORBT(const std::vector& parts) + { + if (parts.size() < c_pnorbt_fields) + { + war(DTR("invalid PNORBT sentence")); + return; + } + + readNumber(parts[8], m_bdist); + } + + void + interpretPNORS1(const std::vector& parts) + { + if (parts.size() < c_pnors1_fields) + { + war(DTR("invalid PNORS sentence")); + return; + } + + readNumber(parts[7], m_euler.psi); + readNumber(parts[9], m_euler.theta); + readNumber(parts[11], m_euler.phi); + m_euler.psi = Angles::normalizeRadian(Angles::radians(m_euler.psi)); + m_euler.theta = Angles::normalizeRadian(Angles::radians(m_euler.theta)); + m_euler.phi = Angles::normalizeRadian(Angles::radians(m_euler.phi)); + dispatch(m_euler); + + readNumber(parts[13], m_prs.value); + m_prs.value *= 100; // dBar -> hPa + dispatch(m_prs); + + readNumber(parts[15], m_temp.value); + dispatch(m_temp); + } + + void + interpretPNORI1(const std::vector& parts) + { + if (parts.size() < c_pnori1_fields) + { + war(DTR("invalid PNORI1 sentence")); + return; + } + + readNumber(parts[6], m_cell_len); + } + + void + interpretPNORC1(const std::vector& parts) + { + if (parts.size() < c_pnorc1_fields) + { + war(DTR("invalid PNORC1 sentence")); + return; + } + + double pos; + readNumber(parts[4], pos); + + if (pos <= m_bdist && pos >= m_bdist - m_cell_len) + { + readNumber(parts[9], m_gvel.x); + readNumber(parts[10], m_gvel.y); + readNumber(parts[11], m_gvel.z); + dispatch(m_gvel); + + inf("vel %f, %f, %f", m_gvel.x, m_gvel.y, m_gvel.z); + } + } + + void + onMain(void) + { + while (!stopping()) + { + waitForMessages(1.0); + } + } + }; + } +} + +DUNE_TASK From d3c04c59a064c80760147ea934611df502f8aaf2 Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Fri, 7 Aug 2015 09:42:25 +0200 Subject: [PATCH 02/19] NortekDVL: commented out EulerAngles emitting --- src/Sensors/NortekDVL/Task.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 2424175f7b..de12e172ae 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -385,13 +385,13 @@ namespace Sensors return; } - readNumber(parts[7], m_euler.psi); - readNumber(parts[9], m_euler.theta); - readNumber(parts[11], m_euler.phi); - m_euler.psi = Angles::normalizeRadian(Angles::radians(m_euler.psi)); - m_euler.theta = Angles::normalizeRadian(Angles::radians(m_euler.theta)); - m_euler.phi = Angles::normalizeRadian(Angles::radians(m_euler.phi)); - dispatch(m_euler); + // readNumber(parts[7], m_euler.psi); + // readNumber(parts[9], m_euler.theta); + // readNumber(parts[11], m_euler.phi); + // m_euler.psi = Angles::normalizeRadian(Angles::radians(m_euler.psi)); + // m_euler.theta = Angles::normalizeRadian(Angles::radians(m_euler.theta)); + // m_euler.phi = Angles::normalizeRadian(Angles::radians(m_euler.phi)); + // dispatch(m_euler); readNumber(parts[13], m_prs.value); m_prs.value *= 100; // dBar -> hPa From f6803d75051cdd53582429b556714422fa4296ee Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Wed, 16 Dec 2015 14:53:26 +0100 Subject: [PATCH 03/19] Sensors/NortekDVL: added support for PNORBT7 sentence --- src/Sensors/NortekDVL/Task.cpp | 68 ++++++---------------------------- 1 file changed, 12 insertions(+), 56 deletions(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index de12e172ae..0657406bbe 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -47,9 +47,8 @@ namespace Sensors static const unsigned c_max_init_cmds = 14; //! Timeout for waitReply() function. static const float c_wait_reply_tout = 4.0; - static const unsigned c_pnorbt_fields = 10; + static const unsigned c_pnorbt7_fields = 9; static const unsigned c_pnors1_fields = 15; - static const unsigned c_pnori1_fields = 7; static const unsigned c_pnorc1_fields = 17; //! Power on delay. static const double c_pwr_on_delay = 5.0; @@ -82,7 +81,6 @@ namespace Sensors IMC::EulerAngles m_euler; //! Task arguments. Arguments m_args; - double m_bdist, m_cell_len; std::string m_init_line; //! Reader thread. Reader* m_reader; @@ -90,8 +88,6 @@ namespace Sensors Task(const std::string& name, Tasks::Context& ctx): Tasks::Task(name, ctx), m_handle(NULL), - m_bdist(0), - m_cell_len(0), m_reader(NULL) { // Define configuration parameters. @@ -346,42 +342,37 @@ namespace Sensors void interpretSentence(std::vector& parts) { - if (parts[0] == "PNORBT") + if (parts[0] == "PNORBT7") // bottom tracking { - interpretPNORBT(parts); + interpretPNORBT7(parts); } else if (parts[0] == "PNORS1") { interpretPNORS1(parts); } - else if (parts[0] == "PNORI1") - { - interpretPNORI1(parts); - } - else if (parts[0] == "PNORC1") - { - interpretPNORC1(parts); - } } void - interpretPNORBT(const std::vector& parts) + interpretPNORBT7(const std::vector& parts) { - if (parts.size() < c_pnorbt_fields) + if (parts.size() != c_pnorbt7_fields) { - war(DTR("invalid PNORBT sentence")); + war(DTR("invalid PNORBT7 sentence")); return; } - readNumber(parts[8], m_bdist); + readNumber(parts[2], m_gvel.x); + readNumber(parts[3], m_gvel.y); + readNumber(parts[4], m_gvel.z); + dispatch(m_gvel); } void interpretPNORS1(const std::vector& parts) { - if (parts.size() < c_pnors1_fields) + if (parts.size() != c_pnors1_fields) { - war(DTR("invalid PNORS sentence")); + war(DTR("invalid PNORS1 sentence")); return; } @@ -401,41 +392,6 @@ namespace Sensors dispatch(m_temp); } - void - interpretPNORI1(const std::vector& parts) - { - if (parts.size() < c_pnori1_fields) - { - war(DTR("invalid PNORI1 sentence")); - return; - } - - readNumber(parts[6], m_cell_len); - } - - void - interpretPNORC1(const std::vector& parts) - { - if (parts.size() < c_pnorc1_fields) - { - war(DTR("invalid PNORC1 sentence")); - return; - } - - double pos; - readNumber(parts[4], pos); - - if (pos <= m_bdist && pos >= m_bdist - m_cell_len) - { - readNumber(parts[9], m_gvel.x); - readNumber(parts[10], m_gvel.y); - readNumber(parts[11], m_gvel.z); - dispatch(m_gvel); - - inf("vel %f, %f, %f", m_gvel.x, m_gvel.y, m_gvel.z); - } - } - void onMain(void) { From 628dab04439687275ab5708a70685b599b2eb7fa Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Tue, 9 Feb 2016 13:30:34 +0100 Subject: [PATCH 04/19] Sensors/NortekDVL: implemented fsm to use command interface --- src/Sensors/NortekDVL/Reader.hpp | 129 +++++++++++++++++++++++++++---- src/Sensors/NortekDVL/Task.cpp | 3 +- 2 files changed, 116 insertions(+), 16 deletions(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index 70faa4b06d..cdc7d94289 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -39,8 +39,7 @@ namespace Sensors //! Read buffer size. static const size_t c_read_buffer_size = 4096; - //! Line termination character. - static const char c_line_term = '\n'; + static const char *c_control_seq = "K1W%!Q\r\n"; class Reader: public Concurrency::Thread { @@ -50,9 +49,9 @@ namespace Sensors //! @param[in] handle I/O handle. Reader(Tasks::Task* task, IO::Handle* handle): m_task(task), - m_handle(handle) + m_handle(handle), + m_state(0) { - m_buffer.resize(c_read_buffer_size); } private: @@ -61,7 +60,9 @@ namespace Sensors //! I/O handle. IO::Handle* m_handle; //! Internal read buffer. - std::vector m_buffer; + char m_buffer[c_read_buffer_size]; + //! State + uint8_t m_state, m_conf_line; //! Current line. std::string m_line; @@ -73,26 +74,124 @@ namespace Sensors m_task->dispatch(msg, DF_LOOP_BACK); } + void + auth(void) + { + if (m_line.rfind("Username: ") != std::string::npos) + { + m_line.clear(); + m_handle->writeString("nortek\n"); + } + else if (m_line.rfind("Password: ") != std::string::npos) + { + m_line.clear(); + m_handle->writeString("\n"); + } + else if (m_line.rfind("Command Interface\r\n") != std::string::npos) { + m_line.clear(); + m_conf_line = 0; + m_handle->writeString(c_control_seq); + m_state = 1; // CONF + } + else if (m_line.rfind("Login failed") != std::string::npos) + { + throw std::runtime_error("Login failed"); + } + } + + void + conf(void) + { + std::string str; + + if (m_line.rfind("OK\r\n") != std::string::npos) + { + m_line.clear(); + switch (m_conf_line++) + { + case 0: + m_handle->writeString("MC\r\n"); + break; + + case 1: + str = String::str("SETDVL,0,\"OFF\",\"INTSR\",%.1f,\"\",0.%.1f,%.1f\r\n", 4., 0., 35.); + m_handle->writeString(str.c_str()); + break; + + case 2: + str = String::str("SETBT,%.2f,%.2f,4,0,307,%.1f,\"XYZ\"\r\n", 30., 5., -20.); + m_handle->writeString(str.c_str()); + break; + + case 3: + m_handle->writeString("START\r\n"); + break; + + default: + m_state = 2; // CAPTURE + } + } + else if (m_line.rfind("ERROR\r\n") != std::string::npos) + { + m_line.clear(); + m_handle->writeString("GETERROR\r\n"); + m_state = 3; // ERROR + } + } + void read(void) { if (!Poll::poll(*m_handle, 1.0)) return; - size_t rv = m_handle->read(&m_buffer[0], m_buffer.size()); + size_t pos; + size_t rv = m_handle->read(m_buffer, c_read_buffer_size); if (rv == 0) throw std::runtime_error(DTR("invalid read size")); - for (size_t i = 0; i < rv; ++i) + switch (m_state) { - 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(); - } + case 0: // INIT + m_line.append(m_buffer, rv); + if (m_line.size() > c_read_buffer_size) + m_line.erase(0, m_line.size() - c_read_buffer_size); + + auth(); + break; + + case 1: // CONF + m_line.append(m_buffer, rv); + if (m_line.size() > c_read_buffer_size) + m_line.erase(0, m_line.size() - c_read_buffer_size); + + conf(); + break; + + case 2: // CAPTURE + m_line.append(m_buffer, rv); + if (m_line.size() > c_read_buffer_size) + m_line.erase(0, m_line.size() - c_read_buffer_size); + + while ((pos = m_line.find('\n')) != std::string::npos) + { + IMC::DevDataText line; + line.value = m_line.substr(0, pos - 1); + m_line.erase(0, pos + 1); + dispatch(line); + } + break; + + case 3: // ERROR + default: + m_line.append(m_buffer, rv); + if (m_line.size() > c_read_buffer_size) + m_line.erase(0, m_line.size() - c_read_buffer_size); + + if ((pos = m_line.find('\n')) != std::string::npos) + throw std::runtime_error(m_line.substr(0, pos - 1)); + + break; } } diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 0657406bbe..bcc97565ff 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -227,7 +227,7 @@ namespace Sensors if (msg->getDestinationEntity() != getEntityId()) return; - spew("inp: %s", sanitize(msg->value).c_str()); + inf("inp: %s", sanitize(msg->value).c_str()); // if (getEntityState() == IMC::EntityState::ESTA_BOOT) // m_init_line = msg->value; @@ -365,6 +365,7 @@ namespace Sensors readNumber(parts[3], m_gvel.y); readNumber(parts[4], m_gvel.z); dispatch(m_gvel); + inf("BT7 %.2f, %.2f, %.2f", m_gvel.x, m_gvel.y, m_gvel.z); } void From 7460eea47f4db2acb4420c1c98a0d500afd8ece0 Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Tue, 9 Feb 2016 14:36:59 +0100 Subject: [PATCH 05/19] Sensors/NortekDVL: added init timeout --- src/Sensors/NortekDVL/Reader.hpp | 6 ++ src/Sensors/NortekDVL/Task.cpp | 124 ++++--------------------------- 2 files changed, 19 insertions(+), 111 deletions(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index cdc7d94289..171d3cb36b 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -54,6 +54,12 @@ namespace Sensors { } + uint8_t + getState(void) + { + return m_state; + } + private: //! Parent task. Tasks::Task* m_task; diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index bcc97565ff..38e3f442d3 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -43,15 +43,9 @@ namespace Sensors { using DUNE_NAMESPACES; - //! Maximum number of initialization commands. - static const unsigned c_max_init_cmds = 14; - //! Timeout for waitReply() function. - static const float c_wait_reply_tout = 4.0; - static const unsigned c_pnorbt7_fields = 9; - static const unsigned c_pnors1_fields = 15; - static const unsigned c_pnorc1_fields = 17; - //! Power on delay. + static const unsigned c_pnorbt7_fields = 10; static const double c_pwr_on_delay = 5.0; + static const double c_init_tout = 5.0; struct Arguments { @@ -59,14 +53,8 @@ namespace Sensors std::string uart_dev; //! Serial port baud rate. unsigned uart_baud; - //! Order of sentences. - std::vector stn_order; //! 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]; //! Power channels. std::vector pwr_channels; }; @@ -101,7 +89,7 @@ namespace Sensors param("Input Timeout", m_args.inp_tout) .units(Units::Second) - .defaultValue("4.0") + .defaultValue("5.0") .minimumValue("0.0") .description("Input timeout"); @@ -109,21 +97,6 @@ namespace Sensors .defaultValue("") .description("Device's power channels"); - param("Sentence Order", m_args.stn_order) - .defaultValue("") - .description("Sentence order"); - - 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(""); - } - m_euler.setSourceEntity(getEntityId()); m_prs.setSourceEntity(getEntityId()); m_temp.setSourceEntity(getEntityId()); @@ -196,25 +169,17 @@ namespace Sensors void onResourceInitialization(void) { - for (unsigned i = 0; i < c_max_init_cmds; ++i) + Counter counter(c_init_tout); + while (!stopping() && !counter.overflow()) { - if (m_args.init_cmds[i].empty()) - continue; - - std::string cmd = String::unescape(m_args.init_cmds[i]); - m_handle->writeString(cmd.c_str()); - - if (!m_args.init_rpls[i].empty()) - { - std::string rpl = String::unescape(m_args.init_rpls[i]); - if (!waitInitReply(rpl)) - { - err("%s: %s", DTR("no reply to command"), m_args.init_cmds[i].c_str()); - throw std::runtime_error(DTR("failed to setup device")); - } - } + waitForMessages(counter.getRemaining()); + if (m_reader->getState() == 2) + break; } + if (counter.overflow()) + throw std::runtime_error(DTR("failed to setup device")); + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); } @@ -227,12 +192,7 @@ namespace Sensors if (msg->getDestinationEntity() != getEntityId()) return; - inf("inp: %s", sanitize(msg->value).c_str()); - - // if (getEntityState() == IMC::EntityState::ESTA_BOOT) - // m_init_line = msg->value; - // else - processSentence(msg->value); + processSentence(msg->value); } void @@ -248,26 +208,6 @@ namespace Sensors throw RestartNeeded(msg->error, 5); } - //! Wait reply to initialization command. - //! @param[in] stn string to compare. - //! @return true on successful match, false otherwise. - bool - waitInitReply(const std::string& stn) - { - Counter counter(c_wait_reply_tout); - while (!stopping() && !counter.overflow()) - { - waitForMessages(counter.getRemaining()); - if (m_init_line == stn) - { - m_init_line.clear(); - return true; - } - } - - return false; - } - //! Read decimal from input string. //! @param[in] str input string. //! @param[out] dst decimal. @@ -340,20 +280,7 @@ namespace Sensors //! Interpret given sentence. //! @param[in] parts vector of strings from sentence. void - interpretSentence(std::vector& parts) - { - if (parts[0] == "PNORBT7") // bottom tracking - { - interpretPNORBT7(parts); - } - else if (parts[0] == "PNORS1") - { - interpretPNORS1(parts); - } - } - - void - interpretPNORBT7(const std::vector& parts) + interpretSentence(const std::vector& parts) { if (parts.size() != c_pnorbt7_fields) { @@ -368,31 +295,6 @@ namespace Sensors inf("BT7 %.2f, %.2f, %.2f", m_gvel.x, m_gvel.y, m_gvel.z); } - void - interpretPNORS1(const std::vector& parts) - { - if (parts.size() != c_pnors1_fields) - { - war(DTR("invalid PNORS1 sentence")); - return; - } - - // readNumber(parts[7], m_euler.psi); - // readNumber(parts[9], m_euler.theta); - // readNumber(parts[11], m_euler.phi); - // m_euler.psi = Angles::normalizeRadian(Angles::radians(m_euler.psi)); - // m_euler.theta = Angles::normalizeRadian(Angles::radians(m_euler.theta)); - // m_euler.phi = Angles::normalizeRadian(Angles::radians(m_euler.phi)); - // dispatch(m_euler); - - readNumber(parts[13], m_prs.value); - m_prs.value *= 100; // dBar -> hPa - dispatch(m_prs); - - readNumber(parts[15], m_temp.value); - dispatch(m_temp); - } - void onMain(void) { From bf6db175102093ac05043873a06626bfee564b1f Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Tue, 9 Feb 2016 15:50:43 +0100 Subject: [PATCH 06/19] Sensors/NortekDVL: added support for parameters for Reader --- src/Sensors/NortekDVL/Reader.hpp | 38 +++++++++++++++++++++++++++----- src/Sensors/NortekDVL/Task.cpp | 3 ++- 2 files changed, 35 insertions(+), 6 deletions(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index 171d3cb36b..d8a6eb93ed 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -44,22 +44,41 @@ namespace Sensors class Reader: public Concurrency::Thread { public: + struct NortekParam + { + std::string username, password; + double rate, sndvel, salinity; + double bt_range, v_range, pwr_level; + }; + //! Constructor. //! @param[in] task parent task. //! @param[in] handle I/O handle. - Reader(Tasks::Task* task, IO::Handle* handle): + Reader(Tasks::Task* task, IO::Handle* handle, NortekParam ¶m): m_task(task), m_handle(handle), - m_state(0) + m_state(0), + m_param(param) { } uint8_t getState(void) { + ScopedMutex m(m_closed_mutex); return m_state; } + void + reconfigure(NortekParam ¶m) + { + ScopedMutex m(m_closed_mutex); + + m_handle->writeString(c_control_seq); + m_state = 1; // CONF + m_param = param; + } + private: //! Parent task. Tasks::Task* m_task; @@ -69,8 +88,10 @@ namespace Sensors char m_buffer[c_read_buffer_size]; //! State uint8_t m_state, m_conf_line; + NortekParam m_param; //! Current line. std::string m_line; + Mutex m_closed_mutex; void dispatch(IMC::Message& msg) @@ -83,14 +104,18 @@ namespace Sensors void auth(void) { + ScopedMutex m(m_closed_mutex); + if (m_line.rfind("Username: ") != std::string::npos) { m_line.clear(); - m_handle->writeString("nortek\n"); + m_handle->writeString(m_param.username.c_str()); + m_handle->writeString("\n"); } else if (m_line.rfind("Password: ") != std::string::npos) { m_line.clear(); + m_handle->writeString(m_param.password.c_str()); m_handle->writeString("\n"); } else if (m_line.rfind("Command Interface\r\n") != std::string::npos) { @@ -108,6 +133,7 @@ namespace Sensors void conf(void) { + ScopedMutex m(m_closed_mutex); std::string str; if (m_line.rfind("OK\r\n") != std::string::npos) @@ -120,12 +146,14 @@ namespace Sensors break; case 1: - str = String::str("SETDVL,0,\"OFF\",\"INTSR\",%.1f,\"\",0.%.1f,%.1f\r\n", 4., 0., 35.); + str = String::str("SETDVL,0,\"OFF\",\"INTSR\",%.1f,\"\",%.1f,%.1f\r\n", + m_param.rate, m_param.sndvel, m_param.salinity); m_handle->writeString(str.c_str()); break; case 2: - str = String::str("SETBT,%.2f,%.2f,4,0,307,%.1f,\"XYZ\"\r\n", 30., 5., -20.); + str = String::str("SETBT,%.2f,%.2f,4,0,307,%.1f,\"XYZ\"\r\n", + m_param.bt_range, m_param.v_range, m_param.pwr_level); m_handle->writeString(str.c_str()); break; diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 38e3f442d3..1d95e7e17e 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -129,7 +129,8 @@ namespace Sensors if (!openSocket()) m_handle = new SerialPort(m_args.uart_dev, m_args.uart_baud); - m_reader = new Reader(this, m_handle); + Reader::NortekParam parm = { "nortek", "", 4, 0, 35, 30, 5, -20 }; + m_reader = new Reader(this, m_handle, parm); m_reader->start(); } catch (...) From 44f17d9e109923801bf153a2735d8da93c54dffe Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Tue, 9 Feb 2016 16:05:59 +0100 Subject: [PATCH 07/19] Sensors/NortekDVL: added task parameters --- src/Sensors/NortekDVL/Task.cpp | 42 ++++++++++++++++++++++++++++++++-- 1 file changed, 40 insertions(+), 2 deletions(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 1d95e7e17e..78f7233089 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -57,6 +57,8 @@ namespace Sensors float inp_tout; //! Power channels. std::vector pwr_channels; + + Reader::NortekParam params; }; struct Task: public Tasks::Task @@ -97,6 +99,43 @@ namespace Sensors .defaultValue("") .description("Device's power channels"); + param("Username", m_args.params.username) + .defaultValue("nortek") + .description("User name to athenticate command interface"); + + param("Password", m_args.params.password) + .defaultValue("") + .description("Password to athenticate command interface"); + + param("Input Rate", m_args.params.rate) + .defaultValue("4.0") + .minimumValue("0.0") + .description("Input rate"); + + param("Sound Velocity", m_args.params.rate) + .defaultValue("0.0") + .description("Sound velocity"); + + param("Sound Velocity", m_args.params.sndvel) + .defaultValue("0.0") + .description("Sound velocity"); + + param("Salinity", m_args.params.salinity) + .defaultValue("0.0") + .description("Salinity"); + + param("Bottom-Track Range", m_args.params.bt_range) + .defaultValue("30.0") + .description("Bottom-track range"); + + param("Velocity Range", m_args.params.v_range) + .defaultValue("5.0") + .description("Velocity range"); + + param("Power Level", m_args.params.pwr_level) + .defaultValue("-20.0") + .description("Power level"); + m_euler.setSourceEntity(getEntityId()); m_prs.setSourceEntity(getEntityId()); m_temp.setSourceEntity(getEntityId()); @@ -129,8 +168,7 @@ namespace Sensors if (!openSocket()) m_handle = new SerialPort(m_args.uart_dev, m_args.uart_baud); - Reader::NortekParam parm = { "nortek", "", 4, 0, 35, 30, 5, -20 }; - m_reader = new Reader(this, m_handle, parm); + m_reader = new Reader(this, m_handle, m_args.params); m_reader->start(); } catch (...) From 3061e78746f2cc5151e0e75f90781bbedc686702 Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Tue, 9 Feb 2016 16:13:58 +0100 Subject: [PATCH 08/19] Sensors/NortekDVL: added dynamically parameters application --- src/Sensors/NortekDVL/Task.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 78f7233089..bdf999dd36 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -145,6 +145,23 @@ namespace Sensors bind(this); } + void + onUpdateParameters(void) + { + if (m_reader) + { + if (paramChanged(m_args.params.username) || + paramChanged(m_args.params.password) || + paramChanged(m_args.params.rate) || + paramChanged(m_args.params.sndvel) || + paramChanged(m_args.params.salinity) || + paramChanged(m_args.params.bt_range) || + paramChanged(m_args.params.v_range) || + paramChanged(m_args.params.pwr_level)) + m_reader->reconfigure(m_args.params); + } + } + void onResourceAcquisition(void) { From d7a85717952215c7ac8fa402f678680f64402315 Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Wed, 10 Feb 2016 15:17:59 +0100 Subject: [PATCH 09/19] Sensors/NortekDVL: implemented binary protocol support --- src/Sensors/NortekDVL/Reader.hpp | 181 ++++++++++++++++++++++++------- src/Sensors/NortekDVL/Task.cpp | 108 ++++-------------- 2 files changed, 163 insertions(+), 126 deletions(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index d8a6eb93ed..8005f8d93b 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -30,6 +30,19 @@ // DUNE headers. #include +#include +#include +#include + +#define MSTA_INIT 0 +#define MSTA_CONF 1 +#define MSTA_ERROR 2 +#define MSTA_SEEK_HDR 3 +#define MSTA_SEEK_CACHED_HDR 4 +#define MSTA_CACHE_HDR 5 +#define MSTA_CACHE_DATA 6 + +#define HDR_SIZE 10 namespace Sensors { @@ -57,7 +70,7 @@ namespace Sensors Reader(Tasks::Task* task, IO::Handle* handle, NortekParam ¶m): m_task(task), m_handle(handle), - m_state(0), + m_state(MSTA_INIT), m_param(param) { } @@ -75,8 +88,9 @@ namespace Sensors ScopedMutex m(m_closed_mutex); m_handle->writeString(c_control_seq); - m_state = 1; // CONF + m_state = MSTA_CONF; m_param = param; + m_conf_line = 0; } private: @@ -86,8 +100,10 @@ namespace Sensors IO::Handle* m_handle; //! Internal read buffer. char m_buffer[c_read_buffer_size]; + uint8_t m_cache[c_read_buffer_size]; //! State uint8_t m_state, m_conf_line; + size_t m_cached; NortekParam m_param; //! Current line. std::string m_line; @@ -122,7 +138,7 @@ namespace Sensors m_line.clear(); m_conf_line = 0; m_handle->writeString(c_control_seq); - m_state = 1; // CONF + m_state = MSTA_CONF; } else if (m_line.rfind("Login failed") != std::string::npos) { @@ -152,7 +168,7 @@ namespace Sensors break; case 2: - str = String::str("SETBT,%.2f,%.2f,4,0,307,%.1f,\"XYZ\"\r\n", + str = String::str("SETBT,%.2f,%.2f,4,0,21,%.1f,\"XYZ\"\r\n", m_param.bt_range, m_param.v_range, m_param.pwr_level); m_handle->writeString(str.c_str()); break; @@ -162,14 +178,14 @@ namespace Sensors break; default: - m_state = 2; // CAPTURE + m_state = MSTA_SEEK_HDR; } } else if (m_line.rfind("ERROR\r\n") != std::string::npos) { m_line.clear(); m_handle->writeString("GETERROR\r\n"); - m_state = 3; // ERROR + m_state = MSTA_ERROR; } } @@ -184,49 +200,138 @@ namespace Sensors if (rv == 0) throw std::runtime_error(DTR("invalid read size")); - switch (m_state) + if (m_state < MSTA_SEEK_HDR) { - case 0: // INIT - m_line.append(m_buffer, rv); - if (m_line.size() > c_read_buffer_size) - m_line.erase(0, m_line.size() - c_read_buffer_size); - - auth(); - break; + m_line.append(m_buffer, rv); + if (m_line.size() > c_read_buffer_size) + m_line.erase(0, m_line.size() - c_read_buffer_size); - case 1: // CONF - m_line.append(m_buffer, rv); - if (m_line.size() > c_read_buffer_size) - m_line.erase(0, m_line.size() - c_read_buffer_size); + switch (m_state) + { + case MSTA_INIT: + auth(); + break; - conf(); - break; + case MSTA_CONF: + conf(); + break; - case 2: // CAPTURE - m_line.append(m_buffer, rv); - if (m_line.size() > c_read_buffer_size) - m_line.erase(0, m_line.size() - c_read_buffer_size); + case MSTA_ERROR: + if ((pos = m_line.find('\n')) != std::string::npos) + throw std::runtime_error(m_line.substr(0, pos - 1)); + break; + } + } + else + { + size_t len, i; - while ((pos = m_line.find('\n')) != std::string::npos) + pos = 0; // current position in m_buffer[] + while (pos < rv) + { + switch (m_state) { - IMC::DevDataText line; - line.value = m_line.substr(0, pos - 1); - m_line.erase(0, pos + 1); - dispatch(line); + case MSTA_SEEK_HDR: // seek header in input buffer + for (; pos < rv; ++pos) + if ((uint8_t)m_buffer[pos] == 0xA5) + { + m_state = MSTA_CACHE_HDR; + m_cached = 0; + break; + } + + break; + + case MSTA_SEEK_CACHED_HDR: // seek header in cache + for (i = 1; i < m_cached; ++i) + if ((uint8_t)m_cache[i] == 0xA5) + { + std::memmove(m_cache, m_cache + i, m_cached - i); + m_cached -= i; + m_state = MSTA_CACHE_HDR; + break; + } + + if (m_state != MSTA_CACHE_HDR && i == m_cached) // not found + { + m_state = MSTA_SEEK_HDR; + } + + break; + + case MSTA_CACHE_HDR: + len = std::min(rv - pos, (size_t)HDR_SIZE - m_cached); + std::memcpy(m_cache + m_cached, m_buffer + pos, len); + m_cached += len; + pos += len; + + if (m_cached == HDR_SIZE) + { + uint16_t sum = m_cache[8] | m_cache[9] << 8; + + if (m_cache[1] != HDR_SIZE || sum != checksum(m_cache, HDR_SIZE - 2)) + m_state = MSTA_SEEK_CACHED_HDR; + else + m_state = MSTA_CACHE_DATA; + } + + break; + + case MSTA_CACHE_DATA: + size_t datalen = m_cache[4] | m_cache[5] << 8; + + len = std::min(rv - pos, (size_t)HDR_SIZE + datalen - m_cached); + std::memcpy(m_cache + m_cached, m_buffer + pos, len); + m_cached += len; + pos += len; + + if (m_cached == HDR_SIZE + datalen) + { + uint16_t sum = m_cache[6] | m_cache[7] << 8; + + if (sum != checksum(m_cache + HDR_SIZE, datalen)) + m_state = MSTA_SEEK_CACHED_HDR; + else + { + processFrame(); + m_state = MSTA_SEEK_HDR; + } + } + + break; } - break; + } + } + } - case 3: // ERROR - default: - m_line.append(m_buffer, rv); - if (m_line.size() > c_read_buffer_size) - m_line.erase(0, m_line.size() - c_read_buffer_size); + void + processFrame(void) + { + if (m_cache[2] == 0x17) // ID == Bottom Track Data Record + return; - if ((pos = m_line.find('\n')) != std::string::npos) - throw std::runtime_error(m_line.substr(0, pos - 1)); + IMC::DevDataBinary data; + data.value.assign(m_cache, m_cache + m_cached); + dispatch(data); + } - break; + uint16_t + checksum(uint8_t *data, size_t len) + { + uint16_t rs = 0xB58C; + size_t nshorts = len >> 1; + len -= nshorts << 1; + + while (nshorts--) + { + rs += (uint16_t)data[0] | ((uint16_t)data[1] << 8); + data += 2; } + + if (len) + rs += ((uint16_t)data[0]) << 8; + + return rs; } void diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index bdf999dd36..749fe49cbc 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -43,9 +43,8 @@ namespace Sensors { using DUNE_NAMESPACES; - static const unsigned c_pnorbt7_fields = 10; static const double c_pwr_on_delay = 5.0; - static const double c_init_tout = 5.0; + static const double c_init_tout = 10.0; struct Arguments { @@ -141,7 +140,7 @@ namespace Sensors m_temp.setSourceEntity(getEntityId()); m_gvel.setSourceEntity(getEntityId()); - bind(this); + bind(this); bind(this); } @@ -225,22 +224,25 @@ namespace Sensors void onResourceInitialization(void) { + bool ok = false; Counter counter(c_init_tout); while (!stopping() && !counter.overflow()) { waitForMessages(counter.getRemaining()); - if (m_reader->getState() == 2) + if (m_reader->getState() >= MSTA_SEEK_HDR) { + ok = true; break; + } } - if (counter.overflow()) + if (!ok) throw std::runtime_error(DTR("failed to setup device")); setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); } void - consume(const IMC::DevDataText* msg) + consume(const IMC::DevDataBinary* msg) { if (msg->getDestination() != getSystemId()) return; @@ -248,7 +250,7 @@ namespace Sensors if (msg->getDestinationEntity() != getEntityId()) return; - processSentence(msg->value); + processFrame(&msg->value[0], msg->value.size()); } void @@ -264,91 +266,21 @@ namespace Sensors throw RestartNeeded(msg->error, 5); } - //! Read decimal from input string. - //! @param[in] str input string. - //! @param[out] dst decimal. - //! @return true if successful, false otherwise. - template - bool - readDecimal(const std::string& str, T& dst) - { - unsigned idx = 0; - while (str[idx] == '0') - ++idx; - - return castLexical(std::string(str, idx), dst); - } - - //! Read number from input string. - //! @param[in] str input string. - //! @param[out] dst number. - //! @return true if successful, false otherwise. - template - bool - readNumber(const std::string& str, T& dst) - { - return castLexical(str, dst); - } - - //! 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; - - // 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) - { - 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(const std::vector& parts) + processFrame(const char *data, size_t len) { - if (parts.size() != c_pnorbt7_fields) - { - war(DTR("invalid PNORBT7 sentence")); - return; - } - - readNumber(parts[2], m_gvel.x); - readNumber(parts[3], m_gvel.y); - readNumber(parts[4], m_gvel.z); + float vx, vy, vz; + std::memcpy(&vx, data + HDR_SIZE + 132, sizeof(float)); + std::memcpy(&vy, data + HDR_SIZE + 136, sizeof(float)); + std::memcpy(&vz, data + HDR_SIZE + 140, sizeof(float)); + m_gvel.x = vx; + m_gvel.y = vy; + m_gvel.z = vz; dispatch(m_gvel); + inf("BT7 %.2f, %.2f, %.2f", m_gvel.x, m_gvel.y, m_gvel.z); + + (void)len; } void From 13910ed8c3fcd1e29923ced3da6168467ab0f72b Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Wed, 10 Feb 2016 16:05:22 +0100 Subject: [PATCH 10/19] Sensors/NortekDVL: added support for several different binary frames --- src/Sensors/NortekDVL/Reader.hpp | 3 --- src/Sensors/NortekDVL/Task.cpp | 43 ++++++++++++++++++++++++++++---- 2 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index 8005f8d93b..8199f0504c 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -307,9 +307,6 @@ namespace Sensors void processFrame(void) { - if (m_cache[2] == 0x17) // ID == Bottom Track Data Record - return; - IMC::DevDataBinary data; data.value.assign(m_cache, m_cache + m_cached); dispatch(data); diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 749fe49cbc..e8c9308257 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -111,10 +111,6 @@ namespace Sensors .minimumValue("0.0") .description("Input rate"); - param("Sound Velocity", m_args.params.rate) - .defaultValue("0.0") - .description("Sound velocity"); - param("Sound Velocity", m_args.params.sndvel) .defaultValue("0.0") .description("Sound velocity"); @@ -268,6 +264,24 @@ namespace Sensors void processFrame(const char *data, size_t len) + { + switch (data[2]) + { + case 0x1B: + processBottomTrack(data, len); + break; + + case 0x15: + processCurrentProfile(data, len); + break; + + default: + inf("not supported: %" PRIx8, data[2]); + } + } + + void + processBottomTrack(const char *data, size_t len) { float vx, vy, vz; std::memcpy(&vx, data + HDR_SIZE + 132, sizeof(float)); @@ -278,11 +292,30 @@ namespace Sensors m_gvel.z = vz; dispatch(m_gvel); - inf("BT7 %.2f, %.2f, %.2f", m_gvel.x, m_gvel.y, m_gvel.z); + float prs; + std::memcpy(&prs, data + HDR_SIZE + 32, sizeof(float)); + m_prs.value = prs * 1000; + dispatch(m_prs); + + float temp; + std::memcpy(&temp, data + HDR_SIZE + 28, sizeof(float)); + m_temp.value = temp; + dispatch(m_temp); + + inf("vel: (%.2f, %.2f, %.2f), prs: %f, temp: %f", + m_gvel.x, m_gvel.y, m_gvel.z, + prs * 10, temp); (void)len; } + void + processCurrentProfile(const char *data, size_t len) + { + (void)data; + (void)len; + } + void onMain(void) { From 9f95b6f86ee30d7de39b4ceb8939cdef50b767e5 Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Wed, 10 Feb 2016 16:38:56 +0100 Subject: [PATCH 11/19] Sensors/NortekDVL: added current profiling --- src/Sensors/NortekDVL/Reader.hpp | 8 +++++++- src/Sensors/NortekDVL/Task.cpp | 15 +++++++++++---- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index 8199f0504c..e2a1609f93 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -162,7 +162,7 @@ namespace Sensors break; case 1: - str = String::str("SETDVL,0,\"OFF\",\"INTSR\",%.1f,\"\",%.1f,%.1f\r\n", + str = String::str("SETDVL,2,\"OFF\",\"INTSR\",%.1f,\"\",%.1f,%.1f\r\n", m_param.rate, m_param.sndvel, m_param.salinity); m_handle->writeString(str.c_str()); break; @@ -174,6 +174,12 @@ namespace Sensors break; case 3: + str = String::str("SETCURPROF,1,0.50,0.10,\"XYZ\",%f,0.000,%f,3,4,0\r\n", + m_param.pwr_level, m_param.v_range); + m_handle->writeString(str.c_str()); + break; + + case 4: m_handle->writeString("START\r\n"); break; diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index e8c9308257..bbb2b83840 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -271,8 +271,8 @@ namespace Sensors processBottomTrack(data, len); break; - case 0x15: - processCurrentProfile(data, len); + case 0x16: + processAverageData(data, len); break; default: @@ -310,9 +310,16 @@ namespace Sensors } void - processCurrentProfile(const char *data, size_t len) + processAverageData(const char *data, size_t len) { - (void)data; + uint16_t yaw; + int16_t roll, pitch; + std::memcpy(&roll, data + HDR_SIZE + 24, sizeof(uint16_t)); + std::memcpy(&pitch, data + HDR_SIZE + 26, sizeof(int16_t)); + std::memcpy(&yaw, data + HDR_SIZE + 28, sizeof(int16_t)); + + inf("rpy: (%.2f , %.2f, %.2f)", + (float)roll * 0.01, (float)pitch * 0.01, (float)yaw * 0.01); (void)len; } From 51b7d5c42b40eebba6c2406e6fa2a7d3beb6113c Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Fri, 12 Feb 2016 12:57:39 +0100 Subject: [PATCH 12/19] Sensors/NortekDVL: added checking of dvl-status before sending imc --- src/Sensors/NortekDVL/Task.cpp | 29 ++++++++++++++++++----------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index bbb2b83840..e1073336e8 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -283,6 +283,9 @@ namespace Sensors void processBottomTrack(const char *data, size_t len) { + uint32_t status; + std::memcpy(&status, data + HDR_SIZE + 20, sizeof(float)); + float vx, vy, vz; std::memcpy(&vx, data + HDR_SIZE + 132, sizeof(float)); std::memcpy(&vy, data + HDR_SIZE + 136, sizeof(float)); @@ -290,7 +293,9 @@ namespace Sensors m_gvel.x = vx; m_gvel.y = vy; m_gvel.z = vz; - dispatch(m_gvel); + + if (((status >> 12) & 0x07) == 0x07) + dispatch(m_gvel); float prs; std::memcpy(&prs, data + HDR_SIZE + 32, sizeof(float)); @@ -302,9 +307,10 @@ namespace Sensors m_temp.value = temp; dispatch(m_temp); - inf("vel: (%.2f, %.2f, %.2f), prs: %f, temp: %f", + spew("vel: (%.2f, %.2f, %.2f), prs: %.2f, temp: %.1f, valid_bits: %d%d%d", m_gvel.x, m_gvel.y, m_gvel.z, - prs * 10, temp); + prs * 10, temp, + (status >> 12) & 1, (status >> 13) & 1, (status >> 14) & 1); (void)len; } @@ -312,14 +318,15 @@ namespace Sensors void processAverageData(const char *data, size_t len) { - uint16_t yaw; - int16_t roll, pitch; - std::memcpy(&roll, data + HDR_SIZE + 24, sizeof(uint16_t)); - std::memcpy(&pitch, data + HDR_SIZE + 26, sizeof(int16_t)); - std::memcpy(&yaw, data + HDR_SIZE + 28, sizeof(int16_t)); - - inf("rpy: (%.2f , %.2f, %.2f)", - (float)roll * 0.01, (float)pitch * 0.01, (float)yaw * 0.01); + // uint16_t yaw; + // int16_t roll, pitch; + // std::memcpy(&roll, data + HDR_SIZE + 24, sizeof(uint16_t)); + // std::memcpy(&pitch, data + HDR_SIZE + 26, sizeof(int16_t)); + // std::memcpy(&yaw, data + HDR_SIZE + 28, sizeof(int16_t)); + + // spew("rpy: (%.2f , %.2f, %.2f)", + // (float)roll, (float)pitch, (float)yaw); + (void)data; (void)len; } From b6a20796d3923922c6f03cf7edec8187ce6df05a Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Fri, 1 Apr 2016 14:38:30 +0200 Subject: [PATCH 13/19] Sensors/NortekDVL: validity bits added to imc --- src/Sensors/NortekDVL/Task.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index e1073336e8..4f58908aec 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -284,15 +284,19 @@ namespace Sensors processBottomTrack(const char *data, size_t len) { uint32_t status; - std::memcpy(&status, data + HDR_SIZE + 20, sizeof(float)); + std::memcpy(&status, data + HDR_SIZE + 20, sizeof(uint32_t)); float vx, vy, vz; std::memcpy(&vx, data + HDR_SIZE + 132, sizeof(float)); std::memcpy(&vy, data + HDR_SIZE + 136, sizeof(float)); std::memcpy(&vz, data + HDR_SIZE + 140, sizeof(float)); + + // TODO: add rotation of DVL-frame + m_gvel.x = vx; m_gvel.y = vy; m_gvel.z = vz; + m_gvel.validity = (status >> 12) & 7; if (((status >> 12) & 0x07) == 0x07) dispatch(m_gvel); From 3b572bc69e8fc87fdd6d7e93654d5c8e24255a1c Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Mon, 25 Apr 2016 11:55:21 +0200 Subject: [PATCH 14/19] Sensors/NortekDVL: added frame-rotation support --- src/Sensors/NortekDVL/Task.cpp | 44 ++++++++++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 5 deletions(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 4f58908aec..530a20f3ae 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -36,6 +36,8 @@ // Local headers. #include "Reader.hpp" +const double deg2rad = M_PI / 180.0; + namespace Sensors { //! Device driver for NMEA capable %GPS devices. @@ -56,6 +58,8 @@ namespace Sensors float inp_tout; //! Power channels. std::vector pwr_channels; + //! Rotation angles of DVL-frame + std::vector rotation; Reader::NortekParam params; }; @@ -73,12 +77,15 @@ namespace Sensors std::string m_init_line; //! Reader thread. Reader* m_reader; + double m_dcm[9]; Task(const std::string& name, Tasks::Context& ctx): Tasks::Task(name, ctx), m_handle(NULL), m_reader(NULL) { + updateDCM(0, 0, 0); + // Define configuration parameters. param("Serial Port - Device", m_args.uart_dev) .defaultValue("") @@ -131,6 +138,11 @@ namespace Sensors .defaultValue("-20.0") .description("Power level"); + param("Rotation", m_args.rotation) + .defaultValue("0, 0, 0") + .size(3) + .description("Rotation angles of DVL-frame"); + m_euler.setSourceEntity(getEntityId()); m_prs.setSourceEntity(getEntityId()); m_temp.setSourceEntity(getEntityId()); @@ -155,6 +167,11 @@ namespace Sensors paramChanged(m_args.params.pwr_level)) m_reader->reconfigure(m_args.params); } + + if (paramChanged(m_args.rotation)) + updateDCM(m_args.rotation[0] * deg2rad, + m_args.rotation[1] * deg2rad, + m_args.rotation[2] * deg2rad); } void @@ -291,11 +308,9 @@ namespace Sensors std::memcpy(&vy, data + HDR_SIZE + 136, sizeof(float)); std::memcpy(&vz, data + HDR_SIZE + 140, sizeof(float)); - // TODO: add rotation of DVL-frame - - m_gvel.x = vx; - m_gvel.y = vy; - m_gvel.z = vz; + m_gvel.x = vx * m_dcm[0] + vy * m_dcm[1] + vz * m_dcm[2]; + m_gvel.y = vx * m_dcm[3] + vy * m_dcm[4] + vz * m_dcm[5]; + m_gvel.z = vx * m_dcm[6] + vy * m_dcm[7] + vz * m_dcm[8]; m_gvel.validity = (status >> 12) & 7; if (((status >> 12) & 0x07) == 0x07) @@ -334,6 +349,25 @@ namespace Sensors (void)len; } + void + updateDCM(double roll, double pitch, double yaw) + { + double cr = cos(roll), cp = cos(pitch), cy = cos(yaw); + double sr = sin(roll), sp = sin(pitch), sy = sin(yaw); + + m_dcm[0] = cp * cy; + m_dcm[1] = sr * sp * cy - cr * sy; + m_dcm[2] = cr * sp * cy + sr * sy; + + m_dcm[3] = cp * sy; + m_dcm[4] = sr * sp * sy + cr * cy; + m_dcm[5] = cr * sp * sy - sr * cy; + + m_dcm[6] = -sp; + m_dcm[7] = sr * cp; + m_dcm[8] = cr * cp; + } + void onMain(void) { From cc678f2228ed37c7e7d0a333a8ae3eb973f06492 Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Mon, 25 Apr 2016 11:58:21 +0200 Subject: [PATCH 15/19] Sensors/NortekDVL: fixed behavior: GroundVelocity was not sent in case of invalid measurements --- src/Sensors/NortekDVL/Task.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 530a20f3ae..28cb748e28 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -313,8 +313,7 @@ namespace Sensors m_gvel.z = vx * m_dcm[6] + vy * m_dcm[7] + vz * m_dcm[8]; m_gvel.validity = (status >> 12) & 7; - if (((status >> 12) & 0x07) == 0x07) - dispatch(m_gvel); + dispatch(m_gvel); float prs; std::memcpy(&prs, data + HDR_SIZE + 32, sizeof(float)); From 566f9875ba1d6906abd28197e77ddcb00f8f2bce Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Mon, 25 Apr 2016 15:53:54 +0200 Subject: [PATCH 16/19] Sensors/NortekDVL: fixed 2 Hz update rate: disabled current-profiling after every 2nd ping --- src/Sensors/NortekDVL/Reader.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index e2a1609f93..d45021bdae 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -162,7 +162,7 @@ namespace Sensors break; case 1: - str = String::str("SETDVL,2,\"OFF\",\"INTSR\",%.1f,\"\",%.1f,%.1f\r\n", + str = String::str("SETDVL,0,\"OFF\",\"INTSR\",%.1f,\"\",%.1f,%.1f\r\n", m_param.rate, m_param.sndvel, m_param.salinity); m_handle->writeString(str.c_str()); break; From 742c65219ada247849ef3e46c2701578e4a8b35a Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Mon, 25 Apr 2016 15:33:50 +0200 Subject: [PATCH 17/19] Sensors/NortekDVL: changed protocol to a new one --- src/Sensors/NortekDVL/Reader.hpp | 4 ++-- src/Sensors/NortekDVL/Task.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Sensors/NortekDVL/Reader.hpp b/src/Sensors/NortekDVL/Reader.hpp index d45021bdae..37bd064ccb 100644 --- a/src/Sensors/NortekDVL/Reader.hpp +++ b/src/Sensors/NortekDVL/Reader.hpp @@ -162,13 +162,13 @@ namespace Sensors break; case 1: - str = String::str("SETDVL,0,\"OFF\",\"INTSR\",%.1f,\"\",%.1f,%.1f\r\n", + str = String::str("SETDVL,0,\"INTSR\",%.1f,\"\",%.1f,%.1f\r\n", m_param.rate, m_param.sndvel, m_param.salinity); m_handle->writeString(str.c_str()); break; case 2: - str = String::str("SETBT,%.2f,%.2f,4,0,21,%.1f,\"XYZ\"\r\n", + str = String::str("SETBT,%.2f,%.2f,4,0,21,%.1f,\"OFF\",22,0.01,\"MAX\"\r\n", m_param.bt_range, m_param.v_range, m_param.pwr_level); m_handle->writeString(str.c_str()); break; diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 28cb748e28..50388b9216 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -114,7 +114,7 @@ namespace Sensors .description("Password to athenticate command interface"); param("Input Rate", m_args.params.rate) - .defaultValue("4.0") + .defaultValue("8.0") .minimumValue("0.0") .description("Input rate"); From 31ce8fd50ca527c9c1a37c3240043e988a30c36e Mon Sep 17 00:00:00 2001 From: Oleksandr Novychenko Date: Fri, 16 Sep 2016 14:00:04 +0200 Subject: [PATCH 18/19] Sensors/NortekDVL: added pressure-offset paremeter (NortekDVL) --- src/Sensors/NortekDVL/Task.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 50388b9216..659d6710dd 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -60,6 +60,8 @@ namespace Sensors std::vector pwr_channels; //! Rotation angles of DVL-frame std::vector rotation; + //! Pressure offset in hPa. + float poff; Reader::NortekParam params; }; @@ -143,6 +145,10 @@ namespace Sensors .size(3) .description("Rotation angles of DVL-frame"); + param("Pressure Offset", m_args.poff) + .defaultValue("0") + .description("Pressure offset in hPa"); + m_euler.setSourceEntity(getEntityId()); m_prs.setSourceEntity(getEntityId()); m_temp.setSourceEntity(getEntityId()); @@ -317,7 +323,7 @@ namespace Sensors float prs; std::memcpy(&prs, data + HDR_SIZE + 32, sizeof(float)); - m_prs.value = prs * 1000; + m_prs.value = prs * 1000 + m_args.poff; dispatch(m_prs); float temp; From 7b8dd09eb565fe5830ed0ff90475a48ed9a6effa Mon Sep 17 00:00:00 2001 From: Ievgenii Glushko Date: Wed, 21 Sep 2016 12:29:19 +0200 Subject: [PATCH 19/19] Sensors/NortekDVL: fix printf format specifier Cross-compiler does not support PRI.. format specifiers. Change-Id: Ie32e5e1e21cfb4e0804c965a2f2298a01bfd5a5b --- src/Sensors/NortekDVL/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Sensors/NortekDVL/Task.cpp b/src/Sensors/NortekDVL/Task.cpp index 659d6710dd..5bddf1907f 100644 --- a/src/Sensors/NortekDVL/Task.cpp +++ b/src/Sensors/NortekDVL/Task.cpp @@ -299,7 +299,7 @@ namespace Sensors break; default: - inf("not supported: %" PRIx8, data[2]); + inf("not supported: %x", data[2]); } }