diff --git a/etc/hardware/acoustic-modems/seatrac_uart.ini b/etc/hardware/acoustic-modems/seatrac_uart.ini index 51d429f7af..7d575da1d4 100644 --- a/etc/hardware/acoustic-modems/seatrac_uart.ini +++ b/etc/hardware/acoustic-modems/seatrac_uart.ini @@ -48,6 +48,9 @@ Serial Port - Device = /dev/uart/1 Pressure Sensor Mode = true Address Section = Seatrac Addresses Debug Level = Debug +Honour Medium = false +Pressure Sensor Mode = true +Use Internal Pressure Sensor as Medium = true [UserInterfaces.MantaPanel] Sections of System Addresses = Seatrac Addresses diff --git a/etc/hardware/lctr-a6xx/seatrac.ini b/etc/hardware/lctr-a6xx/seatrac.ini index d68e397e91..898d7e0637 100644 --- a/etc/hardware/lctr-a6xx/seatrac.ini +++ b/etc/hardware/lctr-a6xx/seatrac.ini @@ -32,22 +32,26 @@ [Transports.Seatrac] Enabled = Hardware Debug Level = None +Active = true # keep always active for LAUVs Entity Label = Acoustic Modem Serial Port - Device = /dev/uart/10 Serial Port - Baud Rate = 115200 -Activation Time = 3.5 +Activation Time = 5 Deactivation Time = 0 Transmit Only Underwater = true Address Section = Seatrac Addresses +Honour Medium = true Max Range = 1000 -AHRS Mode = false +Turn Around Time = 10 Pressure Sensor Mode = true -Use Internal Pressure Sensor for Medium = true +Use Internal Pressure Sensor as Medium = false USBL Mode = true Enhanced USBL = true +# If modem not installed upright either invert output with Blueprint's Seatrac Tools or rotate matrix 180º around Y-Axis (refer to "Beacon User Manual" section 6.5) AHRS Rotation Matrix = 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 +Calibration Threshold = 0.005 Hard-Iron Calibration = 0.0, 0.0, 0.0 [Transports.UAN] diff --git a/src/DUNE/Network.hpp b/src/DUNE/Network.hpp index 36e6841b5d..5ac8c8ce36 100644 --- a/src/DUNE/Network.hpp +++ b/src/DUNE/Network.hpp @@ -46,5 +46,6 @@ namespace DUNE #include #include #include +#include #endif diff --git a/src/DUNE/Network/NodeMap.hpp b/src/DUNE/Network/NodeMap.hpp new file mode 100644 index 0000000000..78102c3058 --- /dev/null +++ b/src/DUNE/Network/NodeMap.hpp @@ -0,0 +1,112 @@ +//*************************************************************************** +// Copyright 2017 OceanScan - Marine Systems & Technology, Lda. * +//*************************************************************************** +// This file is subject to the terms and conditions defined in file * +// 'LICENCE.md', which is part of this source code package. * +//*************************************************************************** +// Author: Ricardo Martins * +//*************************************************************************** + +#ifndef DUNE_NETWORK_NODE_MAP_HPP_INCLUDED_ +#define DUNE_NETWORK_NODE_MAP_HPP_INCLUDED_ + +// ISO C++ 98 headers. +#include +#include + +// DUNE headers. +#include + +namespace DUNE +{ + namespace Network + { + //! Utility class that enables mapping node names to addresses and + //! vice versa. + template + class NodeMap + { + public: + //! Initialize node map from configuration section. + //! + //! @param[in] config config parser instance. + //! @param[in] section config section name. + void + readConfigSection(Parsers::Config& config, const std::string& section) + { + clear(); + + std::vector addrs = config.options(section); + for (unsigned i = 0; i < addrs.size(); ++i) + { + T addr; + config.get(section, addrs[i], "", addr); + add(addrs[i], addr); + } + } + + //! Add a node to the map. + //! + //! @param[in] name canonical node name. + //! @param[in] address node address. + void + add(const std::string& name, const T& address) + { + m_node_names[name] = address; + m_node_addresses[address] = name; + } + + //! Remove all elements from the map. + void + clear() + { + m_node_names.clear(); + m_node_addresses.clear(); + } + + //! Lookup a node's address given its name. + //! + //! @param[in] name canonical node name. + //! @param[out] address node address. + //! + //! @return true if the canonical node name has a corresponding + //! address, false otherwise. + bool + lookupAddress(const std::string& name, T& address) const + { + typename std::map::const_iterator itr = m_node_names.find(name); + if (itr == m_node_names.end()) + return false; + + address = itr->second; + return true; + } + + //! Lookup a node's name given its address. + //! + //! @param[in] address node address. + //! @param[out] name canonical node name. + //! + //! @return true if the node address has a corresponding + //! canonical name, false otherwise. + bool + lookupName(const T& address, std::string& name) const + { + typename std::map::const_iterator itr = m_node_addresses.find(address); + if (itr == m_node_addresses.end()) + return false; + + name = itr->second; + return true; + } + + private: + //! Map node names to addresses. + std::map m_node_names; + //! Map node addresses to names. + std::map m_node_addresses; + }; + } +} + +#endif diff --git a/src/Transports/Seatrac/DataTypes.hpp b/src/Transports/Seatrac/Constants.hpp similarity index 89% rename from src/Transports/Seatrac/DataTypes.hpp rename to src/Transports/Seatrac/Constants.hpp index d3881a7326..e477421e1e 100644 --- a/src/Transports/Seatrac/DataTypes.hpp +++ b/src/Transports/Seatrac/Constants.hpp @@ -2,6 +2,8 @@ // Copyright 2007-2023 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** +// Copyright 2023 OceanScan - Marine Systems & Technology, Lda. * +//*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * // * // Commercial Licence Usage * @@ -26,10 +28,11 @@ //*************************************************************************** // Author: João Teixeira * // Author: Raúl Sáez * +// Author: Maria Costa (small refactor and fix for iUSBL implementation) * //*************************************************************************** -#ifndef TRANSPORTS_SEATRAC_DATA_TYPES_HPP_INCLUDED_ -#define TRANSPORTS_SEATRAC_DATA_TYPES_HPP_INCLUDED_ +#ifndef TRANSPORTS_SEATRAC_CONSTANTS_HPP_INCLUDED_ +#define TRANSPORTS_SEATRAC_CONSTANTS_HPP_INCLUDED_ #define MESSAGE_NUMBER (0x77 +0x1) #define MAX_MESSAGE_ERRORS 5 @@ -39,20 +42,46 @@ //! Defines the minimum message length without preamble nor postamble #define MIN_MESSAGE_LENGTH 6 +// ISO C++ 98 headers. +#include +#include + +// DUNE headers. +#include + namespace Transports { namespace Seatrac { using DUNE_NAMESPACES; + //! Hard Iron calibration parameter name. + static const std::string c_hard_iron_param = "Hard-Iron Calibration"; + //! Number of axis. + static const uint8_t c_number_axis = 3; + //! Acknowledged timeout time multiplier + static const uint8_t c_ack_timeout_multiplier = 6; //! Input Timeout (s). - static const double c_input_tout = 5; + static const double c_input_tout = 5; //! The bitrate of acoustic communication (bits/second). - static const double c_acoustic_bitrate = 100; + static const double c_acoustic_bitrate = 100; //! Message preamble static const char c_preamble = '$'; //! Maximum buffer size. static const int c_bfr_size = 256; + //! Sound speed update window (m/s). + static const uint16_t c_sspeed_window = 1; + + //! Entity states. + enum EntityStates + { + STA_BOOT, + STA_IDLE, + STA_ACTIVE, + STA_ERR_COM, + STA_ERR_STP, + STA_MAX + }; //! States of the internal SM. enum ParserStates @@ -137,7 +166,6 @@ namespace Transports BT_X150 = 0x31B }; - // Status Output Mode enum StatusMode_E { @@ -174,7 +202,8 @@ namespace Transports }; // Control XCVR flags - enum ControlXcvrFlags_E { + enum ControlXcvrFlags_E + { USBL_USE_AHRS_FLAG = 0x1, XCVR_POSFLT_ENABLE_FLAG = 0x2, XCVR_USBL_MSGS_FLAG = 0x20, @@ -338,7 +367,7 @@ namespace Transports int16_t position_easting; int16_t position_northing; int16_t position_depth; - uint8_t outputflags_list[4]; + uint8_t outputflags_list[5]; void outputFlagsComp(void) @@ -347,6 +376,7 @@ namespace Transports outputflags_list[1] = (0x02 & flags); outputflags_list[2] = (0x04 & flags); outputflags_list[3] = (0x08 & flags); + outputflags_list[4] = (0x10 & flags); } }; } diff --git a/src/Transports/Seatrac/DebugMsg.hpp b/src/Transports/Seatrac/DebugMsg.hpp index 36d3977dbd..581e62af97 100644 --- a/src/Transports/Seatrac/DebugMsg.hpp +++ b/src/Transports/Seatrac/DebugMsg.hpp @@ -2,6 +2,8 @@ // Copyright 2007-2023 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** +// Copyright 2023 OceanScan - Marine Systems & Technology, Lda. * +//*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * // * // Commercial Licence Usage * @@ -26,6 +28,7 @@ //*************************************************************************** // Author: João Teixeira * // Author: Raúl Sáez * +// Author: Maria Costa (small refactor and fix for iUSBL implementation) * //*************************************************************************** #ifndef TRANSPORTS_SEATRAC_DEBUG_MSG_HPP_INCLUDED_ @@ -39,7 +42,7 @@ #include // Local headers -#include "MsgTypes.hpp" +#include "Parser.hpp" namespace Transports { @@ -62,28 +65,28 @@ namespace Transports msg_name.c_str(), aco_fix->flags); task->debug("data_Beacon.%s.aco_fix.amsgtype_e %d ", msg_name.c_str(), aco_fix->amsgtype_e); - task->debug("data_Beacon.%s.aco_fix.attitude_yaw %d ", - msg_name.c_str(), aco_fix->attitude_yaw); - task->debug("data_Beacon.%s.aco_fix.attitude_pitch %d ", - msg_name.c_str(), aco_fix->attitude_pitch); - task->debug("data_Beacon.%s.aco_fix.attitude_roll %d ", - msg_name.c_str(), aco_fix->attitude_roll); - task->debug("data_Beacon.%s.aco_fix.depth_local %d ", - msg_name.c_str(), aco_fix->depth_local); - task->debug("data_Beacon.%s.aco_fix.vos %d ", - msg_name.c_str(), aco_fix->vos); - task->debug("data_Beacon.%s.aco_fix.rssi %d ", - msg_name.c_str(), aco_fix->rssi); + task->debug("data_Beacon.%s.aco_fix.attitude_yaw/10 %d \u00B0 ", + msg_name.c_str(), aco_fix->attitude_yaw/10); + task->debug("data_Beacon.%s.aco_fix.attitude_pitch/10 %d \u00B0 ", + msg_name.c_str(), aco_fix->attitude_pitch/10); + task->debug("data_Beacon.%s.aco_fix.attitude_roll/10 %d \u00B0 ", + msg_name.c_str(), aco_fix->attitude_roll/10); + task->debug("data_Beacon.%s.aco_fix.depth_local/10 %d m ", + msg_name.c_str(), aco_fix->depth_local/10); + task->debug("data_Beacon.%s.aco_fix.vos/10 %d m/s ", + msg_name.c_str(), aco_fix->vos/10); + task->debug("data_Beacon.%s.aco_fix.rssi/10 %d dB", + msg_name.c_str(), aco_fix->rssi/10); // Range fields. if (aco_fix->outputflags_list[0]) { task->debug("data_Beacon.%s.aco_fix.range_count %d ", msg_name.c_str(), aco_fix->range_count); - task->debug("data_Beacon.%s.aco_fix.range_time %d ", - msg_name.c_str(), aco_fix->range_time); - task->debug("data_Beacon.%s.aco_fix.range_dist %d ", - msg_name.c_str(), aco_fix->range_dist); + task->debug("data_Beacon.%s.aco_fix.range_time/10000000 %d s ", + msg_name.c_str(), aco_fix->range_time/10000000); + task->debug("data_Beacon.%s.aco_fix.range_dist/10 %d m ", + msg_name.c_str(), aco_fix->range_dist/10); } // USBL fields. @@ -93,26 +96,26 @@ namespace Transports msg_name.c_str(), aco_fix->usbl_channels); for (int i = 0; i < aco_fix->usbl_channels; i++) - task->debug("data_Beacon.%s.aco_fix.usbl_rssi[i] %d", - msg_name.c_str(), aco_fix->usbl_rssi[i]); - - task->debug("data_Beacon.%s.aco_fix.usbl_azimuth %d", - msg_name.c_str(), aco_fix->usbl_azimuth); - task->debug("data_Beacon.%s.aco_fix.usbl_elevation %d", - msg_name.c_str(), aco_fix->usbl_elevation); - task->debug("data_Beacon.%s.aco_fix.usbl_fit_error %d", - msg_name.c_str(), aco_fix->usbl_fit_error); + task->debug("data_Beacon.%s.aco_fix.usbl_rssi[i]/10 %d dB", + msg_name.c_str(), aco_fix->usbl_rssi[i]/10); + + task->debug("data_Beacon.%s.aco_fix.usbl_azimuth/10 %d \u00B0 ", + msg_name.c_str(), aco_fix->usbl_azimuth/10); + task->debug("data_Beacon.%s.aco_fix.usbl_elevation/10 %d \u00B0 ", + msg_name.c_str(), aco_fix->usbl_elevation/10); + task->debug("data_Beacon.%s.aco_fix.usbl_fit_error/100 %d ", + msg_name.c_str(), aco_fix->usbl_fit_error/100); } // Position fields. if (aco_fix->outputflags_list[2]) { - task->debug("data_Beacon.%s.aco_fix.position_easting %ld", - msg_name.c_str(), (long int) aco_fix->position_easting); - task->debug("data_Beacon.%s.aco_fix.position_northing %ld", - msg_name.c_str(), (long int) aco_fix->position_northing); - task->debug("data_Beacon.%s.aco_fix.position_depth %ld", - msg_name.c_str(), (long int) aco_fix->position_depth); + task->debug("data_Beacon.%s.aco_fix.position_easting/10 %ld m", + msg_name.c_str(), (long int) aco_fix->position_easting/10); + task->debug("data_Beacon.%s.aco_fix.position_northing/10 %ld m", + msg_name.c_str(), (long int) aco_fix->position_northing/10); + task->debug("data_Beacon.%s.aco_fix.position_depth/10 %ld m", + msg_name.c_str(), (long int) aco_fix->position_depth/10); } } @@ -136,8 +139,8 @@ namespace Transports // Environment. if (data_Beacon.cid_status_msg.outputflags_list[0]) { - task->spew("data_Beacon.cid_status_msg.environment_supply %d", - data_Beacon.cid_status_msg.environment_supply); + task->spew("data_Beacon.cid_status_msg.environment_supply/1000 %f V", + data_Beacon.cid_status_msg.environment_supply / 1000.0); task->spew("data_Beacon.cid_status_msg.environment_temperature/10 %f \u00B0C", data_Beacon.cid_status_msg.environment_temperature / 10.0); task->spew("data_Beacon.cid_status_msg.environment_pressure %d mbar", @@ -286,7 +289,6 @@ namespace Transports (int)data_Beacon.cid_dat_send_msg.beacon_id); break; case CID_DAT_RECEIVE: //8.3. DAT Protocol Messages - task->debug("MESSAGE CID_DAT_RECEIVE "); printAcoFixData("cid_dat_receive_msg", &data_Beacon.cid_dat_receive_msg.aco_fix, task); @@ -418,8 +420,8 @@ namespace Transports break; case CID_SETTINGS_SET: - task->debug("MESSAGE CID_SETTINGS_SET"); - task->debug("data_Beacon.cid_sys_settings_set_msg.status 0x%02X", + task->trace("MESSAGE CID_SETTINGS_SET"); + task->trace("data_Beacon.cid_sys_settings_set_msg.status 0x%02X", (unsigned)data_Beacon.cid_sys_settings_set_msg.status); break; @@ -437,30 +439,30 @@ namespace Transports case CID_NAV_QUERY_RESP: task->debug("MESSAGE CID_NAV_QUERY_RESP"); - printAcoFixData("cid_nav_querry_resp_msg", - &data_Beacon.cid_nav_querry_resp_msg.aco_fix, task); - task->debug("data_Beacon.cid_nav_querry_resp_msg.query_flags %d ", - data_Beacon.cid_nav_querry_resp_msg.query_flags); - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[0]) - task->debug("data_Beacon.cid_nav_querry_resp_msg.remote_depth %d", - data_Beacon.cid_nav_querry_resp_msg.remote_depth); - - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[1]) - task->debug("data_Beacon.cid_nav_querry_resp_msg.remote_supply %d", - data_Beacon.cid_nav_querry_resp_msg.remote_supply); - - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[2]) - task->debug("data_Beacon.cid_nav_querry_resp_msg.remote_temp %d", - data_Beacon.cid_nav_querry_resp_msg.remote_temp); - - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[3]) + printAcoFixData("cid_nav_query_resp_msg", + &data_Beacon.cid_nav_query_resp_msg.aco_fix, task); + task->debug("data_Beacon.cid_nav_query_resp_msg.query_flags %d ", + data_Beacon.cid_nav_query_resp_msg.query_flags); + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[0]) + task->debug("data_Beacon.cid_nav_query_resp_msg.remote_depth %d", + data_Beacon.cid_nav_query_resp_msg.remote_depth); + + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[1]) + task->debug("data_Beacon.cid_nav_query_resp_msg.remote_supply %d", + data_Beacon.cid_nav_query_resp_msg.remote_supply); + + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[2]) + task->debug("data_Beacon.cid_nav_query_resp_msg.remote_temp %d", + data_Beacon.cid_nav_query_resp_msg.remote_temp); + + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[3]) { - task->debug("data_Beacon.cid_nav_querry_resp_msg.remote_yaw %d", - data_Beacon.cid_nav_querry_resp_msg.remote_yaw); - task->debug("data_Beacon.cid_nav_querry_resp_msg.remote_pitch %d", - data_Beacon.cid_nav_querry_resp_msg.remote_pitch); - task->debug("data_Beacon.cid_nav_querry_resp_msg.remote_roll %d", - data_Beacon.cid_nav_querry_resp_msg.remote_roll); + task->debug("data_Beacon.cid_nav_query_resp_msg.remote_yaw %d", + data_Beacon.cid_nav_query_resp_msg.remote_yaw); + task->debug("data_Beacon.cid_nav_query_resp_msg.remote_pitch %d", + data_Beacon.cid_nav_query_resp_msg.remote_pitch); + task->debug("data_Beacon.cid_nav_query_resp_msg.remote_roll %d", + data_Beacon.cid_nav_query_resp_msg.remote_roll); } break; diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp new file mode 100644 index 0000000000..e95d03b7f1 --- /dev/null +++ b/src/Transports/Seatrac/Driver.hpp @@ -0,0 +1,416 @@ +//*************************************************************************** +// Copyright 2007-2023 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// Copyright 2023 OceanScan - Marine Systems & Technology, Lda. * +//*************************************************************************** +// 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 * +// http://ec.europa.eu/idabc/eupl.hhtml. * +//*************************************************************************** +// Author: Maria Costa (small refactor and fix for iUSBL implementation) * +//*************************************************************************** + +#ifndef TRANSPORTS_SEATRAC_DRIVER_HPP_INCLUDED_ +#define TRANSPORTS_SEATRAC_DRIVER_HPP_INCLUDED_ + +// ISO C++ 98 headers. +#include + +// DUNE headers. +#include + +// Local headers. +#include "DebugMsg.hpp" +#include "Parser.hpp" + +namespace Transports +{ + namespace Seatrac + { + class Driver + { + public: + //! @param[in] task parent task. + //! @param[in] handle I/O handle. + //! @param[in] addr local modem address. + //! @param[out] tstamp read timestamp. + //! @param[out] last_input time of last received input. + //! @param[out] preamble c_preambple detected. + //! @param[out] hard_iron_x AHRS hard-iron calibration parameter (x). + //! @param[out] hard_iron_y AHRS hard-iron calibration parameter (y). + //! @param[out] hard_iron_z AHRS hard-iron calibration parameter (z). + //! @param[out] data_seatrac seatrac data structure where to store received data. + Driver(DUNE::Tasks::Task* task, DUNE::IO::Handle* handle, Seatrac::DataSeatrac& data_seatrac, double& tstamp, double& last_input, bool& preamble, unsigned addr, + float hard_iron_x, float hard_iron_y, float hard_iron_z): + m_handle(handle), + m_task(task), + m_data_beacon(data_seatrac), + m_tstamp(tstamp), + m_last_input(last_input), + m_preamble(preamble), + m_addr(addr), + m_hard_iron_x(hard_iron_x), + m_hard_iron_y(hard_iron_y), + m_hard_iron_z(hard_iron_z) + { + m_task->spew("creating driver"); + m_task->spew("flushing input"); + //! Constructor. + //! + //! @param[in] task parent task. + //! @param[in] handle I/O handle. + DUNE::Time::Delay::wait(1.0); + m_handle->flushInput(); + if (!Poll::poll(*m_handle, 1.0)) + throw std::runtime_error(DTR("failed to poll from modem")); + } + + //! Destructor. + ~Driver() + { } + + //! Seatrac Configuration + //! @param[in] ahrs_mode true if AHRS mode enabled. + //! @param[in] max_range maximum range. + //! @param[in] turn_around_time turn around time. + //! @param[in] sspeed_def default water sound speed. + //! @param[out] usbl_receiver true if USBL modem. + //! @return entity state after modem configuration. + EntityStates + configure(bool& usbl_receiver, bool ahrs_mode, uint16_t max_range, uint16_t turn_around_time, uint16_t sspeed_def) + { + // Retrieve current settings and system information + sendCommandAndWait(createCommand(CID_SETTINGS_GET, m_data_beacon), 1); + sendCommandAndWait(createCommand(CID_SYS_INFO, m_data_beacon), 1); + + if (m_data_beacon.cid_sys_info.hardware.part_number == BT_X150) + usbl_receiver = true; + + uint8_t output_flags = (ENVIRONMENT_FLAG | ATTITUDE_FLAG + | MAG_CAL_FLAG | ACC_CAL_FLAG + | AHRS_RAW_DATA_FLAG | AHRS_COMP_DATA_FLAG); + + uint8_t xcvr_flags = XCVR_FIX_MSGS_FLAG | XCVR_POSFLT_ENABLE_FLAG; + + if (usbl_receiver) + { + if (ahrs_mode) + xcvr_flags |= USBL_USE_AHRS_FLAG | XCVR_USBL_MSGS_FLAG; + else + xcvr_flags |= XCVR_USBL_MSGS_FLAG; + } + + StatusMode_E status_mode = STATUS_MODE_1HZ; + bool ahrs = true; + EntityStates state; + if (ahrs_mode) + { + status_mode = STATUS_MODE_10HZ; + ahrs = isCalibrated(); + } + + // Verify modem settings + if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs, max_range, turn_around_time, sspeed_def)) + { + // Setting correct settings + m_data_beacon.cid_settings_msg.status_flags = status_mode; + m_data_beacon.cid_settings_msg.status_output = output_flags; + m_data_beacon.cid_settings_msg.xcvr_flags = xcvr_flags; + m_data_beacon.cid_settings_msg.xcvr_beacon_id = m_addr; + m_data_beacon.cid_settings_msg.xcvr_range_tmo = max_range; + m_data_beacon.cid_settings_msg.xcvr_resp_time = turn_around_time; + m_data_beacon.cid_settings_msg.env_vos = sspeed_def * 10; + + if(!ahrs) + { + m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x = m_hard_iron_x; + m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y = m_hard_iron_y; + m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z = m_hard_iron_z; + } + + // Updating settings and saving if needed + m_task->inf("Sending updated settings to modem"); + sendCommandAndWait(createCommand(CID_SETTINGS_SET, m_data_beacon), 2); + if(!ahrs) + { + m_task->inf("Saving settings to modem"); + sendCommandAndWait(createCommand(CID_SETTINGS_SAVE, m_data_beacon), 2); + m_task->inf("Rebooting modem"); + sendCommandAndWait(createCommand(CID_SYS_REBOOT, m_data_beacon), 6); + } + sendCommandAndWait(createCommand(CID_SETTINGS_GET, m_data_beacon), 2); + + // Check modem settings again + if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs, max_range, turn_around_time, sspeed_def)) + { + state = STA_ERR_STP; + m_task->war(DTR("Failed to configure device")); + } + + // Configuration complete + m_task->inf("Modem ready (settings successfully set)."); + state = EntityStates::STA_IDLE; + } + else + { + // Configuration complete + m_task->inf("Modem ready."); + state = EntityStates::STA_IDLE; + } + + // Print system information + m_task->inf(DTR("Beacon id=%d | HW P#%d (rev#%d) serial#%d | FW P#%d v%d.%d.%d | App P#%d v%d.%d.%d |%s USBL beacon"), + m_data_beacon.cid_settings_msg.xcvr_beacon_id, + m_data_beacon.cid_sys_info.hardware.part_number, + m_data_beacon.cid_sys_info.hardware.part_rev, + m_data_beacon.cid_sys_info.hardware.serial_number, + m_data_beacon.cid_sys_info.boot_firmware.part_number, + m_data_beacon.cid_sys_info.boot_firmware.version_maj, + m_data_beacon.cid_sys_info.boot_firmware.version_min, + m_data_beacon.cid_sys_info.boot_firmware.version_build, + m_data_beacon.cid_sys_info.main_firmware.part_number, + m_data_beacon.cid_sys_info.main_firmware.version_maj, + m_data_beacon.cid_sys_info.main_firmware.version_min, + m_data_beacon.cid_sys_info.main_firmware.version_build, + usbl_receiver ? "" : " Not"); + + return state; + } + + //! Routine to check if AHRS has matching hard-iron calibration parameters and set new values if needed. + void + runCalibration(void) + { + // Check if vehicle has matching hard-iron calibration parameters. + if (!isCalibrated()) + { + // Set hard-iron calibration parameters and reset device. + if (!setHardIron()) + { + throw RestartNeeded(DTR("failed to set hard-iron correction factors"), 5); + } + } + } + + //! Read sentence from modem. + void + readSentence(void) + { + // Initialize received message parser + char bfr[c_bfr_size]; + uint8_t type = 0; + const char* msg_raw; + + if (!Poll::poll(*m_handle, 0.001)) + return; + + size_t rv = m_handle->readString(bfr, c_bfr_size); + if (rv == 0) + return; + + m_tstamp = Clock::getSinceEpoch(); + m_last_input = Clock::get(); + for (size_t i = 0; i < rv; ++i) + { + // Detected line termination. + if (bfr[i] == '\n') + { + m_dev_data.value.assign(sanitize(m_data)); + m_task->dispatch(m_dev_data); + if (m_preamble) + { + if (checkValidity()) + { + msg_raw = m_datahex.data(); + std::memcpy(&type, msg_raw, 1); + dataParser(type, msg_raw + 1, m_data_beacon); + printDebugFunction(type, m_data_beacon, m_task); + type = 0; + } + } + m_preamble = false; + m_data.clear(); + } + else + { + if (bfr[i] == c_preamble) + { + m_data.clear(); + m_preamble = true; + } + else if (bfr[i] != '\r') + m_data.push_back(bfr[i]); + } + } + } + + //! Send command to the acoustic modem. + //! @param[in] cmd command string. + void + sendCommand(const std::string& cmd) + { + m_handle->writeString(cmd.c_str()); + m_task->spew(DTR("Sent command to the acoustic modem: %s"), cmd.c_str()); + m_dev_data.value.assign(sanitize(cmd)); + m_task->dispatch(m_dev_data); + } + + private: + //! I/O handle. + IO::Handle* m_handle; + //! Parent task. + Tasks::Task* m_task; + //! Seatrac data structure. + DataSeatrac& m_data_beacon; + //! Read timestamp. + double& m_tstamp; + //! Time of last received input. + double& m_last_input; + //! c_preamble detected + bool& m_preamble; + //! Modem address. + unsigned m_addr; + //! Hard-iron calibration parameter (x). + float m_hard_iron_x; + //! Hard-iron calibration parameter (y). + float m_hard_iron_y; + //! Hard-iron calibration parameter (z). + float m_hard_iron_z; + //! Data buffer. + std::string m_data; + //! Converted data buffer. + std::string m_datahex; + //! Save modem commands. + IMC::DevDataText m_dev_data; + + //! Verification of current settings + //! @param[in] status_mode status output mode + //! @param[in] output_flags status output flags + //! @param[in] xcvr_flags control XCVR flags + //! @param[in] ahrs AHRS hard-iron calibration parameters match (when AHRS mode enabled) + //! @param[in] max_range maximum range + //! @param[in] turn_around_time turn around time + //! @param[in] sspeed_def default water sound speed + //! @return true if settings match, false otherwise + bool + checkSettings(StatusMode_E status_mode, uint8_t output_flags, uint8_t xcvr_flags, bool ahrs, uint16_t max_range, uint16_t turn_around_time, uint16_t sspeed_def) + { + return ((m_data_beacon.cid_settings_msg.xcvr_beacon_id == m_addr) + && (m_data_beacon.cid_settings_msg.status_flags == status_mode) + && (m_data_beacon.cid_settings_msg.status_output == output_flags) + && (m_data_beacon.cid_settings_msg.xcvr_flags == xcvr_flags) + && (m_data_beacon.cid_settings_msg.xcvr_range_tmo == max_range) + && (m_data_beacon.cid_settings_msg.xcvr_resp_time == turn_around_time) + && (m_data_beacon.cid_settings_msg.env_vos == sspeed_def * 10) + && ahrs); + } + + //! Check if AHRS sensor has matching hard-iron calibration parameters. + //! @return true if the parameters are the same, false otherwise. + bool + isCalibrated(void) + { + if( ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x*100)) != ((int32_t) (m_hard_iron_x*100)) + || ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y*100)) != ((int32_t) (m_hard_iron_y*100)) + || ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z*100)) != ((int32_t) (m_hard_iron_z*100))) + { + m_task->inf(DTR("AHRS has different calibration parameters.")); + return false; + } + + m_task->inf("AHRS is calibrated (matching parameters)."); + return true; + } + + //! Set new hard-iron calibration parameters. + //! @return true if successful, false otherwise. + bool + setHardIron(void) + { + m_task->inf("Set new hard-iron calibration parameters and reset device."); + m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x = m_hard_iron_x; + m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y = m_hard_iron_y; + m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z = m_hard_iron_z; + sendCommandAndWait(createCommand(CID_SETTINGS_SET, m_data_beacon), 2); + sendCommandAndWait(createCommand(CID_SETTINGS_SAVE, m_data_beacon), 2); + sendCommandAndWait(createCommand(CID_SETTINGS_GET, m_data_beacon), 2); + + if((m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x != m_hard_iron_x) + || (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y != m_hard_iron_y) + || (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z != m_hard_iron_z)) + return false; + + return true; + } + + //! Process sentence to check validity (CRC and minimum length). + //! @return true if message is valid, false otherwise. + bool + checkValidity(void) + { + bool msg_validity = false; + uint16_t crc, crc2; + if(m_data.size() >= MIN_MESSAGE_LENGTH) + { + std::string msg = String::fromHex(m_data.substr((m_data.size() - 4), 4)); + std::memcpy(&crc2, msg.data(), 2); + m_datahex = String::fromHex(m_data.erase((m_data.size() - 4), 4)); + crc = CRC16::compute((uint8_t*) m_datahex.data(), m_datahex.size(),0); + if (crc == crc2) + msg_validity = true; + else + m_task->war("Received message not valid: %s", DTR(Status::getString(Status::CODE_INVALID_CHECKSUM))); + } + else + m_task->war(DTR("Received message not valid: insufficient message length")); + return msg_validity; + } + + //! Send command and waits for response. + //! @param[in] cmd command string. + //! @param[in] wait wait time for reply after sending command. + void + sendCommandAndWait(const std::string& cmd, double wait) + { + // Send command + sendCommand(cmd); + // Wait for reply + processReply(wait); + } + + //! Wait for reply and process it. + //! @param[in] timeout time to wait for reply. + void + processReply(double timeout = 1.0) + { + double deadline = Clock::get() + timeout; + do + { + readSentence(); + } + while (Clock::get() <= deadline); + } + + }; + } +} + +#endif diff --git a/src/Transports/Seatrac/MsgTypes.hpp b/src/Transports/Seatrac/MsgTypes.hpp index 38cbf378c0..cfbed74271 100644 --- a/src/Transports/Seatrac/MsgTypes.hpp +++ b/src/Transports/Seatrac/MsgTypes.hpp @@ -2,6 +2,8 @@ // Copyright 2007-2023 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** +// Copyright 2023 OceanScan - Marine Systems & Technology, Lda. * +//*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * // * // Commercial Licence Usage * @@ -26,6 +28,7 @@ //*************************************************************************** // Author: João Teixeira * // Author: Raúl Sáez * +// Author: Maria Costa (small refactor and fix for iUSBL implementation) * //*************************************************************************** #ifndef TRANSPORTS_SEATRAC_MSG_TYPES_HPP_INCLUDED_ @@ -40,7 +43,7 @@ #include // Local headers -#include "DataTypes.hpp" +#include "Constants.hpp" namespace Transports { @@ -66,7 +69,7 @@ namespace Transports // Mag cal. uint8_t mag_cal_buf; bool mag_cal_valid; - int32_t mag_cal_age; + uint32_t mag_cal_age; uint8_t mag_cal_fit; // Acc cal. int16_t acc_lim_min_x; @@ -107,7 +110,6 @@ namespace Transports outputflags_list[4] = (AHRS_RAW_DATA_FLAG & output_flags); outputflags_list[5] = (AHRS_COMP_DATA_FLAG & output_flags); } - }; struct CidPingRespMsg @@ -148,7 +150,7 @@ namespace Transports uint8_t status; }; - struct CidPingErrorMsg + struct CidPingErrorMsg { CST_E status; uint8_t beacon_id; @@ -297,15 +299,15 @@ namespace Transports int packetDataBuild(std::vector msg, int dest_id_t) { - int erro_code = 1; + int error_code = 1; if (msg.size() % 2 != 0) - return erro_code = 3; + return error_code = 3; if (msg_timer.overflow()) { msg_timer.reset(); lock_flag = 0; - erro_code = 2; + error_code = 2; } if (lock_flag == 0) @@ -337,7 +339,7 @@ namespace Transports } return 0; } - return erro_code; + return error_code; } //! Builds the next msg package @@ -436,7 +438,7 @@ namespace Transports struct CidXcvrFixMsg { - Acofix_t aco_fix; + Acofix_t aco_fix; }; struct CidSysRebootMsg @@ -451,9 +453,9 @@ namespace Transports uint8_t status; }; - struct CidNavQuerryRespMsg + struct CidNavQueryRespMsg { - Acofix_t aco_fix; + Acofix_t aco_fix; uint8_t query_flags; int32_t remote_depth; int16_t remote_supply; diff --git a/src/Transports/Seatrac/Parser.hpp b/src/Transports/Seatrac/Parser.hpp index 27ad8e1604..44ddcf8875 100644 --- a/src/Transports/Seatrac/Parser.hpp +++ b/src/Transports/Seatrac/Parser.hpp @@ -2,6 +2,8 @@ // Copyright 2007-2023 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** +// Copyright 2023 OceanScan - Marine Systems & Technology, Lda. * +//*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * // * // Commercial Licence Usage * @@ -26,6 +28,7 @@ //*************************************************************************** // Author: João Teixeira * // Author: Raúl Sáez * +// Author: Maria Costa (small refactor and fix for iUSBL implementation) * //*************************************************************************** #ifndef TRANSPORTS_SEATRAC_PARSER_HPP_INCLUDED_ @@ -54,13 +57,13 @@ namespace Transports // Status message. CidStatusMsg cid_status_msg; // Ping protocol messages. - CidPingRequestMsg cid_ping_req_msg; - CidPingSendMsg cid_ping_send_msg; + CidPingRequestMsg cid_ping_req_msg; + CidPingSendMsg cid_ping_send_msg; CidPingRespMsg cid_ping_resp_msg; CidPingErrorMsg cid_ping_error_msg; // Data protocol messages. CidDatReceiveMsg cid_dat_receive_msg; - CidDatSendMsg cid_dat_send_msg; + CidDatSendMsg cid_dat_send_msg; // Configuration protocol messages. CidSysInfo cid_sys_info; CidSettingsMsg cid_settings_msg; @@ -71,7 +74,7 @@ namespace Transports CidXcvrFixMsg cid_xcvr_fix_msg; CidNavQueryReqMsg cid_nav_query_req_msg; CidNavQuerySendMsg cid_nav_query_send_msg; - CidNavQuerryRespMsg cid_nav_querry_resp_msg; + CidNavQueryRespMsg cid_nav_query_resp_msg; CidNavBeaconPosUpdateMsg cid_nav_beacon_pos_update_msg; CidNavBeaconPosSendMsg cid_nav_beacon_pos_send_msg; CidNavRefPosSendMsg cid_nav_ref_pos_send_msg; @@ -109,7 +112,7 @@ namespace Transports //! Verify if new message was received. //! @param[in] setdata type of msg that was received. //! @return true if was received new message and flag is clean. - uint8_t + bool newDataAvailable(unsigned setdata) { if (setdata < MESSAGE_NUMBER) @@ -117,20 +120,20 @@ namespace Transports if (new_message[setdata] == 1) { new_message[setdata] = 0; - return 1; + return true; } } - return 0; + return false; } }; //! Extract data to a Acofix_t structure. - //! @param[out] aco_fix pointer hwre data is stored. - //! @param[in] ind raw messagem index. + //! @param[out] aco_fix pointer where data is stored. + //! @param[in] ind raw message index. //! @param[in] msg_raw pointer to raw message. uint16_t - updateEcoFix(Acofix_t* aco_fix, uint16_t ind, const char* msg_raw) + updateAcoFix(Acofix_t* aco_fix, uint16_t ind, const char* msg_raw) { std::memcpy(&(*aco_fix).dest_id, msg_raw + ind, 1); std::memcpy(&(*aco_fix).src_id, msg_raw + ind + 1, 1); @@ -183,11 +186,11 @@ namespace Transports } //! Extract to DataSeatrac data structure. - //! @param[in] message_type type of msessage to decode. - //! @param[in] msg_raw raw messagem received by uart - //! @param[out] data_Beacon pointer where the data is stored. + //! @param[in] message_type type of message to decode. + //! @param[in] msg_raw raw message received by uart + //! @param[out] data_Beacon pointer to where the data is stored. void - dataParser(uint16_t message_type, const char* msg_raw, DataSeatrac& data_Beacon) + dataParser(uint8_t message_type, const char* msg_raw, DataSeatrac& data_Beacon) { uint16_t ind = 0; @@ -276,12 +279,12 @@ namespace Transports case CID_PING_REQ: data_Beacon.set(CID_PING_REQ); - ind = updateEcoFix(&data_Beacon.cid_ping_req_msg.aco_fix, ind, msg_raw); + ind = updateAcoFix(&data_Beacon.cid_ping_req_msg.aco_fix, ind, msg_raw); break; case CID_PING_RESP: // Message sent when a PING response is received. data_Beacon.set(CID_PING_RESP); - ind = updateEcoFix(&data_Beacon.cid_ping_resp_msg.aco_fix, ind, msg_raw); + ind = updateAcoFix(&data_Beacon.cid_ping_resp_msg.aco_fix, ind, msg_raw); break; case CID_PING_ERROR: @@ -303,7 +306,7 @@ namespace Transports break; case CID_DAT_RECEIVE: - ind = updateEcoFix(&data_Beacon.cid_dat_receive_msg.aco_fix, ind, msg_raw); + ind = updateAcoFix(&data_Beacon.cid_dat_receive_msg.aco_fix, ind, msg_raw); std::memcpy(&data_Beacon.cid_dat_receive_msg.ack_flag, msg_raw + ind, 1); std::memcpy(&data_Beacon.cid_dat_receive_msg.packet_len, msg_raw + ind +1, 1); ind += 2; @@ -373,46 +376,46 @@ namespace Transports case CID_NAV_QUERY_REQ: data_Beacon.set(CID_NAV_QUERY_REQ); - ind = updateEcoFix(&data_Beacon.cid_nav_query_req_msg.aco_fix, ind, msg_raw); + ind = updateAcoFix(&data_Beacon.cid_nav_query_req_msg.aco_fix, ind, msg_raw); std::memcpy(&data_Beacon.cid_nav_query_req_msg.nav_query_t, msg_raw + ind, 1); break; case CID_XCVR_FIX: data_Beacon.set(CID_XCVR_FIX); - ind = updateEcoFix(&data_Beacon.cid_xcvr_fix_msg.aco_fix, ind, msg_raw); + ind = updateAcoFix(&data_Beacon.cid_xcvr_fix_msg.aco_fix, ind, msg_raw); break; case CID_NAV_QUERY_RESP: data_Beacon.set(CID_NAV_QUERY_RESP); - ind = updateEcoFix(&data_Beacon.cid_nav_querry_resp_msg.aco_fix, ind, msg_raw); - std::memcpy(&data_Beacon.cid_nav_querry_resp_msg.query_flags, msg_raw + ind, 1); + ind = updateAcoFix(&data_Beacon.cid_nav_query_resp_msg.aco_fix, ind, msg_raw); + std::memcpy(&data_Beacon.cid_nav_query_resp_msg.query_flags, msg_raw + ind, 1); ind += 1; - data_Beacon.cid_nav_querry_resp_msg.queryFlagsExtract(); + data_Beacon.cid_nav_query_resp_msg.queryFlagsExtract(); - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[0]) + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[0]) { - std::memcpy(&data_Beacon.cid_nav_querry_resp_msg.remote_depth, msg_raw + ind,4); + std::memcpy(&data_Beacon.cid_nav_query_resp_msg.remote_depth, msg_raw + ind,4); ind += 4; } - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[1]) + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[1]) { - std::memcpy(&data_Beacon.cid_nav_querry_resp_msg.remote_supply, msg_raw + ind, 2); + std::memcpy(&data_Beacon.cid_nav_query_resp_msg.remote_supply, msg_raw + ind, 2); ind += 2; } - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[2]) + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[2]) { - std::memcpy(&data_Beacon.cid_nav_querry_resp_msg.remote_temp, msg_raw + ind, 2); + std::memcpy(&data_Beacon.cid_nav_query_resp_msg.remote_temp, msg_raw + ind, 2); ind += 2; } - if (data_Beacon.cid_nav_querry_resp_msg.query_flags_list[3]) + if (data_Beacon.cid_nav_query_resp_msg.query_flags_list[3]) { - std::memcpy(&data_Beacon.cid_nav_querry_resp_msg.remote_yaw, msg_raw + ind, 2); - std::memcpy(&data_Beacon.cid_nav_querry_resp_msg.remote_pitch, + std::memcpy(&data_Beacon.cid_nav_query_resp_msg.remote_yaw, msg_raw + ind, 2); + std::memcpy(&data_Beacon.cid_nav_query_resp_msg.remote_pitch, msg_raw + ind + 2, 2); - std::memcpy(&data_Beacon.cid_nav_querry_resp_msg.remote_roll, + std::memcpy(&data_Beacon.cid_nav_query_resp_msg.remote_roll, msg_raw + ind + 4, 2); } break; @@ -431,7 +434,7 @@ namespace Transports case CID_NAV_BEACON_POS_UPDATE: data_Beacon.set(CID_NAV_BEACON_POS_UPDATE); - ind = updateEcoFix(&data_Beacon.cid_nav_beacon_pos_update_msg.aco_fix, ind, msg_raw); + ind = updateAcoFix(&data_Beacon.cid_nav_beacon_pos_update_msg.aco_fix, ind, msg_raw); std::memcpy(&data_Beacon.cid_nav_beacon_pos_update_msg.beacon_id, msg_raw + ind, 1); std::memcpy(&data_Beacon.cid_nav_beacon_pos_update_msg.position_easting, msg_raw + ind + 1, 2); @@ -451,7 +454,7 @@ namespace Transports case CID_NAV_REF_POS_UPDATE: data_Beacon.set(CID_NAV_REF_POS_UPDATE); - ind = updateEcoFix(&data_Beacon.cid_nav_ref_pos_update_msg.aco_fix, ind, msg_raw); + ind = updateAcoFix(&data_Beacon.cid_nav_ref_pos_update_msg.aco_fix, ind, msg_raw); std::memcpy(&data_Beacon.cid_nav_ref_pos_update_msg.beacon_id, msg_raw + ind, 1); std::memcpy(&data_Beacon.cid_nav_ref_pos_update_msg.position_latitude, msg_raw + ind + 1, 4); @@ -515,9 +518,8 @@ namespace Transports ByteCopy::fromLE(data_Beacon.cid_xcvr_usbl_msg.signal_fit_error, (const uint8_t*)&msg_raw[ind]); break; - // Should never get here. + // Should never get here. default: - //m_data_state = DP_COMPLETE; break; } } @@ -527,7 +529,7 @@ namespace Transports //! @param[in] data_Beacon message structure. //! @return string with the command. std::string - commandCreateSeatrac(CommandID cid_type, DataSeatrac& data_Beacon) + createCommand(CommandID cid_type, DataSeatrac& data_Beacon) { std::string cmd = "#"; std::string check_sum; @@ -550,6 +552,13 @@ namespace Transports message_build += data_Beacon.cid_dat_send_msg.packet_data; break; + case CID_DAT_QUEUE_SET: + message_build += String::str("%02X%02X", + ((uint8_t)data_Beacon.cid_ping_send_msg.dest_id), + ((uint8_t)data_Beacon.cid_dat_send_msg.packet_len)); + message_build += data_Beacon.cid_dat_send_msg.packet_data; + break; + case CID_SETTINGS_SET: { std::vector msg_temp(110, 0); @@ -639,7 +648,7 @@ namespace Transports ((uint8_t) data_Beacon.cid_nav_query_send_msg.query_flags)); break; - // should never get here. + // should never get here. default: break; } diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 650a744563..2e298b2856 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -2,6 +2,8 @@ // Copyright 2007-2023 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** +// Copyright 2023 OceanScan - Marine Systems & Technology, Lda. * +//*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * // * // Commercial Licence Usage * @@ -26,6 +28,7 @@ // Author: João Teixeira * // Author: Raúl Sáez * // Author: Paulo Dias * +// Author: Maria Costa (small refactor and fix for iUSBL implementation) * //*************************************************************************** // ISO C++ 98 headers. @@ -39,10 +42,7 @@ #include // Local headers -#include "Parser.hpp" -#include "MsgTypes.hpp" -#include "DataTypes.hpp" -#include "DebugMsg.hpp" +#include "Driver.hpp" namespace Transports { @@ -54,22 +54,6 @@ namespace Transports { using DUNE_NAMESPACES; - //! Hard Iron calibration parameter name. - static const std::string c_hard_iron_param = "Hard-Iron Calibration"; - //! Number of axis. - static const uint8_t c_number_axis = 3; - - //! Entity states. - enum EntityStates - { - STA_BOOT, - STA_IDLE, - STA_ACTIVE, - STA_ERR_COM, - STA_ERR_STP, - STA_MAX - }; - //! Task arguments. struct Arguments { @@ -77,81 +61,78 @@ namespace Transports std::string uart_dev; //! Serial port baud rate. unsigned uart_baud; + //! Transmit only when medium is suitable. + bool honour_medium; //! Transmit only underwater. bool only_underwater; //! Addresses Number - modem std::string addr_section; - //! Enable ARHS mode - bool arhs_mode; + //! Enable AHRS mode + bool ahrs_mode; //! Enable pressure sensor bool pressure_sensor_mode; //! Enable pressure sensor use for checking if underwater - bool use_pressure_sensor_for_medium; + bool pressure_as_medium; //! Enable usbl mode bool usbl_mode; - //! Hard iron calibration. + //! Hard-iron calibration. std::vector hard_iron; //! Enhanced usbl information will be requested. bool enhanced_usbl; - // Rotation matrix values. + //! Rotation matrix values. std::vector rotation_mx; //! Calibration threshold. double calib_threshold; - //! max range + //! Maximum range. uint16_t max_range; - //! Timeout time multiplier for ack wait - uint8_t ack_timeout_time_multiplier; - //! dummy connection - bool dummy_connection; + //! Transponder turn around time. + uint16_t turn_around_time; + //! Sound speed on water. + double sound_speed_def; + //! Entity label of sound speed provider. + std::string sound_speed_elabel; + //! Attitude update frequency when manually set. + float freq_attitude; + //! Fit error threshold + float fit_threshold; }; - //! Map of system's names. - typedef std::map MapName; - //! Map of system's addresses. - typedef std::map MapAddr; - struct Task: public DUNE::Tasks::Task { //! Serial port handle. IO::Handle* m_handle; + //! Driver. + Driver* m_driver; //! Task arguments. Arguments m_args; //! Config Status. bool m_config_status; //! c_preamble detected - bool m_pre_detected; + bool m_preamble; //! Current state. - EntityStates m_state_entity; + EntityStates m_state; //! Entity states. IMC::EntityState m_states[STA_MAX]; - //! Stop reports on the ground. - bool m_stop_comms; //! True if the beacon has an USBL receiver. bool m_usbl_receiver; //! Modem address. unsigned m_addr; - //! Data buffer. - std::string m_data; - //! Converted data buffer. - std::string m_datahex; - //! Seatrac data structures. + //! Seatrac data structure. DataSeatrac m_data_beacon; - //! Time of last serial port input. + //! Time of last received input. double m_last_input; //! Read timestamp. double m_tstamp; //! Timer to manage the fragmentation of OWAY messages. Time::Counter m_oway_timer; - //! hard iron calibration parameters. + //! Hard-iron calibration parameters (after rotation). float m_hard_iron[3]; - //! Map of seatrac modems by name. - MapName m_modem_names; - //! Map of seatrac modems by address. - MapAddr m_modem_addrs; + //! Node map. + NodeMap m_node_map; //! Current transmission ticket. Ticket* m_ticket; - // Save modem commands. - IMC::DevDataText m_dev_data; + //! Vehicle Medium. + IMC::VehicleMedium m_medium; //! Euler angles message. IMC::EulerAngles m_euler; //! Acceleration message. @@ -160,32 +141,38 @@ namespace Transports IMC::AngularVelocity m_agvel; //! Magnetometer Vector message. IMC::MagneticField m_magfield; - //! Current sound speed. + //! Sound speed entity id. + int m_sound_speed_eid; + //! Modem sound speed. IMC::SoundSpeed m_sspeed; - // Depth. + //! Modem depth. IMC::Depth m_depth; - // Pressure. + //! Modem measured pressure. IMC::Pressure m_pressure; - // Measured temperature. + //! Modem measured temperature. IMC::Temperature m_temperature; //! Rotation Matrix to correct mounting position. Math::Matrix m_rotation; + //! Timer. + Time::Counter m_timer; //! Constructor. //! @param[in] name task name. //! @param[in] ctx context. Task(const std::string& name, Tasks::Context& ctx) : DUNE::Tasks::Task(name, ctx), - m_handle(NULL), + m_handle(nullptr), + m_driver(nullptr), m_config_status(false), - m_pre_detected(false), - m_stop_comms(false), + m_preamble(false), m_usbl_receiver(false), - m_tstamp(0), - m_ticket(NULL) + m_addr(0), + m_tstamp(0), + m_ticket(nullptr), + m_sound_speed_eid(-1) { // Define configuration parameters. - paramActive(Tasks::Parameter::SCOPE_MANEUVER, + paramActive(Tasks::Parameter::SCOPE_GLOBAL, Tasks::Parameter::VISIBILITY_USER); param("Serial Port - Device", m_args.uart_dev) @@ -197,29 +184,33 @@ namespace Transports .defaultValue("19200") .description("Serial port baud rate"); + param("Honour Medium", m_args.honour_medium) + .defaultValue("false") + .description("Set to true to transmit only when the medium is suitable"); + param("Transmit Only Underwater", m_args.only_underwater) .defaultValue("false") - .description("Do not transmit when at water surface"); + .description("Do not transmit when at surface (if modem installed upright on the vehicle, for instance)"); param("Address Section", m_args.addr_section) .defaultValue("Seatrac Addresses") .description("Name of the configuration section with modem addresses"); - param("AHRS Mode", m_args.arhs_mode) + param("AHRS Mode", m_args.ahrs_mode) .defaultValue("false") - .description("Enable the AHRS information to used in navigation"); + .description("Enable the AHRS information to be used in navigation (not recommended to be used with USBL receivers)."); param("Pressure Sensor Mode", m_args.pressure_sensor_mode) .defaultValue("false") - .description("Enable the pressure sensor, depth, sound velocity and temperature information "); + .description("Enables pressure sensor, depth, sound velocity and temperature information"); - param("Use Internal Pressure Sensor for Medium", m_args.use_pressure_sensor_for_medium) + param("Use Internal Pressure Sensor as Medium", m_args.pressure_as_medium) .defaultValue("false") - .description("Enable pressure sensor use for checking if underwater"); + .description("Enable internal pressure sensor use for honour medium"); param("USBL Mode", m_args.usbl_mode) .defaultValue("false") - .description("Enable the USBL mode. USBL receivers can obtain position information."); + .description("Enable USBL mode. USBL receivers can obtain position information."); param("Enhanced USBL", m_args.enhanced_usbl) .defaultValue("false") @@ -241,21 +232,41 @@ namespace Transports .defaultValue("0.1") .units(Units::Gauss) .minimumValue("0.0") - .description("Minimum magnetic field calibration values to reset hard iron parameters"); + .description("Minimum magnetic field calibration values to reset hard-iron parameters"); param("Max Range", m_args.max_range) .defaultValue("1000") - .minimumValue("250") - .description("Maximum value of distance at which Ranges are considered"); - - param("Acknowledged timeout time multiplier", m_args.ack_timeout_time_multiplier) - .defaultValue("6") - .minimumValue("3") - .description("A time multiplier to wait before timeout for acknowledge (it ack requested)"); - - param("Dummy Connection", m_args.dummy_connection) - .defaultValue("false") - .description("To assume a dummy connection and not a modem (no replies"); + .minimumValue("100") + .maximumValue("3000") + .description("Range timeout that specifies a distance beyond which replies are ignored and the remote beacon is considered to have timed out."); + + param("Turn Around Time", m_args.turn_around_time) + .units( Units::Millisecond) + .defaultValue("10") + .minimumValue("10") + .maximumValue("1000") + .description("Specifies how long the beacon will wait between receiving a request message and starting transmission of the response message." + "All beacons communicating must have the same value."); + + param("Sound Speed - Default Value", m_args.sound_speed_def) + .units(Units::MeterPerSecond) + .defaultValue("1500") + .description("Default sound speed value to be used when computing ranges."); + + param("Sound Speed - Entity Label", m_args.sound_speed_elabel) + .description("Entity label of sound speed provider"); + + param("Attitude update frequency", m_args.freq_attitude) + .units(DUNE::Units::Hertz) + .defaultValue("20") + .minimumValue("0") + .maximumValue("50") + .description("Frequency at which the attitude is updated on the modem, when manually set."); + + param("Fit Error Threshold", m_args.fit_threshold) + .defaultValue("3.4") + .description("The fit error indicates the quality of fit (or confidence) of the signal azimuth and elevation values" + " from the timing and phase-angle data available."); // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; @@ -269,6 +280,12 @@ namespace Transports m_states[STA_ERR_STP].state = IMC::EntityState::ESTA_ERROR; m_states[STA_ERR_STP].description = DTR("failed to configure modem, possible serial port communication error"); + // Initialize medium. + m_medium.medium = IMC::VehicleMedium::VM_UNKNOWN; + + bind(this); + bind(this); + bind(this); bind(this); bind(this); } @@ -276,118 +293,11 @@ namespace Transports //! Set entity state. //! @param[in] state new entity state. void - setAndSendState(EntityStates state) - { - m_state_entity = state; - setEntityState((IMC::EntityState::StateEnum) m_states[m_state_entity].state, - m_states[m_state_entity].description); - } - - //! Process sentence. - //! @return true if message was correctly processed, false otherwise. - bool - processSentence(void) + setState(EntityStates state) { - bool msg_validity = false; - uint16_t crc, crc2; - if(m_data.size() >= MIN_MESSAGE_LENGTH) - { - std::string msg = String::fromHex(m_data.substr((m_data.size() - 4), 4)); - std::memcpy(&crc2, msg.data(), 2); - m_datahex = String::fromHex(m_data.erase((m_data.size() - 4), 4)); - crc = CRC16::compute((uint8_t*) m_datahex.data(), m_datahex.size(),0); - if (crc == crc2) - msg_validity = true; - else - war("%s", DTR(Status::getString(Status::CODE_INVALID_CHECKSUM))); - } - return msg_validity; - } - - //! Process new data. - void - processNewData(void) - { - if(m_config_status==true) - { - if (m_data_beacon.newDataAvailable(CID_DAT_RECEIVE)) - handleBinaryMessage(); - - if (m_data_beacon.newDataAvailable(CID_DAT_SEND)) - handleDatSendResponse(); - - if (m_data_beacon.newDataAvailable(CID_DAT_ERROR)) - handleCommunicationError(); - - if(m_data_beacon.newDataAvailable(CID_STATUS)) - { - if(m_args.arhs_mode == true) - { - handleAhrsData(); - } - if(m_args.pressure_sensor_mode == true) - { - handlePressureSensor(); - } - - //todo send environment_supply - //m_data_beacon.cid_status_msg.environment_supply; //uint16_t - } - } - } - - //! Release - //! Read sentence. - void - readSentence(void) - { - // Initialize received message parser - char bfr[c_bfr_size]; - uint16_t typemes = 0; - const char* msg_raw; - size_t rv; - - if (Poll::poll(*m_handle, 0.001)) - { - rv = m_handle->readString(bfr, c_bfr_size); - m_tstamp = Clock::getSinceEpoch(); - m_last_input = Clock::get(); - for (size_t i = 0; i < rv; ++i) - { - // Detected line termination. - if (bfr[i] == '\n') - { - m_dev_data.value.assign(sanitize(m_data)); - dispatch(m_dev_data); - if(m_pre_detected==true) - { - if (processSentence()) - { - msg_raw = m_datahex.data(); - std::memcpy(&typemes, msg_raw, 1); - dataParser(typemes, msg_raw + 1, m_data_beacon); - processNewData(); - printDebugFunction(typemes, m_data_beacon, this); - typemes = 0; - } - } - m_pre_detected = false; - m_data.clear(); - } - else - { - if (bfr[i] == c_preamble) - { - m_data.clear(); - m_pre_detected = true; - } - else if (bfr[i] != '\r') - { - m_data.push_back(bfr[i]); - } - } - } - } + m_state = state; + setEntityState((IMC::EntityState::StateEnum) m_states[m_state].state, + m_states[m_state].description); } //! Open TCP socket. @@ -407,16 +317,26 @@ namespace Transports return true; } + void + onEntityResolution() override + { + try + { + m_sound_speed_eid = resolveEntity(m_args.sound_speed_elabel); + } + catch (...) + { + debug("Dynamic sound speed corrections are disabled (using default sound speed value)."); + } + } + //! Acquire resources. void onResourceAcquisition(void) { - setAndSendState(STA_BOOT); + setState(STA_BOOT); try { - if (m_args.only_underwater == true) - m_stop_comms = true; - if (openSocket()) return; @@ -435,123 +355,44 @@ namespace Transports void onResourceInitialization(void) { - // Process modem addresses. - std::string agent = getSystemName(); - std::vector addrs = m_ctx.config.options(m_args.addr_section); + m_node_map.readConfigSection(m_ctx.config, m_args.addr_section); + if (!m_node_map.lookupAddress(getSystemName(), m_addr)) + throw std::runtime_error(DTR("no modem address for current system")); - // verify modem local address value. - for (unsigned i = 0; i < addrs.size(); ++i) - { - unsigned addr = 0; - m_ctx.config.get(m_args.addr_section, addrs[i], "0", addr); - m_modem_names[addrs[i]] = addr; - m_modem_addrs[addr] = addrs[i]; - } - - m_ctx.config.get(m_args.addr_section, agent, "1024", m_addr); if (m_addr < 1 || m_addr > 15) + throw std::runtime_error(String::str(DTR("modem address for agent '%s' is invalid"), getSystemName())); + + try { - throw std::runtime_error(String::str(DTR("modem address for agent '%s' is invalid"), agent.c_str())); + // Initialize driver + m_driver = new Driver(this, m_handle, m_data_beacon, m_tstamp, m_last_input, m_preamble, m_addr, + m_hard_iron[0], m_hard_iron[1], m_hard_iron[2]); } - - m_last_input = Clock::get(); - processInput(); - - if (hasConnection()) + catch (std::runtime_error& e) { - do - { - sendCommand(commandCreateSeatrac(CID_SETTINGS_GET, m_data_beacon)); - processInput(); - } - while (m_data_beacon.newDataAvailable(CID_SETTINGS_GET) == 0 && !m_args.dummy_connection); - - sendCommandAndWait(commandCreateSeatrac(CID_SYS_INFO, m_data_beacon), 1); - - if (m_data_beacon.cid_sys_info.hardware.part_number == BT_X150) - m_usbl_receiver = true; - - uint8_t output_flags = (ENVIRONMENT_FLAG | ATTITUDE_FLAG - | MAG_CAL_FLAG | ACC_CAL_FLAG - | AHRS_RAW_DATA_FLAG | AHRS_COMP_DATA_FLAG); - - uint8_t xcvr_flags = XCVR_FIX_MSGS_FLAG | XCVR_POSFLT_ENABLE_FLAG; - - if (m_usbl_receiver) - xcvr_flags |= USBL_USE_AHRS_FLAG | XCVR_USBL_MSGS_FLAG; - - StatusMode_E status_mode= STATUS_MODE_1HZ; - bool chage_IMU = true; - if (m_args.arhs_mode == true) - { - status_mode = STATUS_MODE_10HZ; - chage_IMU = isCalibrated(); - } - if (!((m_data_beacon.cid_settings_msg.xcvr_beacon_id == m_addr) - && (m_data_beacon.cid_settings_msg.status_flags == status_mode) - && (m_data_beacon.cid_settings_msg.status_output == output_flags) - && (m_data_beacon.cid_settings_msg.xcvr_flags == xcvr_flags) - && (m_data_beacon.cid_settings_msg.xcvr_range_tmo == m_args.max_range) - && chage_IMU == true)) - { - m_data_beacon.cid_settings_msg.status_flags = status_mode; - m_data_beacon.cid_settings_msg.status_output = output_flags; - m_data_beacon.cid_settings_msg.xcvr_flags = xcvr_flags; - m_data_beacon.cid_settings_msg.xcvr_beacon_id = m_addr; - m_data_beacon.cid_settings_msg.xcvr_range_tmo = m_args.max_range; - - if(chage_IMU == false) - { - m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x = m_args.hard_iron[0]; - m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y = m_args.hard_iron[1]; - m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z = m_args.hard_iron[2]; - } - - inf("asking to save settings to modem"); - sendCommandAndWait(commandCreateSeatrac(CID_SETTINGS_SET, m_data_beacon), 2); - sendCommandAndWait(commandCreateSeatrac(CID_SETTINGS_SAVE, m_data_beacon), 2); - inf("rebooting modem"); - sendCommandAndWait(commandCreateSeatrac(CID_SYS_REBOOT, m_data_beacon), 6); - sendCommandAndWait(commandCreateSeatrac(CID_SETTINGS_GET, m_data_beacon), 2); - - if (m_data_beacon.cid_settings_msg.xcvr_beacon_id != m_addr) - { - setAndSendState(STA_ERR_STP); - war(DTR("failed to configure device")); - } - - inf("ready"); - setAndSendState(STA_IDLE); - m_config_status = true; - } - else + if (m_state != STA_ERR_COM) { - inf("ready (settings already set)"); - setAndSendState(STA_IDLE); - m_config_status = true; + err("%s: %s", DTR(Status::getString(CODE_COM_ERROR)), e.what()); + setState(STA_ERR_COM); } + return; + } - inf(DTR("Beacon id=%d | HW P#%d (rev#%d) serial#%d | FW P#%d v%d.%d.%d | App P#%d v%d.%d.%d | %s USBL beacon"), - m_data_beacon.cid_settings_msg.xcvr_beacon_id, - m_data_beacon.cid_sys_info.hardware.part_number, - m_data_beacon.cid_sys_info.hardware.part_rev, - m_data_beacon.cid_sys_info.hardware.serial_number, - m_data_beacon.cid_sys_info.boot_firmware.part_number, - m_data_beacon.cid_sys_info.boot_firmware.version_maj, - m_data_beacon.cid_sys_info.boot_firmware.version_min, - m_data_beacon.cid_sys_info.boot_firmware.version_build, - m_data_beacon.cid_sys_info.main_firmware.part_number, - m_data_beacon.cid_sys_info.main_firmware.version_maj, - m_data_beacon.cid_sys_info.main_firmware.version_min, - m_data_beacon.cid_sys_info.main_firmware.version_build, - m_usbl_receiver ? "Is" : "Not"); + try + { + // Configure modem + setState(m_driver->configure(m_usbl_receiver, m_args.ahrs_mode, m_args.max_range, m_args.turn_around_time, m_args.sound_speed_def)); + m_config_status = true; } - else + catch (...) { err("%s", DTR(Status::getString(CODE_COM_ERROR))); - setAndSendState(STA_ERR_STP); - throw std::runtime_error(m_states[m_state_entity].description); + setState(STA_ERR_STP); + throw std::runtime_error(m_states[m_state].description); } + + if (m_args.freq_attitude != 0) + m_timer.setTop(1/m_args.freq_attitude); } //! Update parameters. @@ -560,7 +401,7 @@ namespace Transports { m_rotation.fill(3, 3, &m_args.rotation_mx[0]); - // Rotate calibration parameters. + // Rotate hard-iron calibration parameters. Math::Matrix data(3, 1); for (unsigned i = 0; i < 3; i++) @@ -569,34 +410,62 @@ namespace Transports for (unsigned i = 0; i < 3; i++) m_hard_iron[i] = data(i); - if (m_handle != NULL) + if (m_handle != nullptr) { - - if (paramChanged(m_args.hard_iron)) - runCalibration(); + if (paramChanged(m_args.hard_iron) && m_args.ahrs_mode) + m_driver->runCalibration(); } } - //! Routine to run calibration proceedings. + + //! Release resources. + void + onResourceRelease(void) + { + clearTicket(IMC::UamTxStatus::UTS_CANCELED); + Memory::clear(m_handle); + Memory::clear(m_driver); + } + void - runCalibration(void) + consume(const IMC::EulerAngles* msg) { - if (m_handle == NULL) + if (m_state != STA_ACTIVE) return; - // See if vehicle has same hard iron calibration parameters. - if (!isCalibrated()) - { - // Set hard iron calibration parameters and reset device. - if (!setHardIron()) - { - throw RestartNeeded(DTR("failed to set hard-iron correction factors"), 5); - } - } + if (m_args.ahrs_mode || !m_timer.overflow() || !m_args.freq_attitude) + return; + + // Rotate attitude values to transponder reference frame. + Math::Matrix data(3, 1); + data(0) = msg->phi; + data(1) = msg->theta; + data(2) = msg->psi; + data = transpose(m_rotation) * data; + + // Change to seatrac defined intervals (Roll: -180.0:180.0º | Pitch: -90.0:90.0º | Yaw: 0.0:359.9º) + for (size_t i=0; i<3; i++) + data(i) = Angles::degrees(data(i)); + if (data(2) < 0) + data(2) = 360 + data(2); + + // Set current attitude on transponder + m_data_beacon.cid_settings_msg.xcvr_roll = (uint16_t)(data(0) * 10); + m_data_beacon.cid_settings_msg.xcvr_pitch = (uint16_t)(data(1) * 10); + m_data_beacon.cid_settings_msg.xcvr_yaw = (uint16_t)(data(2) * 10); + m_driver->sendCommand(createCommand(CID_SETTINGS_SET, m_data_beacon)); + + m_timer.reset(); } void consume(const IMC::MagneticField* msg) { + if (m_state != STA_ACTIVE) + return; + + if(!m_args.ahrs_mode) + return; + if (msg->getDestinationEntity() != getEntityId()) return; @@ -622,78 +491,52 @@ namespace Transports dispatch(sp); } - //! Check if sensor has the same hard iron calibration parameters. - //! @return true if the parameters are the same, false otherwise. - bool - isCalibrated(void) + void + consume(const IMC::SoundSpeed* msg) { - if( ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x*100)) != ( (int32_t) (m_args.hard_iron[0]*100))) - { - war(DTR("different calibration parameters")); - return false; - } - if( ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y*100)) != ( (int32_t) (m_args.hard_iron[1]*100))) - { - war(DTR("different calibration parameters")); - return false; - } - if( ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z*100)) != ( (int32_t) (m_args.hard_iron[2]*100))) - { - war(DTR("different calibration parameters")); - return false; - } - debug("Is calibrated"); - return true; - } + if (m_state != STA_ACTIVE) + return; - //! Set new hard iron calibration parameters. - //! @return true if successful, false otherwise. - bool - setHardIron(void) - { - m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x = m_args.hard_iron[0]; - m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y = m_args.hard_iron[1]; - m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z = m_args.hard_iron[2]; - sendCommandAndWait(commandCreateSeatrac(CID_SETTINGS_SET, m_data_beacon), 2); - sendCommandAndWait(commandCreateSeatrac(CID_SETTINGS_SAVE, m_data_beacon), 2); - sendCommandAndWait(commandCreateSeatrac(CID_SETTINGS_GET, m_data_beacon), 2); - if(m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x != m_args.hard_iron[0]) - return false; - if(m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y != m_args.hard_iron[1]) - return false; - if(m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z != m_args.hard_iron[2]) - return false; + if ((int)msg->getSourceEntity() != m_sound_speed_eid) + return; - return true; - } - //! Release resources. - void - onResourceRelease(void) - { - clearTicket(IMC::UamTxStatus::UTS_CANCELED); - Memory::clear(m_handle); + if (msg->value < 0) + return; + + // Do not change if new value difference to current value is below c_sspeed_window (m/s). + if (std::abs(m_data_beacon.cid_settings_msg.env_vos/10 - msg->value) < c_sspeed_window) + return; + + // Set water sound speed on transponder + m_data_beacon.cid_settings_msg.env_vos = (uint16_t) msg->value * 10; + m_driver->sendCommand(createCommand(CID_SETTINGS_SET, m_data_beacon)); } - //! Send command and waits for response. - //! @param[in] cmd command string. - //! @param[in] delay_bef delay before send comamnd. - //! @param[in] delay_aft delay after send comamnd. void - sendCommandAndWait(const std::string& cmd, double delay_aft) + consume(const IMC::VehicleMedium* msg) { - sendCommand(cmd); - processInput(delay_aft); + m_medium = *msg; } - //! Check if medium and configuration for protected msg send. + //! Check if communications are restricted by medium. + //! @return true if medium is not suitable for communications to happen. bool - isCommsBlockedByMedium(void) + isRestricted(void) { - if (m_args.only_underwater && m_args.pressure_sensor_mode - && m_args.use_pressure_sensor_for_medium) - return m_pressure.value <= 0; - - return m_stop_comms; + bool restricted = false; + float pressure = (((fp32_t) (m_data_beacon.cid_status_msg.environment_pressure)) / 1000.0) * Math::c_pascal_per_bar; + + if (m_args.honour_medium) + { + if ((m_medium.medium < IMC::VehicleMedium::VM_WATER) || (m_medium.medium == IMC::VehicleMedium::VM_UNKNOWN)) + restricted = true; + if (m_args.pressure_as_medium) + restricted = (pressure <= 0) ? true : false; + if (m_args.only_underwater && (m_medium.medium != IMC::VehicleMedium::VM_UNDERWATER)) + restricted = true; + } + + return restricted; } //! Send command if the modem has conditions to operate. @@ -701,26 +544,14 @@ namespace Transports void sendProtectedCommand(const std::string& cmd) { - if (isCommsBlockedByMedium()) + if (isRestricted()) { - war(DTR("Sending stopped: Communication out of water forbidden.")); + war(DTR("Send stopped: Communications out of water forbidden.")); clearTicket(IMC::UamTxStatus::UTS_FAILED); m_data_beacon.cid_dat_send_msg.lock_flag = 0; return; } - sendCommand(cmd); - } - - //! Send command to the acoustic modem. - //! @param[in] cmd command string. - void - sendCommand(const std::string& cmd) - { - debug(DTR("Send command to the acoustic modem %s"), cmd.c_str()); - m_handle->writeString(cmd.c_str()); - debug(DTR("Sent done")); - m_dev_data.value.assign(sanitize(cmd)); - dispatch(m_dev_data); + m_driver->sendCommand(cmd); } //! Checks if the modem is working. @@ -728,11 +559,15 @@ namespace Transports bool hasConnection(void) { - if (Clock::get() >= (m_last_input + c_input_tout)) - { - return false; - } - return true; + // Don't verify connection if task is not active + if (!isActive()) + m_last_input = Clock::get(); + + // Throw runtime error if connection problem persists + if ((Clock::get() > (m_last_input + c_input_tout + 30)) && m_config_status) + throw RestartNeeded(m_states[STA_ERR_COM].description, 30); + + return (Clock::get() < (m_last_input + c_input_tout)); } //! Processing incoming data. @@ -742,25 +577,25 @@ namespace Transports if (m_data_beacon.cid_dat_receive_msg.ack_flag != 0) { // if msg has more than 1 packet, send next part - if (m_ticket != NULL) + if (m_ticket != nullptr) { - debug(DTR("Success transmission complete (part %d of %d) for ticket %d (in %f s)"), + trace(DTR("Success transmission complete (part %d of %d) for ticket %d (in %f s)"), m_data_beacon.cid_dat_send_msg.message_index, m_data_beacon.cid_dat_send_msg.n_sub_messages, m_ticket->seq, m_oway_timer.getElapsed()); } - if (m_ticket != NULL && m_data_beacon.cid_dat_send_msg.packetDataNextPart(1) != -1) + if (m_ticket != nullptr && m_data_beacon.cid_dat_send_msg.packetDataNextPart(1) != -1) { resetOneWayTimer(); - debug(DTR("Sending (handleBinaryMessage) part %d of %d for ticket %d will take up to %f s for %d bytes"), + trace(DTR("Sending (handleBinaryMessage) part %d of %d for ticket %d will take up to %f s for %d bytes"), m_data_beacon.cid_dat_send_msg.message_index, m_data_beacon.cid_dat_send_msg.n_sub_messages, - m_ticket == NULL ? -1 : m_ticket->seq, + m_ticket == nullptr ? -1 : m_ticket->seq, m_oway_timer.getTop(), m_data_beacon.cid_dat_send_msg.packet_len); - sendProtectedCommand(commandCreateSeatrac(CID_DAT_SEND, m_data_beacon)); + sendProtectedCommand(createCommand(CID_DAT_SEND, m_data_beacon)); } else { @@ -768,10 +603,13 @@ namespace Transports // Acoustic information can be computed when the target beacon replies with ACK. handleAcousticInformation(m_data_beacon.cid_dat_receive_msg.aco_fix); + if (m_data_beacon.cid_dat_receive_msg.packet_len > 0) + handleRxData(false); + // Data communication done - if (m_ticket != NULL) + if (m_ticket != nullptr) { - debug(DTR("Msg transmission complete for ticket %d (in %f s)"), + trace(DTR("Msg transmission complete for ticket %d (in %f s)"), m_ticket->seq, m_oway_timer.getElapsed()); clearTicket(IMC::UamTxStatus::UTS_DONE); @@ -780,30 +618,34 @@ namespace Transports return; } else - { - int data_rec_flag = m_data_beacon.cid_dat_receive_msg.packetDataDecode(); - if (data_rec_flag == 1) - { - std::string msg; - m_data_beacon.cid_dat_receive_msg.getFullMsg(msg); - handleRxMessage(msg); - debug("new data"); - } - - if (data_rec_flag == -1) - war(DTR("wrong message order")); + handleRxData(); + } - if (data_rec_flag == 0) - debug("colecting data"); - if(data_rec_flag == -2) - debug("no data size"); + void + handleRxData(bool acoustic=true) + { + int data_rec_flag = m_data_beacon.cid_dat_receive_msg.packetDataDecode(); + if (data_rec_flag == 1) + { + std::string msg; + m_data_beacon.cid_dat_receive_msg.getFullMsg(msg); + handleRxMessage(msg, acoustic); + trace("new data"); } + + if (data_rec_flag == -1) + war(DTR("wrong message order")); + if (data_rec_flag == 0) + trace("collecting data"); + if(data_rec_flag == -2) + trace("no data size"); } //! Publish received acoustic message. //! @param[in] str received message. + //! @param[in] acoustic handle acoustic information. void - handleRxMessage(const std::string& str) + handleRxMessage(const std::string& str, bool acoustic) { IMC::UamRxFrame msg; msg.data.assign((uint8_t*) &str[0], (uint8_t*) &str[str.size()]); @@ -811,7 +653,7 @@ namespace Transports // Lookup source system name. try { - msg.sys_src = lookupSystemName(m_data_beacon.cid_dat_receive_msg.aco_fix.src_id); + m_node_map.lookupName(m_data_beacon.cid_dat_receive_msg.aco_fix.src_id, msg.sys_src); } catch (...) { @@ -821,7 +663,7 @@ namespace Transports // Lookup destination system name. try { - msg.sys_dst = lookupSystemName(m_data_beacon.cid_dat_receive_msg.aco_fix.dest_id); + m_node_map.lookupName(m_data_beacon.cid_dat_receive_msg.aco_fix.dest_id, msg.sys_dst); } catch (...) { @@ -834,7 +676,8 @@ namespace Transports msg.flags |= IMC::UamRxFrame::URF_DELAYED; dispatch(msg); - handleAcousticInformation(m_data_beacon.cid_dat_receive_msg.aco_fix); + if (acoustic) + handleAcousticInformation(m_data_beacon.cid_dat_receive_msg.aco_fix); } //! Handle acoustic information from received data. @@ -846,7 +689,7 @@ namespace Transports // Lookup source system name. try { - sys_src = lookupSystemName(aco_fix.src_id); + m_node_map.lookupName(aco_fix.src_id, sys_src); } catch (...) { } @@ -861,7 +704,7 @@ namespace Transports { IMC::UamRxRange range; range.sys = sys_src; - if (m_ticket != NULL) + if (m_ticket != nullptr) range.seq = m_ticket->seq; range.value = range_dist; @@ -880,6 +723,20 @@ namespace Transports usblPosition.e = aco_fix.position_easting / 10.0; usblPosition.n = aco_fix.position_northing / 10.0; usblPosition.d = aco_fix.position_depth / 10.0; + usblPosition.accuracy = aco_fix.usbl_fit_error / 100.0; + + if (aco_fix.outputflags_list[4]) + { + debug("Position received with filter error flag set! Discarding it!"); + return; + } + + if ((float)(aco_fix.usbl_fit_error/100) > m_args.fit_threshold) + { + debug("Position received with fit error above defined threshold (%f > %f)! Discarding it!", + (float)(aco_fix.usbl_fit_error/100), m_args.fit_threshold); + return; + } dispatch(usblPosition); } @@ -897,6 +754,7 @@ namespace Transports usblAnglesMsg.phi = Angles::radians(aco_fix.attitude_roll / 10.0); usblAnglesMsg.theta = Angles::radians(aco_fix.attitude_pitch / 10.0); usblAnglesMsg.psi = Angles::radians(aco_fix.attitude_yaw / 10.0); + usblAnglesMsg.accuracy = aco_fix.usbl_fit_error / 100.0; dispatch(usblAnglesMsg); } @@ -912,28 +770,26 @@ namespace Transports if( !(m_data_beacon.cid_dat_send_msg.msg_type == MSG_OWAY || m_data_beacon.cid_dat_send_msg.msg_type == MSG_OWAYU)) { - int next_part_code = m_ticket == NULL ? -1 : m_data_beacon.cid_dat_send_msg.packetDataNextPart(0); + int next_part_code = m_ticket == nullptr ? -1 : m_data_beacon.cid_dat_send_msg.packetDataNextPart(0); if (next_part_code < MAX_MESSAGE_ERRORS && next_part_code > 0) { resetOneWayTimer(); debug(DTR("Error sending (handleCommunicationError) part %d of %d for ticket %d, resending"), m_data_beacon.cid_dat_send_msg.message_index, m_data_beacon.cid_dat_send_msg.n_sub_messages, - m_ticket == NULL ? -1 : m_ticket->seq); - sendProtectedCommand(commandCreateSeatrac(CID_DAT_SEND, m_data_beacon)); + m_ticket == nullptr ? -1 : m_ticket->seq); + sendProtectedCommand(createCommand(CID_DAT_SEND, m_data_beacon)); } else { war(DTR("Communication failed for ticket %d %d"), - m_ticket == NULL ? -1 : m_ticket->seq, + m_ticket == nullptr ? -1 : m_ticket->seq, next_part_code); clearTicket(IMC::UamTxStatus::UTS_FAILED); } } else - { - war(DTR("Next msg or part send to son for ticket %d with ERROR"), m_ticket == NULL ? -1 : m_ticket->seq); - } + war(DTR("Next msg or part send to son for ticket %d with ERROR"), m_ticket == nullptr ? -1 : m_ticket->seq); } //! Correct data according with mounting position. @@ -991,21 +847,22 @@ namespace Transports m_magfield.x = (fp32_t) m_data_beacon.cid_status_msg.ahrs_comp_mag_x; m_magfield.y = (fp32_t)m_data_beacon.cid_status_msg.ahrs_comp_mag_y; m_magfield.z = (fp32_t) m_data_beacon.cid_status_msg.ahrs_comp_mag_z; - - m_euler.theta= Angles::radians( ((fp32_t) m_data_beacon.cid_status_msg.attitude_roll)/10); - m_euler.psi = Angles::radians(((fp32_t) m_data_beacon.cid_status_msg.attitude_pitch)/10); + //Euler angles. + m_euler.theta = Angles::radians( ((fp32_t) m_data_beacon.cid_status_msg.attitude_roll)/10); + m_euler.psi = Angles::radians(((fp32_t) m_data_beacon.cid_status_msg.attitude_pitch)/10); m_euler.psi_magnetic = m_euler.psi; - m_euler.phi= Angles::radians(((fp32_t) m_data_beacon.cid_status_msg.attitude_yaw)/10); + m_euler.phi = Angles::radians(((fp32_t) m_data_beacon.cid_status_msg.attitude_yaw)/10); // Angular Velocity. m_agvel.x = Angles::radians((fp32_t) m_data_beacon.cid_status_msg.ahrs_comp_gyro_x); m_agvel.y = Angles::radians((fp32_t) m_data_beacon.cid_status_msg.ahrs_comp_gyro_y); m_agvel.z = Angles::radians((fp32_t) m_data_beacon.cid_status_msg.ahrs_comp_gyro_z); - //Euler angles. + m_euler.time = ((fp32_t) m_data_beacon.cid_status_msg.timestamp)/1000; m_accel.time = m_euler.time; m_agvel.time = m_euler.time; m_magfield.time = m_euler.time; rotateData(); + // Dispatch messages. dispatch(m_euler, DF_KEEP_TIME); dispatch(m_accel, DF_KEEP_TIME); @@ -1017,17 +874,17 @@ namespace Transports //! Handle Pressure, Depth, Temperature and Sound Speed data and dispatch. //! The method tries to dispatch data prom sensors: Pressure, Depth, Temperature, and Sound Speed data void - handlePressureSensor (void) + handlePressureSensor(void) { - m_depth.value = ((fp32_t) (m_data_beacon.cid_status_msg.environment_depth)) / 10.0; //int32_t // m_channel_readout * m_args.depth_conv; - m_pressure.value = (((fp32_t) (m_data_beacon.cid_status_msg.environment_pressure)) / 1000.0) * Math::c_pascal_per_bar; - m_temperature.value = ((fp32_t) (m_data_beacon.cid_status_msg.environment_temperature)) / 10.0; //int16_t//m_channel_readout; - m_sspeed.value = ((fp32_t) (m_data_beacon.cid_status_msg.environment_vos)) / 10.0; //uint16_t + m_depth.value = ((fp32_t) (m_data_beacon.cid_status_msg.environment_depth)) / 10.0; + m_pressure.value = (((fp32_t) (m_data_beacon.cid_status_msg.environment_pressure)) / 1000.0) * Math::c_pascal_per_bar; + m_temperature.value = ((fp32_t) (m_data_beacon.cid_status_msg.environment_temperature)) / 10.0; + m_sspeed.value = ((fp32_t) (m_data_beacon.cid_status_msg.environment_vos)) / 10.0; dispatch(m_depth); dispatch(m_pressure); dispatch(m_temperature); dispatch(m_sspeed); - trace("Received from modem: Depth %f m | Presure %f P | Temperature %f \u00B0C | SoundSpeed %f m/s", + trace("Received from modem: Depth %f m | Pressure %f P | Temperature %f \u00B0C | SoundSpeed %f m/s", m_depth.value, m_pressure.value, m_temperature.value, @@ -1038,7 +895,7 @@ namespace Transports //! If the acknowledged message is an OWAY and it is compound //! by more than one packet, the method sends the following packet. //! If the sending fails, it tries to send the packet again. - //! If the modem is busy, it tries to send the packet to poll the status. + //! If the modem is busy, the packet to be sent is queued. void handleDatSendResponse(void) { @@ -1046,17 +903,21 @@ namespace Transports { if (m_data_beacon.cid_dat_send_msg.status != CST_XCVR_BUSY) handleCommunicationError(); + else + { + inf("Modem is Busy! Queuing message."); + sendProtectedCommand(createCommand(CID_DAT_QUEUE_SET, m_data_beacon)); + } } } void consume(const IMC::UamTxFrame* msg) { - debug(DTR("Received UamTxFrame with dst=0x%04X. Msg for system '%s'"), msg->getDestination(), msg->sys_dst.c_str()); + if (m_state != STA_ACTIVE) + return; - std::string hex = String::toHex(msg->data); - std::vector data_t; - std::copy(hex.begin(), hex.end(), std::back_inserter(data_t)); + debug(DTR("Received UamTxFrame with dst=0x%04X. Msg for system '%s'"), msg->getDestination(), msg->sys_dst.c_str()); if (msg->getDestination() != getSystemId()) return; @@ -1064,6 +925,9 @@ namespace Transports if (msg->getDestinationEntity() != 255 && msg->getDestinationEntity() != getEntityId()) return; + std::string hex = String::toHex(msg->data); + std::vector data_t; + std::copy(hex.begin(), hex.end(), std::back_inserter(data_t)); // Create and fill new ticket. Ticket ticket; @@ -1072,7 +936,7 @@ namespace Transports ticket.seq = msg->seq; ticket.ack = (msg->flags & IMC::UamTxFrame::UTF_ACK) != 0; - debug(DTR("Creating ticket %d"), ticket.seq); + trace(DTR("Creating ticket %d"), ticket.seq); if (msg->sys_dst == getSystemName()) { @@ -1081,32 +945,30 @@ namespace Transports return; } - try - { - ticket.addr = lookupSystemAddress(msg->sys_dst); - } - catch (...) + unsigned dst = 0; + if (!m_node_map.lookupAddress(msg->sys_dst, dst)) { - war(DTR("invalid system name %s"), msg->sys_dst.c_str()); + war(DTR("invalid system name: %s"), msg->sys_dst.c_str()); sendTxStatus(ticket, IMC::UamTxStatus::UTS_INV_ADDR); return; } + ticket.addr = dst; // Fail if busy. if (m_data_beacon.cid_dat_send_msg.packetDataSendStatus()) { sendTxStatus(ticket, IMC::UamTxStatus::UTS_BUSY); - debug(DTR("Sending UamTxStatus::UTS_BUSY. Ticket %d died"), ticket.seq); + debug(DTR("Sending UamTxStatus::UTS_BUSY. Ticket %d died"), ticket.seq); return; } // Replace ticket and transmit. replaceTicket(ticket); sendTxStatus(ticket, IMC::UamTxStatus::UTS_IP); - debug(DTR("Sending UamTxStatus::UTS_IP. Ticket %d being processed"), ticket.seq); + trace(DTR("Sending UamTxStatus::UTS_IP. Ticket %d being processed"), ticket.seq); // Fill the message type. - if ((ticket.addr != 0) && (ticket.ack == true)) + if ((ticket.addr != 0) && ticket.ack) { if (m_args.usbl_mode) { @@ -1119,7 +981,7 @@ namespace Transports { m_data_beacon.cid_dat_send_msg.msg_type = MSG_REQ; } - debug(DTR("Configuration as %s %s"), m_args.usbl_mode ? "USBL" : "MSG_ONLY", m_args.usbl_mode && m_args.enhanced_usbl ? "enhanced" : ""); + trace(DTR("Configuration as %s %s"), m_args.usbl_mode ? "USBL" : "MSG_ONLY", m_args.usbl_mode && m_args.enhanced_usbl ? "enhanced" : ""); } else { @@ -1127,7 +989,7 @@ namespace Transports m_data_beacon.cid_dat_send_msg.msg_type = MSG_OWAYU; else m_data_beacon.cid_dat_send_msg.msg_type = MSG_OWAY; - debug(DTR("Configuration as ONEWAY %s"), m_args.usbl_mode ? "USBL" : "MSG_ONLY"); + trace(DTR("Configuration as ONEWAY %s"), m_args.usbl_mode ? "USBL" : "MSG_ONLY"); } int code; @@ -1146,14 +1008,14 @@ namespace Transports break; default: resetOneWayTimer(); - debug(DTR("Sending package %f s"), m_oway_timer.getTop()); - debug(DTR("Sending (consume UamTxFrame) part %d of %d for ticket %d will take up to %f s for %d bytes"), + trace(DTR("Sending package %f s"), m_oway_timer.getTop()); + debug(DTR("Sending (consume UamTxFrame) part %d of %d for ticket %d will take up to %f s for %d bytes"), m_data_beacon.cid_dat_send_msg.message_index, m_data_beacon.cid_dat_send_msg.n_sub_messages, ticket.seq, m_oway_timer.getTop(), m_data_beacon.cid_dat_send_msg.packet_len); - sendProtectedCommand(commandCreateSeatrac(CID_DAT_SEND, m_data_beacon)); + sendProtectedCommand(createCommand(CID_DAT_SEND, m_data_beacon)); break; } } @@ -1179,14 +1041,13 @@ namespace Transports //! @param[in] reason reason value. //! @param[in] error error message. void - clearTicket(IMC::UamTxStatus::ValueEnum reason, - const std::string& error = "") + clearTicket(IMC::UamTxStatus::ValueEnum reason, const std::string& error = "") { - if (m_ticket != NULL) + if (m_ticket != nullptr) { sendTxStatus(*m_ticket, reason, error); delete m_ticket; - m_ticket = NULL; + m_ticket = nullptr; } } @@ -1199,30 +1060,26 @@ namespace Transports m_ticket = new Ticket(ticket); } - //! Lookup system address. - //! @param[in] name system name - //! @return system address. - unsigned - lookupSystemAddress(const std::string& name) + //! Process new data. + void + processNewData(void) { - MapName::iterator itr = m_modem_names.find(name); - if (itr == m_modem_names.end()) - throw std::runtime_error("unknown system name"); + if (m_data_beacon.newDataAvailable(CID_DAT_RECEIVE)) + handleBinaryMessage(); - return itr->second; - } + if (m_data_beacon.newDataAvailable(CID_DAT_SEND)) + handleDatSendResponse(); - //! Lookup system name. - //! @param[in] addr system address. - //! @return system name. - std::string - lookupSystemName(unsigned addr) - { - MapAddr::iterator itr = m_modem_addrs.find(addr); - if (itr == m_modem_addrs.end()) - throw std::runtime_error("unknown system address"); + if (m_data_beacon.newDataAvailable(CID_DAT_ERROR)) + handleCommunicationError(); - return itr->second; + if(m_data_beacon.newDataAvailable(CID_STATUS)) + { + if(m_args.ahrs_mode) + handleAhrsData(); + if(m_args.pressure_sensor_mode) + handlePressureSensor(); + } } //! Check for incoming data. @@ -1230,46 +1087,37 @@ namespace Transports void processInput(double timeout = 1.0) { + // Wait for modem configuration + if (!isActive() || !m_config_status) + return; + double deadline = Clock::get() + timeout; do { consumeMessages(); - readSentence(); + m_driver->readSentence(); + processNewData(); checkTxOWAY(); - if (m_state_entity != STA_ERR_STP) + if ((m_state != STA_ERR_STP) && hasConnection()) { - if (isActive()) - setAndSendState(STA_ACTIVE); + if (!isRestricted()) + setState(STA_ACTIVE); else - setAndSendState(STA_IDLE); + setState(STA_IDLE); } } while (Clock::get() <= deadline); } void - consume(const IMC::VehicleMedium* msg) - { - if (m_args.only_underwater) - { - if (msg->medium == IMC::VehicleMedium::VM_UNDERWATER) - m_stop_comms = false; - else - m_stop_comms = true; - return; - } - - m_stop_comms = false; - } - - void - resetOneWayTimer() + resetOneWayTimer(void) { int multiplier = 2; if(!(m_data_beacon.cid_dat_send_msg.msg_type == MSG_OWAY || m_data_beacon.cid_dat_send_msg.msg_type == MSG_OWAYU)) - multiplier = m_args.ack_timeout_time_multiplier; + multiplier = c_ack_timeout_multiplier; + m_oway_timer.setTop((m_data_beacon.cid_dat_send_msg.packet_len * 8 * 1.0/c_acoustic_bitrate + (m_args.max_range * 1.0 / MIN_SOUND_SPEED)) * multiplier ); @@ -1292,27 +1140,27 @@ namespace Transports { if (m_oway_timer.overflow()) { - debug(DTR("NOACK Success transmission complete (part %d of %d) for ticket %d (in %f s)"), + trace(DTR("NOACK Success transmission complete (part %d of %d) for ticket %d (in %f s)"), m_data_beacon.cid_dat_send_msg.message_index, m_data_beacon.cid_dat_send_msg.n_sub_messages, - m_ticket == NULL ? -1 : m_ticket->seq, + m_ticket == nullptr ? -1 : m_ticket->seq, m_oway_timer.getElapsed()); if (m_data_beacon.cid_dat_send_msg.packetDataNextPart(1) != -1) { resetOneWayTimer(); - debug(DTR("Sending (checkTxOWAY) part %d of %d for ticket %d will take up to %f s for %d bytes"), + trace(DTR("Sending (checkTxOWAY) part %d of %d for ticket %d will take up to %f s for %d bytes"), m_data_beacon.cid_dat_send_msg.message_index, m_data_beacon.cid_dat_send_msg.n_sub_messages, - m_ticket == NULL ? -1 : m_ticket->seq, + m_ticket == nullptr ? -1 : m_ticket->seq, m_oway_timer.getTop(), m_data_beacon.cid_dat_send_msg.packet_len); - sendProtectedCommand(commandCreateSeatrac(CID_DAT_SEND, m_data_beacon)); + sendProtectedCommand(createCommand(CID_DAT_SEND, m_data_beacon)); } else { - debug(DTR("Msg transmission complete for ticket %d (in %f s)"), - m_ticket == NULL ? -1 : m_ticket->seq, + trace(DTR("Msg transmission complete for ticket %d (in %f s)"), + m_ticket == nullptr ? -1 : m_ticket->seq, m_oway_timer.getElapsed()); clearTicket(IMC::UamTxStatus::UTS_DONE); } @@ -1321,7 +1169,7 @@ namespace Transports else { // is with ack - if (m_ticket != NULL && m_oway_timer.overflow()) + if (m_ticket != nullptr && m_oway_timer.overflow()) { //Took too long, lets bail with error war(DTR("ACK TIMEOUT: Msg transmission with ack for ticket %d timeout ACK. Lets bail with error!! (%f s > %f s)"), @@ -1341,11 +1189,19 @@ namespace Transports { while (!stopping()) { - // Modem. + // Retry initialization if it wasn't possible on boot + if (m_driver == nullptr && !m_config_status && isActive()) + onResourceInitialization(); + else if (!isActive() && m_state == STA_ACTIVE) + setState(STA_IDLE); + + // Check for incoming data. processInput(); + waitForMessages(1.0); - if (Clock::get() >= (m_last_input + c_input_tout)) - setAndSendState(STA_ERR_COM); + // Check modem connection + if (!hasConnection() && isActive()) + setState(STA_ERR_COM); } } };