From ec22971077ce430aa686643a5b46f149e8ad8e89 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Sat, 4 Mar 2023 11:59:56 +0000 Subject: [PATCH 01/35] Transports/Seatrac: Fixed small typos. --- src/Transports/Seatrac/DataTypes.hpp | 3 +- src/Transports/Seatrac/DebugMsg.hpp | 46 +++++++++++++-------------- src/Transports/Seatrac/MsgTypes.hpp | 16 +++++----- src/Transports/Seatrac/Parser.hpp | 47 ++++++++++++++-------------- src/Transports/Seatrac/Task.cpp | 22 ++++++------- 5 files changed, 66 insertions(+), 68 deletions(-) diff --git a/src/Transports/Seatrac/DataTypes.hpp b/src/Transports/Seatrac/DataTypes.hpp index d3881a7326..ee4d051fca 100644 --- a/src/Transports/Seatrac/DataTypes.hpp +++ b/src/Transports/Seatrac/DataTypes.hpp @@ -174,7 +174,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, diff --git a/src/Transports/Seatrac/DebugMsg.hpp b/src/Transports/Seatrac/DebugMsg.hpp index 36d3977dbd..0ac7face7d 100644 --- a/src/Transports/Seatrac/DebugMsg.hpp +++ b/src/Transports/Seatrac/DebugMsg.hpp @@ -437,30 +437,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/MsgTypes.hpp b/src/Transports/Seatrac/MsgTypes.hpp index 38cbf378c0..7e4397ea19 100644 --- a/src/Transports/Seatrac/MsgTypes.hpp +++ b/src/Transports/Seatrac/MsgTypes.hpp @@ -148,7 +148,7 @@ namespace Transports uint8_t status; }; - struct CidPingErrorMsg + struct CidPingErrorMsg { CST_E status; uint8_t beacon_id; @@ -297,15 +297,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 +337,7 @@ namespace Transports } return 0; } - return erro_code; + return error_code; } //! Builds the next msg package @@ -436,7 +436,7 @@ namespace Transports struct CidXcvrFixMsg { - Acofix_t aco_fix; + Acofix_t aco_fix; }; struct CidSysRebootMsg @@ -451,9 +451,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..787c8f9e9b 100644 --- a/src/Transports/Seatrac/Parser.hpp +++ b/src/Transports/Seatrac/Parser.hpp @@ -54,13 +54,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 +71,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; @@ -126,8 +126,8 @@ namespace Transports }; //! 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) @@ -183,8 +183,8 @@ 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[in] message_type type of message to decode. + //! @param[in] msg_raw raw message received by uart //! @param[out] data_Beacon pointer where the data is stored. void dataParser(uint16_t message_type, const char* msg_raw, DataSeatrac& data_Beacon) @@ -384,35 +384,35 @@ namespace Transports 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 = updateEcoFix(&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; @@ -515,9 +515,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; } } @@ -639,7 +638,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..3872f317bd 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -93,7 +93,7 @@ namespace Transports 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; @@ -150,7 +150,7 @@ namespace Transports MapAddr m_modem_addrs; //! Current transmission ticket. Ticket* m_ticket; - // Save modem commands. + //! Save modem commands. IMC::DevDataText m_dev_data; //! Euler angles message. IMC::EulerAngles m_euler; @@ -162,11 +162,11 @@ namespace Transports IMC::MagneticField m_magfield; //! Current sound speed. IMC::SoundSpeed m_sspeed; - // Depth. + //! Depth. IMC::Depth m_depth; - // Pressure. + //! Pressure. IMC::Pressure m_pressure; - // Measured temperature. + //! Measured temperature. IMC::Temperature m_temperature; //! Rotation Matrix to correct mounting position. Math::Matrix m_rotation; @@ -330,7 +330,7 @@ namespace Transports handlePressureSensor(); } - //todo send environment_supply + //TODO: send environment_supply //m_data_beacon.cid_status_msg.environment_supply; //uint16_t } } @@ -1017,10 +1017,10 @@ 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_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 dispatch(m_depth); @@ -1064,7 +1064,6 @@ namespace Transports if (msg->getDestinationEntity() != 255 && msg->getDestinationEntity() != getEntityId()) return; - // Create and fill new ticket. Ticket ticket; ticket.imc_sid = msg->getSource(); @@ -1179,8 +1178,7 @@ 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) { @@ -1264,7 +1262,7 @@ namespace Transports } void - resetOneWayTimer() + resetOneWayTimer(void) { int multiplier = 2; if(!(m_data_beacon.cid_dat_send_msg.msg_type == MSG_OWAY || From 66ea7e8d0b908823274024800a42f4a9ae78ae5b Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Tue, 7 Mar 2023 18:08:36 +0000 Subject: [PATCH 02/35] Transports/Seatrac: Pedantic changes. --- src/Transports/Seatrac/Task.cpp | 128 +++++++++++++++----------------- 1 file changed, 59 insertions(+), 69 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 3872f317bd..c41b74c996 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -119,7 +119,7 @@ namespace Transports //! Config Status. bool m_config_status; //! c_preamble detected - bool m_pre_detected; + bool m_preamble; //! Current state. EntityStates m_state_entity; //! Entity states. @@ -176,13 +176,13 @@ namespace Transports //! @param[in] ctx context. Task(const std::string& name, Tasks::Context& ctx) : DUNE::Tasks::Task(name, ctx), - m_handle(NULL), + m_handle(nullptr), m_config_status(false), - m_pre_detected(false), + m_preamble(false), m_stop_comms(false), m_usbl_receiver(false), m_tstamp(0), - m_ticket(NULL) + m_ticket(nullptr) { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_MANEUVER, @@ -207,11 +207,11 @@ namespace Transports param("AHRS Mode", m_args.arhs_mode) .defaultValue("false") - .description("Enable the AHRS information to used in navigation"); + .description("Enable the AHRS information to be used in navigation"); param("Pressure Sensor Mode", m_args.pressure_sensor_mode) .defaultValue("false") - .description("Enable the pressure sensor, depth, sound velocity and temperature information "); + .description("Enable pressure sensor, depth, sound velocity and temperature information "); param("Use Internal Pressure Sensor for Medium", m_args.use_pressure_sensor_for_medium) .defaultValue("false") @@ -219,7 +219,7 @@ namespace Transports 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,7 +241,7 @@ 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") @@ -251,11 +251,11 @@ namespace Transports 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)"); + .description("A time multiplier to wait before timeout for acknowledge (if ack requested)"); param("Dummy Connection", m_args.dummy_connection) .defaultValue("false") - .description("To assume a dummy connection and not a modem (no replies"); + .description("To assume a dummy connection and not a modem (no replies)"); // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; @@ -276,17 +276,17 @@ namespace Transports //! Set entity state. //! @param[in] state new entity state. void - setAndSendState(EntityStates state) + setState(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. + //! Check validity of received sentence (CRC and minimum length). + //! @return true if message is valid, false otherwise. bool - processSentence(void) + checkValidity(void) { bool msg_validity = false; uint16_t crc, crc2; @@ -299,8 +299,10 @@ namespace Transports if (crc == crc2) msg_validity = true; else - war("%s", DTR(Status::getString(Status::CODE_INVALID_CHECKSUM))); + war("Received message not valid: %s", DTR(Status::getString(Status::CODE_INVALID_CHECKSUM))); } + else + war(DTR("Received message not valid: insufficient message length")); return msg_validity; } @@ -308,7 +310,7 @@ namespace Transports void processNewData(void) { - if(m_config_status==true) + if(m_config_status) { if (m_data_beacon.newDataAvailable(CID_DAT_RECEIVE)) handleBinaryMessage(); @@ -321,14 +323,10 @@ namespace Transports if(m_data_beacon.newDataAvailable(CID_STATUS)) { - if(m_args.arhs_mode == true) - { + if(m_args.arhs_mode) handleAhrsData(); - } - if(m_args.pressure_sensor_mode == true) - { + if(m_args.pressure_sensor_mode) handlePressureSensor(); - } //TODO: send environment_supply //m_data_beacon.cid_status_msg.environment_supply; //uint16_t @@ -345,11 +343,10 @@ namespace Transports 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); + size_t 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) @@ -359,9 +356,9 @@ namespace Transports { m_dev_data.value.assign(sanitize(m_data)); dispatch(m_dev_data); - if(m_pre_detected==true) + if(m_preamble) { - if (processSentence()) + if (checkValidity()) { msg_raw = m_datahex.data(); std::memcpy(&typemes, msg_raw, 1); @@ -371,7 +368,7 @@ namespace Transports typemes = 0; } } - m_pre_detected = false; + m_preamble = false; m_data.clear(); } else @@ -379,7 +376,7 @@ namespace Transports if (bfr[i] == c_preamble) { m_data.clear(); - m_pre_detected = true; + m_preamble = true; } else if (bfr[i] != '\r') { @@ -411,10 +408,10 @@ namespace Transports void onResourceAcquisition(void) { - setAndSendState(STA_BOOT); + setState(STA_BOOT); try { - if (m_args.only_underwater == true) + if (m_args.only_underwater) m_stop_comms = true; if (openSocket()) @@ -482,7 +479,7 @@ namespace Transports StatusMode_E status_mode= STATUS_MODE_1HZ; bool chage_IMU = true; - if (m_args.arhs_mode == true) + if (m_args.arhs_mode) { status_mode = STATUS_MODE_10HZ; chage_IMU = isCalibrated(); @@ -492,7 +489,7 @@ namespace Transports && (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)) + && chage_IMU)) { m_data_beacon.cid_settings_msg.status_flags = status_mode; m_data_beacon.cid_settings_msg.status_output = output_flags; @@ -500,7 +497,7 @@ namespace Transports 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) + if(!chage_IMU) { 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]; @@ -516,18 +513,18 @@ namespace Transports if (m_data_beacon.cid_settings_msg.xcvr_beacon_id != m_addr) { - setAndSendState(STA_ERR_STP); + setState(STA_ERR_STP); war(DTR("failed to configure device")); } inf("ready"); - setAndSendState(STA_IDLE); + setState(STA_IDLE); m_config_status = true; } else { inf("ready (settings already set)"); - setAndSendState(STA_IDLE); + setState(STA_IDLE); m_config_status = true; } @@ -549,7 +546,7 @@ namespace Transports else { err("%s", DTR(Status::getString(CODE_COM_ERROR))); - setAndSendState(STA_ERR_STP); + setState(STA_ERR_STP); throw std::runtime_error(m_states[m_state_entity].description); } } @@ -569,7 +566,7 @@ 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)) @@ -580,7 +577,7 @@ namespace Transports void runCalibration(void) { - if (m_handle == NULL) + if (m_handle == nullptr) return; // See if vehicle has same hard iron calibration parameters. @@ -728,11 +725,7 @@ namespace Transports bool hasConnection(void) { - if (Clock::get() >= (m_last_input + c_input_tout)) - { - return false; - } - return true; + return (Clock::get() < (m_last_input + c_input_tout)); } //! Processing incoming data. @@ -742,7 +735,7 @@ 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)"), m_data_beacon.cid_dat_send_msg.message_index, @@ -751,13 +744,13 @@ namespace Transports 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"), 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)); @@ -769,7 +762,7 @@ namespace Transports handleAcousticInformation(m_data_beacon.cid_dat_receive_msg.aco_fix); // Data communication done - if (m_ticket != NULL) + if (m_ticket != nullptr) { debug(DTR("Msg transmission complete for ticket %d (in %f s)"), m_ticket->seq, @@ -794,7 +787,7 @@ namespace Transports war(DTR("wrong message order")); if (data_rec_flag == 0) - debug("colecting data"); + debug("collecting data"); if(data_rec_flag == -2) debug("no data size"); } @@ -861,7 +854,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; @@ -912,27 +905,27 @@ 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); + m_ticket == nullptr ? -1 : m_ticket->seq); sendProtectedCommand(commandCreateSeatrac(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); } } @@ -1105,7 +1098,7 @@ namespace Transports debug(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) { @@ -1180,11 +1173,11 @@ namespace Transports void 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; } } @@ -1238,9 +1231,9 @@ namespace Transports if (m_state_entity != STA_ERR_STP) { if (isActive()) - setAndSendState(STA_ACTIVE); + setState(STA_ACTIVE); else - setAndSendState(STA_IDLE); + setState(STA_IDLE); } } while (Clock::get() <= deadline); @@ -1251,10 +1244,7 @@ namespace Transports { if (m_args.only_underwater) { - if (msg->medium == IMC::VehicleMedium::VM_UNDERWATER) - m_stop_comms = false; - else - m_stop_comms = true; + m_stop_comms = !(msg->medium == IMC::VehicleMedium::VM_UNDERWATER); return; } @@ -1293,7 +1283,7 @@ namespace Transports debug(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) @@ -1302,7 +1292,7 @@ namespace Transports debug(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)); @@ -1310,7 +1300,7 @@ namespace Transports else { debug(DTR("Msg transmission complete for ticket %d (in %f s)"), - m_ticket == NULL ? -1 : m_ticket->seq, + m_ticket == nullptr ? -1 : m_ticket->seq, m_oway_timer.getElapsed()); clearTicket(IMC::UamTxStatus::UTS_DONE); } @@ -1319,7 +1309,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)"), @@ -1342,8 +1332,8 @@ namespace Transports // Modem. processInput(); - if (Clock::get() >= (m_last_input + c_input_tout)) - setAndSendState(STA_ERR_COM); + if (!hasConnection()) + setState(STA_ERR_COM); } } }; From b606254b8203efdc82d86d319ad3528c8efc7c3f Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 8 Mar 2023 09:40:59 +0000 Subject: [PATCH 03/35] Transports/Seatrac: Moved modem configuration to dedicated method and other small changes. --- src/Transports/Seatrac/Parser.hpp | 8 +- src/Transports/Seatrac/Task.cpp | 332 +++++++++++++++--------------- 2 files changed, 172 insertions(+), 168 deletions(-) diff --git a/src/Transports/Seatrac/Parser.hpp b/src/Transports/Seatrac/Parser.hpp index 787c8f9e9b..70ee0791ef 100644 --- a/src/Transports/Seatrac/Parser.hpp +++ b/src/Transports/Seatrac/Parser.hpp @@ -109,7 +109,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,11 +117,11 @@ namespace Transports if (new_message[setdata] == 1) { new_message[setdata] = 0; - return 1; + return true; } } - return 0; + return false; } }; @@ -526,7 +526,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; diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index c41b74c996..50c94d713c 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -81,15 +81,15 @@ namespace Transports 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; //! 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; @@ -101,8 +101,6 @@ namespace Transports uint16_t max_range; //! Timeout time multiplier for ack wait uint8_t ack_timeout_time_multiplier; - //! dummy connection - bool dummy_connection; }; //! Map of system's names. @@ -142,7 +140,7 @@ namespace Transports 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; @@ -205,7 +203,7 @@ namespace Transports .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 be used in navigation"); @@ -253,10 +251,6 @@ namespace Transports .minimumValue("3") .description("A time multiplier to wait before timeout for acknowledge (if ack requested)"); - param("Dummy Connection", m_args.dummy_connection) - .defaultValue("false") - .description("To assume a dummy connection and not a modem (no replies)"); - // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); @@ -323,7 +317,7 @@ namespace Transports if(m_data_beacon.newDataAvailable(CID_STATUS)) { - if(m_args.arhs_mode) + if(m_args.ahrs_mode) handleAhrsData(); if(m_args.pressure_sensor_mode) handlePressureSensor(); @@ -341,48 +335,46 @@ namespace Transports { // Initialize received message parser char bfr[c_bfr_size]; - uint16_t typemes = 0; + uint16_t type = 0; const char* msg_raw; - if (Poll::poll(*m_handle, 0.001)) + if (!Poll::poll(*m_handle, 0.001)) + return; + + size_t 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) { - size_t 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') { - // Detected line termination. - if (bfr[i] == '\n') + m_dev_data.value.assign(sanitize(m_data)); + dispatch(m_dev_data); + if (m_preamble) { - m_dev_data.value.assign(sanitize(m_data)); - dispatch(m_dev_data); - if(m_preamble) + if (checkValidity()) { - if (checkValidity()) - { - 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; - } + msg_raw = m_datahex.data(); + std::memcpy(&type, msg_raw, 1); + dataParser(type, msg_raw + 1, m_data_beacon); + processNewData(); + printDebugFunction(type, m_data_beacon, this); + type = 0; } - m_preamble = false; - m_data.clear(); } - else + m_preamble = false; + m_data.clear(); + } + else + { + if (bfr[i] == c_preamble) { - if (bfr[i] == c_preamble) - { - m_data.clear(); - m_preamble = true; - } - else if (bfr[i] != '\r') - { - m_data.push_back(bfr[i]); - } + m_data.clear(); + m_preamble = true; } + else if (bfr[i] != '\r') + m_data.push_back(bfr[i]); } } } @@ -436,7 +428,7 @@ namespace Transports std::string agent = getSystemName(); std::vector addrs = m_ctx.config.options(m_args.addr_section); - // verify modem local address value. + // Verify modem local address value. for (unsigned i = 0; i < addrs.size(); ++i) { unsigned addr = 0; @@ -447,108 +439,125 @@ namespace Transports 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"), agent.c_str())); + + try + { + configure(); } + catch (...) + { + err("%s", DTR(Status::getString(CODE_COM_ERROR))); + setState(STA_ERR_STP); + throw std::runtime_error(m_states[m_state_entity].description); + } + } - m_last_input = Clock::get(); - processInput(); + //! 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) + //! @return true if settings match, false otherwise + bool + checkSettings(StatusMode_E status_mode, uint8_t output_flags, uint8_t xcvr_flags, bool ahrs) + { + 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 == m_args.max_range) + && ahrs); + } - if (hasConnection()) - { - do - { - sendCommand(commandCreateSeatrac(CID_SETTINGS_GET, m_data_beacon)); - processInput(); - } - while (m_data_beacon.newDataAvailable(CID_SETTINGS_GET) == 0 && !m_args.dummy_connection); + //! Seatrac Configuration + void + configure(void) + { + // Retrieve current settings and system information + sendCommandAndWait(createCommand(CID_SETTINGS_GET, m_data_beacon), 1); + sendCommandAndWait(createCommand(CID_SYS_INFO, m_data_beacon), 1); - 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; - 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 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; - 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; - if (m_usbl_receiver) - xcvr_flags |= USBL_USE_AHRS_FLAG | XCVR_USBL_MSGS_FLAG; + StatusMode_E status_mode = STATUS_MODE_1HZ; + bool ahrs = true; + if (m_args.ahrs_mode) + { + status_mode = STATUS_MODE_10HZ; + ahrs = isCalibrated(); + } - StatusMode_E status_mode= STATUS_MODE_1HZ; - bool chage_IMU = true; - if (m_args.arhs_mode) + // Verify modem settings + if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs)) + { + // 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 = m_args.max_range; + + if(!ahrs) { - status_mode = STATUS_MODE_10HZ; - chage_IMU = isCalibrated(); + 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]; } - 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)) - { - 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) - { - 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) - { - setState(STA_ERR_STP); - war(DTR("failed to configure device")); - } - inf("ready"); - setState(STA_IDLE); - m_config_status = true; - } - else + // Saving settings + inf("Saving settings to modem"); + sendCommandAndWait(createCommand(CID_SETTINGS_SET, m_data_beacon), 2); + sendCommandAndWait(createCommand(CID_SETTINGS_SAVE, m_data_beacon), 2); + 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)) { - inf("ready (settings already set)"); - setState(STA_IDLE); - m_config_status = true; + setState(STA_ERR_STP); + war(DTR("Failed to configure device")); } - 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"); + // Configuration complete + inf("Modem ready."); + setState(STA_IDLE); + m_config_status = true; } else { - err("%s", DTR(Status::getString(CODE_COM_ERROR))); - setState(STA_ERR_STP); - throw std::runtime_error(m_states[m_state_entity].description); + // Configuration complete + inf("Modem ready (settings successfully set)."); + setState(STA_IDLE); + m_config_status = true; } + + // Print system information + 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 ? "" : " Not"); } //! Update parameters. @@ -557,7 +566,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++) @@ -568,22 +577,19 @@ namespace Transports if (m_handle != nullptr) { - if (paramChanged(m_args.hard_iron)) runCalibration(); } } - //! Routine to run calibration proceedings. + + //! Routine to check if AHRS has matching hard-iron calibration parameters and set new values if needed. void runCalibration(void) { - if (m_handle == nullptr) - return; - - // See if vehicle has same hard iron calibration parameters. + // Check if vehicle has matching hard-iron calibration parameters. if (!isCalibrated()) { - // Set hard iron calibration parameters and reset device. + // Set hard-iron calibration parameters and reset device. if (!setHardIron()) { throw RestartNeeded(DTR("failed to set hard-iron correction factors"), 5); @@ -619,46 +625,39 @@ namespace Transports dispatch(sp); } - //! Check if sensor has the same hard iron calibration parameters. + //! 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_args.hard_iron[0]*100))) + if( ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_x*100)) != ( (int32_t) (m_args.hard_iron[0]*100)) + || ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y*100)) != ( (int32_t) (m_args.hard_iron[1]*100)) + || ((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; - } - 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; + inf(DTR("AHRS has different calibration parameters.")); + return false; } - debug("Is calibrated"); + + inf("AHRS is calibrated (matching parameters)."); return true; } - //! Set new hard iron calibration parameters. + //! Set new hard-iron calibration parameters. //! @return true if successful, false otherwise. bool setHardIron(void) { + inf("Set new hard-iron calibration parameters and reset device."); 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]) + 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_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])) return false; return true; @@ -753,7 +752,7 @@ namespace Transports 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 { @@ -913,7 +912,7 @@ namespace Transports m_data_beacon.cid_dat_send_msg.message_index, m_data_beacon.cid_dat_send_msg.n_sub_messages, m_ticket == nullptr ? -1 : m_ticket->seq); - sendProtectedCommand(commandCreateSeatrac(CID_DAT_SEND, m_data_beacon)); + sendProtectedCommand(createCommand(CID_DAT_SEND, m_data_beacon)); } else { @@ -1145,7 +1144,7 @@ namespace Transports 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; } } @@ -1295,7 +1294,7 @@ namespace Transports 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 { @@ -1329,9 +1328,14 @@ namespace Transports { while (!stopping()) { - // Modem. + // Wait for modem configuration + if (!m_config_status) + continue; + + // Check for incoming data. processInput(); + // Check modem connection if (!hasConnection()) setState(STA_ERR_COM); } From cdf9ee9469e21cb26ea1095d1df5e513a54cc6d5 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 8 Mar 2023 10:31:17 +0000 Subject: [PATCH 04/35] Transports/Seatrac: Use NodeMap class. --- src/Transports/Seatrac/Task.cpp | 75 +++++++-------------------------- 1 file changed, 16 insertions(+), 59 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 50c94d713c..fe98e45872 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -103,11 +103,6 @@ namespace Transports uint8_t ack_timeout_time_multiplier; }; - //! 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. @@ -142,10 +137,8 @@ namespace Transports Time::Counter m_oway_timer; //! 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. @@ -179,7 +172,9 @@ namespace Transports m_preamble(false), m_stop_comms(false), m_usbl_receiver(false), - m_tstamp(0), + m_addr(0), + m_tstamp(0), + m_ticket(nullptr) { // Define configuration parameters. @@ -424,22 +419,12 @@ namespace Transports void onResourceInitialization(void) { - // Process modem addresses. - std::string agent = getSystemName(); - std::vector addrs = m_ctx.config.options(m_args.addr_section); - - // 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_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")); - 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"), agent.c_str())); + throw std::runtime_error(String::str(DTR("modem address for agent '%s' is invalid"), getSystemName())); try { @@ -803,7 +788,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 (...) { @@ -813,7 +798,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 (...) { @@ -838,7 +823,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 (...) { } @@ -1072,16 +1057,14 @@ 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()) @@ -1189,32 +1172,6 @@ namespace Transports m_ticket = new Ticket(ticket); } - //! Lookup system address. - //! @param[in] name system name - //! @return system address. - unsigned - lookupSystemAddress(const std::string& name) - { - MapName::iterator itr = m_modem_names.find(name); - if (itr == m_modem_names.end()) - throw std::runtime_error("unknown system name"); - - return itr->second; - } - - //! 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"); - - return itr->second; - } - //! Check for incoming data. //! @param[in] timeout timeout time. void From e1d52c1fdeff99972b60dbaa2c4f897387cc6c5e Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 8 Mar 2023 14:39:15 +0000 Subject: [PATCH 05/35] Transports/Seatrac: Making honour medium options more streamlined. --- src/Transports/Seatrac/Task.cpp | 70 ++++++++++++++++----------------- 1 file changed, 35 insertions(+), 35 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index fe98e45872..0db4e1fec8 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -77,8 +77,8 @@ namespace Transports std::string uart_dev; //! Serial port baud rate. unsigned uart_baud; - //! Transmit only underwater. - bool only_underwater; + //! Transmit only when medium is suitable. + bool honour_medium; //! Addresses Number - modem std::string addr_section; //! Enable AHRS mode @@ -86,7 +86,7 @@ namespace Transports //! 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. @@ -117,8 +117,6 @@ namespace Transports EntityStates m_state_entity; //! 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. @@ -141,6 +139,8 @@ namespace Transports NodeMap m_node_map; //! Current transmission ticket. Ticket* m_ticket; + //! Vehicle Medium. + IMC::VehicleMedium m_medium; //! Save modem commands. IMC::DevDataText m_dev_data; //! Euler angles message. @@ -170,7 +170,6 @@ namespace Transports m_handle(nullptr), m_config_status(false), m_preamble(false), - m_stop_comms(false), m_usbl_receiver(false), m_addr(0), m_tstamp(0), @@ -190,9 +189,9 @@ namespace Transports .defaultValue("19200") .description("Serial port baud rate"); - param("Transmit Only Underwater", m_args.only_underwater) + param("Honour Medium", m_args.honour_medium) .defaultValue("false") - .description("Do not transmit when at water surface"); + .description("Set to true to transmit only when the medium is suitable"); param("Address Section", m_args.addr_section) .defaultValue("Seatrac Addresses") @@ -204,11 +203,11 @@ namespace Transports param("Pressure Sensor Mode", m_args.pressure_sensor_mode) .defaultValue("false") - .description("Enable 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") @@ -258,6 +257,9 @@ 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); } @@ -398,9 +400,6 @@ namespace Transports setState(STA_BOOT); try { - if (m_args.only_underwater) - m_stop_comms = true; - if (openSocket()) return; @@ -666,15 +665,23 @@ namespace Transports processInput(delay_aft); } - //! 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; + } + + return restricted; } //! Send command if the modem has conditions to operate. @@ -682,9 +689,9 @@ 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; @@ -697,9 +704,8 @@ namespace Transports 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")); + trace(DTR("Sent command to the acoustic modem: %s"), cmd.c_str()); m_dev_data.value.assign(sanitize(cmd)); dispatch(m_dev_data); } @@ -996,10 +1002,10 @@ namespace Transports 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_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; //int16_t//m_channel_readout; - m_sspeed.value = ((fp32_t) (m_data_beacon.cid_status_msg.environment_vos)) / 10.0; //uint16_t + 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); @@ -1198,13 +1204,7 @@ namespace Transports void consume(const IMC::VehicleMedium* msg) { - if (m_args.only_underwater) - { - m_stop_comms = !(msg->medium == IMC::VehicleMedium::VM_UNDERWATER); - return; - } - - m_stop_comms = false; + m_medium = *msg; } void From fff9c7907fec524594fc577257bc8bf51c2381bb Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Thu, 9 Mar 2023 10:07:36 +0000 Subject: [PATCH 06/35] Transports/Seatrac: Extract driver. --- src/Transports/Seatrac/DataTypes.hpp | 11 + src/Transports/Seatrac/DebugMsg.hpp | 1 + src/Transports/Seatrac/Driver.hpp | 375 +++++++++++++++++++++++++++ src/Transports/Seatrac/Parser.hpp | 2 +- src/Transports/Seatrac/Task.cpp | 360 ++++--------------------- 5 files changed, 436 insertions(+), 313 deletions(-) create mode 100644 src/Transports/Seatrac/Driver.hpp diff --git a/src/Transports/Seatrac/DataTypes.hpp b/src/Transports/Seatrac/DataTypes.hpp index ee4d051fca..e40c85a130 100644 --- a/src/Transports/Seatrac/DataTypes.hpp +++ b/src/Transports/Seatrac/DataTypes.hpp @@ -54,6 +54,17 @@ namespace Transports //! Maximum buffer size. static const int c_bfr_size = 256; + //! 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 { diff --git a/src/Transports/Seatrac/DebugMsg.hpp b/src/Transports/Seatrac/DebugMsg.hpp index 0ac7face7d..398fed32d2 100644 --- a/src/Transports/Seatrac/DebugMsg.hpp +++ b/src/Transports/Seatrac/DebugMsg.hpp @@ -40,6 +40,7 @@ // Local headers #include "MsgTypes.hpp" +#include "Parser.hpp" namespace Transports { diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp new file mode 100644 index 0000000000..6a7f74f6ed --- /dev/null +++ b/src/Transports/Seatrac/Driver.hpp @@ -0,0 +1,375 @@ +//*************************************************************************** +// Copyright 2023 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: Maria Costa * +//*************************************************************************** + +#ifndef TRANSPORTS_SEATRAC_DRIVER_HPP_INCLUDED_ +#define TRANSPORTS_SEATRAC_DRIVER_HPP_INCLUDED_ + +// ISO C++ 98 headers. +#include + +// DUNE headers. +#include + +// Local headers. +#include "DataTypes.hpp" +#include "DebugMsg.hpp" +#include "MsgTypes.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(); + } + + //! Destructor. + ~Driver() + { } + + //! Seatrac Configuration + //! @param[in] ahrs_mode true if AHRS mode enabled. + //! @param[out] usbl_receiver true if USBL modem. + //! @param[out] max_range maximum range. + //! @return entity state after modem configuration. + EntityStates + configure(bool& usbl_receiver, bool ahrs_mode, uint16_t& max_range) + { + // 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) + xcvr_flags |= USBL_USE_AHRS_FLAG | 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)) + { + // 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; + + 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; + } + + // Saving settings + m_task->inf("Saving settings to modem"); + sendCommandAndWait(createCommand(CID_SETTINGS_SET, m_data_beacon), 2); + 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)) + { + state = STA_ERR_STP; + m_task->war(DTR("Failed to configure device")); + } + + // Configuration complete + m_task->inf("Modem ready."); + state = EntityStates::STA_IDLE; + } + else + { + // Configuration complete + m_task->inf("Modem ready (settings successfully set)."); + 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]; + uint16_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); + 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->trace(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 + //! @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) + { + 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) + && 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/Parser.hpp b/src/Transports/Seatrac/Parser.hpp index 70ee0791ef..1529c28fa1 100644 --- a/src/Transports/Seatrac/Parser.hpp +++ b/src/Transports/Seatrac/Parser.hpp @@ -185,7 +185,7 @@ namespace Transports //! Extract to DataSeatrac data structure. //! @param[in] message_type type of message to decode. //! @param[in] msg_raw raw message received by uart - //! @param[out] data_Beacon pointer where the data is stored. + //! @param[out] data_Beacon pointer to where the data is stored. void dataParser(uint16_t message_type, const char* msg_raw, DataSeatrac& data_Beacon) { diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 0db4e1fec8..4c6d5fd176 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -39,6 +39,7 @@ #include // Local headers +#include "Driver.hpp" #include "Parser.hpp" #include "MsgTypes.hpp" #include "DataTypes.hpp" @@ -59,17 +60,6 @@ namespace Transports //! 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 { @@ -97,7 +87,7 @@ namespace Transports 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; @@ -107,6 +97,8 @@ namespace Transports { //! Serial port handle. IO::Handle* m_handle; + //! Driver. + Driver* m_driver; //! Task arguments. Arguments m_args; //! Config Status. @@ -121,13 +113,9 @@ namespace Transports 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; @@ -141,8 +129,6 @@ namespace Transports Ticket* m_ticket; //! Vehicle Medium. IMC::VehicleMedium m_medium; - //! Save modem commands. - IMC::DevDataText m_dev_data; //! Euler angles message. IMC::EulerAngles m_euler; //! Acceleration message. @@ -168,12 +154,12 @@ namespace Transports Task(const std::string& name, Tasks::Context& ctx) : DUNE::Tasks::Task(name, ctx), m_handle(nullptr), + m_driver(nullptr), m_config_status(false), m_preamble(false), m_usbl_receiver(false), m_addr(0), m_tstamp(0), - m_ticket(nullptr) { // Define configuration parameters. @@ -274,108 +260,6 @@ namespace Transports m_states[m_state_entity].description); } - //! Check validity of received sentence (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 - war("Received message not valid: %s", DTR(Status::getString(Status::CODE_INVALID_CHECKSUM))); - } - else - war(DTR("Received message not valid: insufficient message length")); - return msg_validity; - } - - //! Process new data. - void - processNewData(void) - { - if(m_config_status) - { - 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.ahrs_mode) - handleAhrsData(); - if(m_args.pressure_sensor_mode) - 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 type = 0; - const char* msg_raw; - - if (!Poll::poll(*m_handle, 0.001)) - return; - - size_t 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_preamble) - { - if (checkValidity()) - { - msg_raw = m_datahex.data(); - std::memcpy(&type, msg_raw, 1); - dataParser(type, msg_raw + 1, m_data_beacon); - processNewData(); - printDebugFunction(type, m_data_beacon, this); - 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]); - } - } - } - //! Open TCP socket. //! @return true if socket was opened, false otherwise. bool @@ -425,9 +309,15 @@ namespace Transports if (m_addr < 1 || m_addr > 15) throw std::runtime_error(String::str(DTR("modem address for agent '%s' is invalid"), getSystemName())); + // Initialize driver + m_driver = new Driver(this, m_handle, m_data_beacon, m_tstamp, m_last_input, m_preamble, m_addr, + m_args.hard_iron[0], m_args.hard_iron[1], m_args.hard_iron[2]); + try { - configure(); + // Configure modem + setState(m_driver->configure(m_usbl_receiver, m_args.ahrs_mode, m_args.max_range)); + m_config_status = true; } catch (...) { @@ -437,113 +327,6 @@ namespace Transports } } - //! 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) - //! @return true if settings match, false otherwise - bool - checkSettings(StatusMode_E status_mode, uint8_t output_flags, uint8_t xcvr_flags, bool ahrs) - { - 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 == m_args.max_range) - && ahrs); - } - - //! Seatrac Configuration - void - configure(void) - { - // 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) - 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 ahrs = true; - if (m_args.ahrs_mode) - { - status_mode = STATUS_MODE_10HZ; - ahrs = isCalibrated(); - } - - // Verify modem settings - if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs)) - { - // 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 = m_args.max_range; - - if(!ahrs) - { - 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]; - } - - // Saving settings - inf("Saving settings to modem"); - sendCommandAndWait(createCommand(CID_SETTINGS_SET, m_data_beacon), 2); - sendCommandAndWait(createCommand(CID_SETTINGS_SAVE, m_data_beacon), 2); - 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)) - { - setState(STA_ERR_STP); - war(DTR("Failed to configure device")); - } - - // Configuration complete - inf("Modem ready."); - setState(STA_IDLE); - m_config_status = true; - } - else - { - // Configuration complete - inf("Modem ready (settings successfully set)."); - setState(STA_IDLE); - m_config_status = true; - } - - // Print system information - 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 ? "" : " Not"); - } - //! Update parameters. void onUpdateParameters(void) @@ -562,22 +345,7 @@ namespace Transports if (m_handle != nullptr) { if (paramChanged(m_args.hard_iron)) - runCalibration(); - } - } - - //! 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); - } + m_driver->runCalibration(); } } @@ -609,60 +377,19 @@ namespace Transports dispatch(sp); } - //! 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_args.hard_iron[0]*100)) - || ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_y*100)) != ( (int32_t) (m_args.hard_iron[1]*100)) - || ((int32_t) (m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z*100)) != ( (int32_t) (m_args.hard_iron[2]*100))) - { - inf(DTR("AHRS has different calibration parameters.")); - return false; - } - - inf("AHRS is calibrated (matching parameters)."); - return true; - } - - //! Set new hard-iron calibration parameters. - //! @return true if successful, false otherwise. - bool - setHardIron(void) - { - inf("Set new hard-iron calibration parameters and reset device."); - 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(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_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])) - return false; - - return true; - } //! Release resources. void onResourceRelease(void) { clearTicket(IMC::UamTxStatus::UTS_CANCELED); Memory::clear(m_handle); + Memory::clear(m_driver); } - //! 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 communications are restricted by medium. @@ -696,18 +423,7 @@ namespace Transports 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) - { - m_handle->writeString(cmd.c_str()); - trace(DTR("Sent command to the acoustic modem: %s"), cmd.c_str()); - m_dev_data.value.assign(sanitize(cmd)); - dispatch(m_dev_data); + m_driver->sendCommand(cmd); } //! Checks if the modem is working. @@ -914,9 +630,7 @@ namespace Transports } } else - { 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. @@ -1178,6 +892,31 @@ namespace Transports m_ticket = new Ticket(ticket); } + //! Process new data. + void + processNewData(void) + { + 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.ahrs_mode) + handleAhrsData(); + if(m_args.pressure_sensor_mode) + handlePressureSensor(); + + //TODO: send environment_supply + //m_data_beacon.cid_status_msg.environment_supply; //uint16_t + } + } + //! Check for incoming data. //! @param[in] timeout timeout time. void @@ -1187,7 +926,9 @@ namespace Transports do { consumeMessages(); - readSentence(); + m_driver->readSentence(); + //m_tstamp = Clock::getSinceEpoch() - m_last_input; + processNewData(); checkTxOWAY(); if (m_state_entity != STA_ERR_STP) @@ -1201,12 +942,6 @@ namespace Transports while (Clock::get() <= deadline); } - void - consume(const IMC::VehicleMedium* msg) - { - m_medium = *msg; - } - void resetOneWayTimer(void) { @@ -1291,6 +1026,7 @@ namespace Transports // Check for incoming data. processInput(); + waitForMessages(1.0); // Check modem connection if (!hasConnection()) From 9287b483a8215accc0cef544e967a61b0c07836d Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Thu, 9 Mar 2023 10:18:48 +0000 Subject: [PATCH 07/35] Transports/Seatrac: Changed acknowledge time multiplier parameter to a constant. --- src/Transports/Seatrac/Task.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 4c6d5fd176..936feef98e 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -59,6 +59,8 @@ namespace Transports 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; //! Task arguments. struct Arguments @@ -89,8 +91,6 @@ namespace Transports double calib_threshold; //! Maximum range. uint16_t max_range; - //! Timeout time multiplier for ack wait - uint8_t ack_timeout_time_multiplier; }; struct Task: public DUNE::Tasks::Task @@ -226,11 +226,6 @@ namespace Transports .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 (if ack requested)"); - // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); @@ -927,7 +922,6 @@ namespace Transports { consumeMessages(); m_driver->readSentence(); - //m_tstamp = Clock::getSinceEpoch() - m_last_input; processNewData(); checkTxOWAY(); @@ -948,7 +942,7 @@ namespace Transports 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 ); From 7d9376419602556d1a54ab997adebf284e20cbc1 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Thu, 9 Mar 2023 10:34:50 +0000 Subject: [PATCH 08/35] Transports/Seatrac: Clarified 'Max Range' parameter description. --- src/Transports/Seatrac/Task.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 936feef98e..4877a469c6 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -222,10 +222,11 @@ namespace Transports .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"); - + .defaultValue("1500") + .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."); + // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); From 858ef713a22455883ad6becc13d171bcd4000ba8 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Fri, 10 Mar 2023 08:42:41 +0000 Subject: [PATCH 09/35] Transports/Seatrac: Added missing MagneticField bind and other small corrections / additions to the AHRS usage. --- src/Transports/Seatrac/Driver.hpp | 6 +++--- src/Transports/Seatrac/Task.cpp | 34 ++++++++++++++++++++++--------- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index 6a7f74f6ed..3bda630db6 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -244,11 +244,11 @@ namespace Transports //! Seatrac data structure. DataSeatrac& m_data_beacon; //! Read timestamp. - double m_tstamp; + double& m_tstamp; //! Time of last received input. - double m_last_input; + double& m_last_input; //! c_preamble detected - bool m_preamble; + bool &m_preamble; //! Modem address. unsigned m_addr; //! Hard-iron calibration parameter (x). diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 4877a469c6..acc719445f 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -85,6 +85,8 @@ namespace Transports std::vector hard_iron; //! Enhanced usbl information will be requested. bool enhanced_usbl; + //! Log AHRS data. + bool log_ahrs; //! Rotation matrix values. std::vector rotation_mx; //! Calibration threshold. @@ -185,7 +187,7 @@ namespace Transports param("AHRS Mode", m_args.ahrs_mode) .defaultValue("false") - .description("Enable the AHRS information to be 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") @@ -205,6 +207,10 @@ namespace Transports "information in transmissions. This parameter is useful only when " "the beacon has an USBL receiver."); + param("Log AHRS Data", m_args.log_ahrs) + .defaultValue("false") + .description("Enable to log AHRS data when 'AHRS Mode' disabled."); + param("AHRS Rotation Matrix", m_args.rotation_mx) .defaultValue("") .size(9) @@ -226,7 +232,7 @@ namespace Transports .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."); - + // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); @@ -242,6 +248,7 @@ namespace Transports // Initialize medium. m_medium.medium = IMC::VehicleMedium::VM_UNKNOWN; + bind(this); bind(this); bind(this); } @@ -307,7 +314,7 @@ namespace Transports // Initialize driver m_driver = new Driver(this, m_handle, m_data_beacon, m_tstamp, m_last_input, m_preamble, m_addr, - m_args.hard_iron[0], m_args.hard_iron[1], m_args.hard_iron[2]); + m_hard_iron[0], m_hard_iron[1], m_hard_iron[2]); try { @@ -340,7 +347,7 @@ namespace Transports if (m_handle != nullptr) { - if (paramChanged(m_args.hard_iron)) + if (paramChanged(m_args.hard_iron) && m_args.ahrs_mode) m_driver->runCalibration(); } } @@ -348,6 +355,9 @@ namespace Transports void consume(const IMC::MagneticField* msg) { + if(!m_args.ahrs_mode) + return; + if (msg->getDestinationEntity() != getEntityId()) return; @@ -684,7 +694,7 @@ 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; - + //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; @@ -693,12 +703,13 @@ namespace Transports 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); @@ -747,9 +758,8 @@ namespace Transports { debug(DTR("Received UamTxFrame with dst=0x%04X. Msg for system '%s'"), msg->getDestination(), msg->sys_dst.c_str()); - std::string hex = String::toHex(msg->data); - std::vector data_t; - std::copy(hex.begin(), hex.end(), std::back_inserter(data_t)); + if (m_state_entity != STA_ACTIVE) + return; if (msg->getDestination() != getSystemId()) return; @@ -757,6 +767,10 @@ 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; ticket.imc_sid = msg->getSource(); @@ -903,7 +917,7 @@ namespace Transports if(m_data_beacon.newDataAvailable(CID_STATUS)) { - if(m_args.ahrs_mode) + if(m_args.ahrs_mode || m_args.log_ahrs) handleAhrsData(); if(m_args.pressure_sensor_mode) handlePressureSensor(); From 4619ad9db5731e24e4f8df0e6edc646e76e11cc4 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Mon, 20 Mar 2023 18:19:53 +0000 Subject: [PATCH 10/35] Transports/Seatrac: When modem is busy queue message to send, thus piggybacking on auto-reply (fix for iUSBL implementation). --- src/Transports/Seatrac/DebugMsg.hpp | 1 - src/Transports/Seatrac/Driver.hpp | 4 +- src/Transports/Seatrac/Parser.hpp | 27 ++++++++----- src/Transports/Seatrac/Task.cpp | 61 +++++++++++++++++------------ 4 files changed, 55 insertions(+), 38 deletions(-) diff --git a/src/Transports/Seatrac/DebugMsg.hpp b/src/Transports/Seatrac/DebugMsg.hpp index 398fed32d2..f246f4fab1 100644 --- a/src/Transports/Seatrac/DebugMsg.hpp +++ b/src/Transports/Seatrac/DebugMsg.hpp @@ -287,7 +287,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); diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index 3bda630db6..a97ca98e26 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -182,7 +182,7 @@ namespace Transports { // Initialize received message parser char bfr[c_bfr_size]; - uint16_t type = 0; + uint8_t type = 0; const char* msg_raw; if (!Poll::poll(*m_handle, 0.001)) @@ -248,7 +248,7 @@ namespace Transports //! Time of last received input. double& m_last_input; //! c_preamble detected - bool &m_preamble; + bool& m_preamble; //! Modem address. unsigned m_addr; //! Hard-iron calibration parameter (x). diff --git a/src/Transports/Seatrac/Parser.hpp b/src/Transports/Seatrac/Parser.hpp index 1529c28fa1..2e048dea89 100644 --- a/src/Transports/Seatrac/Parser.hpp +++ b/src/Transports/Seatrac/Parser.hpp @@ -130,7 +130,7 @@ namespace Transports //! @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); @@ -187,7 +187,7 @@ namespace Transports //! @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 +276,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 +303,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,18 +373,18 @@ 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_query_resp_msg.aco_fix, ind, msg_raw); + 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_query_resp_msg.queryFlagsExtract(); @@ -431,7 +431,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 +451,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); @@ -549,6 +549,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); diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index acc719445f..8008bb6760 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -473,6 +473,9 @@ 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 != nullptr) { @@ -485,30 +488,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("collecting 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); + debug("new data"); } + + if (data_rec_flag == -1) + war(DTR("wrong message order")); + if (data_rec_flag == 0) + debug("collecting data"); + if(data_rec_flag == -2) + debug("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()]); @@ -539,7 +546,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. @@ -742,7 +750,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) { @@ -750,6 +758,11 @@ 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)); + } } } @@ -758,9 +771,6 @@ namespace Transports { debug(DTR("Received UamTxFrame with dst=0x%04X. Msg for system '%s'"), msg->getDestination(), msg->sys_dst.c_str()); - if (m_state_entity != STA_ACTIVE) - return; - if (msg->getDestination() != getSystemId()) return; @@ -800,14 +810,14 @@ namespace Transports 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); + debug(DTR("Sending UamTxStatus::UTS_IP. Ticket %d being processed"), ticket.seq); // Fill the message type. if ((ticket.addr != 0) && ticket.ack) @@ -851,7 +861,7 @@ namespace Transports 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"), + 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, @@ -958,6 +968,7 @@ 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)) 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 ); From 55629297251455b010b38fe97a493a6966da75a6 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Tue, 21 Mar 2023 11:35:58 +0000 Subject: [PATCH 11/35] Transports/Seatrac: Reinstated 'Transmit Only Underwater' parameter (lauv-arlhawaii has modem installed upright). --- src/Transports/Seatrac/Task.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 8008bb6760..855091b91a 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -71,6 +71,8 @@ namespace Transports 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 AHRS mode @@ -181,6 +183,10 @@ namespace Transports .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 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"); @@ -412,6 +418,8 @@ namespace Transports 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; From 28427980d85befacf6239b0ce5d4e5d81ab89204 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Tue, 21 Mar 2023 12:25:00 +0000 Subject: [PATCH 12/35] Transports/Seatrac: Renamed DataTypes.hpp as Constants.hpp and slightly reduced verbosity of debug output. --- .../Seatrac/{DataTypes.hpp => Constants.hpp} | 18 +++++++-- src/Transports/Seatrac/DebugMsg.hpp | 1 - src/Transports/Seatrac/Driver.hpp | 2 - src/Transports/Seatrac/MsgTypes.hpp | 3 +- src/Transports/Seatrac/Task.cpp | 39 +++++++------------ 5 files changed, 30 insertions(+), 33 deletions(-) rename src/Transports/Seatrac/{DataTypes.hpp => Constants.hpp} (95%) diff --git a/src/Transports/Seatrac/DataTypes.hpp b/src/Transports/Seatrac/Constants.hpp similarity index 95% rename from src/Transports/Seatrac/DataTypes.hpp rename to src/Transports/Seatrac/Constants.hpp index e40c85a130..81d1ebf0aa 100644 --- a/src/Transports/Seatrac/DataTypes.hpp +++ b/src/Transports/Seatrac/Constants.hpp @@ -28,8 +28,8 @@ // Author: Raúl Sáez * //*************************************************************************** -#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,12 +39,25 @@ //! 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; //! The bitrate of acoustic communication (bits/second). @@ -148,7 +161,6 @@ namespace Transports BT_X150 = 0x31B }; - // Status Output Mode enum StatusMode_E { diff --git a/src/Transports/Seatrac/DebugMsg.hpp b/src/Transports/Seatrac/DebugMsg.hpp index f246f4fab1..141f2e330e 100644 --- a/src/Transports/Seatrac/DebugMsg.hpp +++ b/src/Transports/Seatrac/DebugMsg.hpp @@ -39,7 +39,6 @@ #include // Local headers -#include "MsgTypes.hpp" #include "Parser.hpp" namespace Transports diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index a97ca98e26..b10e03bf93 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -17,9 +17,7 @@ #include // Local headers. -#include "DataTypes.hpp" #include "DebugMsg.hpp" -#include "MsgTypes.hpp" #include "Parser.hpp" namespace Transports diff --git a/src/Transports/Seatrac/MsgTypes.hpp b/src/Transports/Seatrac/MsgTypes.hpp index 7e4397ea19..63e89451e1 100644 --- a/src/Transports/Seatrac/MsgTypes.hpp +++ b/src/Transports/Seatrac/MsgTypes.hpp @@ -40,7 +40,7 @@ #include // Local headers -#include "DataTypes.hpp" +#include "Constants.hpp" namespace Transports { @@ -107,7 +107,6 @@ namespace Transports outputflags_list[4] = (AHRS_RAW_DATA_FLAG & output_flags); outputflags_list[5] = (AHRS_COMP_DATA_FLAG & output_flags); } - }; struct CidPingRespMsg diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 855091b91a..77fc4265a1 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -40,10 +40,6 @@ // Local headers #include "Driver.hpp" -#include "Parser.hpp" -#include "MsgTypes.hpp" -#include "DataTypes.hpp" -#include "DebugMsg.hpp" namespace Transports { @@ -55,13 +51,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; - //! Acknowledged timeout time multiplier - static const uint8_t c_ack_timeout_multiplier = 6; - //! Task arguments. struct Arguments { @@ -457,7 +446,7 @@ namespace Transports // if msg has more than 1 packet, send next part 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, @@ -467,7 +456,7 @@ namespace Transports 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 == nullptr ? -1 : m_ticket->seq, @@ -487,7 +476,7 @@ namespace Transports // Data communication done 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); @@ -508,15 +497,15 @@ namespace Transports std::string msg; m_data_beacon.cid_dat_receive_msg.getFullMsg(msg); handleRxMessage(msg, acoustic); - debug("new data"); + trace("new data"); } if (data_rec_flag == -1) war(DTR("wrong message order")); if (data_rec_flag == 0) - debug("collecting data"); + trace("collecting data"); if(data_rec_flag == -2) - debug("no data size"); + trace("no data size"); } //! Publish received acoustic message. @@ -796,7 +785,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()) { @@ -825,7 +814,7 @@ namespace Transports // 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) @@ -841,7 +830,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 { @@ -849,7 +838,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; @@ -868,7 +857,7 @@ namespace Transports break; default: resetOneWayTimer(); - debug(DTR("Sending package %f s"), m_oway_timer.getTop()); + 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, @@ -999,7 +988,7 @@ 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 == nullptr ? -1 : m_ticket->seq, @@ -1008,7 +997,7 @@ namespace Transports 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 == nullptr ? -1 : m_ticket->seq, @@ -1018,7 +1007,7 @@ namespace Transports } else { - debug(DTR("Msg transmission complete for ticket %d (in %f s)"), + 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); From b6cbe91892577a2ef06123013523d6b4ae2c1b99 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 22 Mar 2023 10:10:53 +0000 Subject: [PATCH 13/35] Transports/Seatrac: Entity state will only be idle if communications restricted, otherwise active. --- src/Transports/Seatrac/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 77fc4265a1..be2034a564 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -949,7 +949,7 @@ namespace Transports if (m_state_entity != STA_ERR_STP) { - if (isActive()) + if (!isRestricted()) setState(STA_ACTIVE); else setState(STA_IDLE); From 07043234e274e61fd3a18d85b64f12e30e8cad8a Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Fri, 26 May 2023 16:28:38 +0100 Subject: [PATCH 14/35] Config: etc/hardware: Updated default seatrac configurations for lctr-a6xx and lctr-a9xx systems. --- etc/hardware/acoustic-modems/seatrac_uart.ini | 3 +++ etc/hardware/lctr-a6xx/seatrac.ini | 9 ++++++--- 2 files changed, 9 insertions(+), 3 deletions(-) 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..03ee57d220 100644 --- a/etc/hardware/lctr-a6xx/seatrac.ini +++ b/etc/hardware/lctr-a6xx/seatrac.ini @@ -35,19 +35,22 @@ Debug Level = None 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 +Log AHRS Data = false 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] From 7bfa81e414574a2c02a6be056de8dc58a977c0fd Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Mon, 27 Mar 2023 18:55:22 +0100 Subject: [PATCH 15/35] Transports/Seatrac: Updated copyright. --- src/Transports/Seatrac/Constants.hpp | 3 +++ src/Transports/Seatrac/DebugMsg.hpp | 3 +++ src/Transports/Seatrac/Driver.hpp | 27 ++++++++++++++++++++++++--- src/Transports/Seatrac/MsgTypes.hpp | 3 +++ src/Transports/Seatrac/Parser.hpp | 3 +++ src/Transports/Seatrac/Task.cpp | 3 +++ 6 files changed, 39 insertions(+), 3 deletions(-) diff --git a/src/Transports/Seatrac/Constants.hpp b/src/Transports/Seatrac/Constants.hpp index 81d1ebf0aa..dcaf67a908 100644 --- a/src/Transports/Seatrac/Constants.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,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_CONSTANTS_HPP_INCLUDED_ diff --git a/src/Transports/Seatrac/DebugMsg.hpp b/src/Transports/Seatrac/DebugMsg.hpp index 141f2e330e..fad1cec867 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_ diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index b10e03bf93..9eecb67e64 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -1,10 +1,31 @@ //*************************************************************************** +// 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 subject to the terms and conditions defined in file * -// 'LICENCE.md', which is part of this source code package. * +// 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 * +// Author: Maria Costa (small refactor and fix for iUSBL implementation) * //*************************************************************************** #ifndef TRANSPORTS_SEATRAC_DRIVER_HPP_INCLUDED_ diff --git a/src/Transports/Seatrac/MsgTypes.hpp b/src/Transports/Seatrac/MsgTypes.hpp index 63e89451e1..9421d5e3c5 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_ diff --git a/src/Transports/Seatrac/Parser.hpp b/src/Transports/Seatrac/Parser.hpp index 2e048dea89..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_ diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index be2034a564..a00a545922 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. From 10205163887ec48257699be510b8877d215541e1 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Tue, 28 Mar 2023 16:18:42 +0100 Subject: [PATCH 16/35] Transports/Seatrac: Added parameter to set transponder's turn around time. --- etc/hardware/lctr-a6xx/seatrac.ini | 1 + src/Transports/Seatrac/Driver.hpp | 14 +++++++++----- src/Transports/Seatrac/Task.cpp | 22 ++++++++++++++++------ 3 files changed, 26 insertions(+), 11 deletions(-) diff --git a/etc/hardware/lctr-a6xx/seatrac.ini b/etc/hardware/lctr-a6xx/seatrac.ini index 03ee57d220..cb671789fd 100644 --- a/etc/hardware/lctr-a6xx/seatrac.ini +++ b/etc/hardware/lctr-a6xx/seatrac.ini @@ -41,6 +41,7 @@ Transmit Only Underwater = true Address Section = Seatrac Addresses Honour Medium = true Max Range = 1000 +Turn Around Time = 10 Log AHRS Data = false Pressure Sensor Mode = true Use Internal Pressure Sensor as Medium = false diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index 9eecb67e64..027b1fdba8 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -87,11 +87,12 @@ namespace Transports //! 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[out] usbl_receiver true if USBL modem. - //! @param[out] max_range maximum range. //! @return entity state after modem configuration. EntityStates - configure(bool& usbl_receiver, bool ahrs_mode, uint16_t& max_range) + configure(bool& usbl_receiver, bool ahrs_mode, uint16_t max_range, uint16_t turn_around_time) { // Retrieve current settings and system information sendCommandAndWait(createCommand(CID_SETTINGS_GET, m_data_beacon), 1); @@ -119,7 +120,7 @@ namespace Transports } // Verify modem settings - if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs, max_range)) + if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs, max_range, turn_around_time)) { // Setting correct settings m_data_beacon.cid_settings_msg.status_flags = status_mode; @@ -127,6 +128,7 @@ namespace Transports 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; if(!ahrs) { @@ -144,7 +146,7 @@ namespace Transports sendCommandAndWait(createCommand(CID_SETTINGS_GET, m_data_beacon), 2); // Check modem settings again - if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs, max_range)) + if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs, max_range, turn_around_time)) { state = STA_ERR_STP; m_task->war(DTR("Failed to configure device")); @@ -289,15 +291,17 @@ namespace Transports //! @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 //! @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) + checkSettings(StatusMode_E status_mode, uint8_t output_flags, uint8_t xcvr_flags, bool ahrs, uint16_t max_range, uint16_t turn_around_time) { 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) && ahrs); } diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index a00a545922..dd56e09a6a 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -87,6 +87,8 @@ namespace Transports double calib_threshold; //! Maximum range. uint16_t max_range; + //! Transponder turn around time. + uint16_t turn_around_time; }; struct Task: public DUNE::Tasks::Task @@ -133,13 +135,13 @@ namespace Transports IMC::AngularVelocity m_agvel; //! Magnetometer Vector message. IMC::MagneticField m_magfield; - //! Current sound speed. + //! 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; @@ -226,11 +228,19 @@ namespace Transports .description("Minimum magnetic field calibration values to reset hard-iron parameters"); param("Max Range", m_args.max_range) - .defaultValue("1500") + .defaultValue("1000") .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."); + // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); @@ -317,7 +327,7 @@ namespace Transports try { // Configure modem - setState(m_driver->configure(m_usbl_receiver, m_args.ahrs_mode, m_args.max_range)); + setState(m_driver->configure(m_usbl_receiver, m_args.ahrs_mode, m_args.max_range, m_args.turn_around_time)); m_config_status = true; } catch (...) From 95e65af5d5942e73c3d6c619d5db00f08eae5f08 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Tue, 28 Mar 2023 19:38:59 +0100 Subject: [PATCH 17/35] Transports/Seatrac: SoundSpeed is now manually set on transponder (using default value or from a specified sensor). --- src/Transports/Seatrac/Constants.hpp | 2 + src/Transports/Seatrac/Driver.hpp | 14 +++--- src/Transports/Seatrac/Task.cpp | 66 ++++++++++++++++++++++++---- 3 files changed, 69 insertions(+), 13 deletions(-) diff --git a/src/Transports/Seatrac/Constants.hpp b/src/Transports/Seatrac/Constants.hpp index dcaf67a908..3fb90e1858 100644 --- a/src/Transports/Seatrac/Constants.hpp +++ b/src/Transports/Seatrac/Constants.hpp @@ -69,6 +69,8 @@ namespace Transports 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 diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index 027b1fdba8..c9e195c398 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -89,10 +89,11 @@ namespace Transports //! @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) + 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); @@ -120,7 +121,7 @@ namespace Transports } // Verify modem settings - if (!checkSettings(status_mode, output_flags, xcvr_flags, ahrs, max_range, turn_around_time)) + 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; @@ -129,6 +130,7 @@ namespace Transports 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) { @@ -146,7 +148,7 @@ namespace Transports 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)) + 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")); @@ -252,7 +254,7 @@ namespace Transports sendCommand(const std::string& cmd) { m_handle->writeString(cmd.c_str()); - m_task->trace(DTR("Sent command to the acoustic modem: %s"), 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); } @@ -292,9 +294,10 @@ namespace Transports //! @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) + 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) @@ -302,6 +305,7 @@ namespace Transports && (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) && ahrs); } diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index dd56e09a6a..54a3e64272 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -89,6 +89,10 @@ namespace Transports uint16_t max_range; //! 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; }; struct Task: public DUNE::Tasks::Task @@ -135,6 +139,8 @@ namespace Transports IMC::AngularVelocity m_agvel; //! Magnetometer Vector message. IMC::MagneticField m_magfield; + //! Sound speed entity id. + int m_sound_speed_eid; //! Modem sound speed. IMC::SoundSpeed m_sspeed; //! Modem depth. @@ -158,7 +164,8 @@ namespace Transports m_usbl_receiver(false), m_addr(0), m_tstamp(0), - m_ticket(nullptr) + m_ticket(nullptr), + m_sound_speed_eid(-1) { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_MANEUVER, @@ -241,6 +248,14 @@ namespace Transports .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"); + // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); @@ -257,6 +272,7 @@ namespace Transports m_medium.medium = IMC::VehicleMedium::VM_UNKNOWN; bind(this); + bind(this); bind(this); bind(this); } @@ -288,6 +304,19 @@ 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) @@ -327,7 +356,7 @@ namespace Transports try { // Configure modem - setState(m_driver->configure(m_usbl_receiver, m_args.ahrs_mode, m_args.max_range, m_args.turn_around_time)); + 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; } catch (...) @@ -360,6 +389,15 @@ namespace Transports } } + //! Release resources. + void + onResourceRelease(void) + { + clearTicket(IMC::UamTxStatus::UTS_CANCELED); + Memory::clear(m_handle); + Memory::clear(m_driver); + } + void consume(const IMC::MagneticField* msg) { @@ -391,13 +429,25 @@ namespace Transports dispatch(sp); } - //! Release resources. void - onResourceRelease(void) + consume(const IMC::SoundSpeed* msg) { - clearTicket(IMC::UamTxStatus::UTS_CANCELED); - Memory::clear(m_handle); - Memory::clear(m_driver); + if (m_state_entity != STA_ACTIVE) + return; + + if ((int)msg->getSourceEntity() != m_sound_speed_eid) + return; + + 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)); } void @@ -749,7 +799,7 @@ namespace Transports 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, From 716ca6d7e86681cb5755dd5c93486831b58878b6 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 29 Mar 2023 16:20:21 +0100 Subject: [PATCH 18/35] Transports/Seatrac: Only save new hard-iron calibration parameters to EEPROM (other settings will now only be set during runtime). --- src/Transports/Seatrac/Driver.hpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index c9e195c398..22176548a1 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -139,12 +139,16 @@ namespace Transports m_data_beacon.cid_settings_msg.ahrs_cal.mag_hard_z = m_hard_iron_z; } - // Saving settings - m_task->inf("Saving settings to modem"); + // Updating settings and saving if needed + m_task->inf("Sending updated settings to modem"); sendCommandAndWait(createCommand(CID_SETTINGS_SET, m_data_beacon), 2); - sendCommandAndWait(createCommand(CID_SETTINGS_SAVE, m_data_beacon), 2); - m_task->inf("Rebooting modem"); - sendCommandAndWait(createCommand(CID_SYS_REBOOT, m_data_beacon), 6); + 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 @@ -155,13 +159,13 @@ namespace Transports } // Configuration complete - m_task->inf("Modem ready."); + m_task->inf("Modem ready (settings successfully set)."); state = EntityStates::STA_IDLE; } else { // Configuration complete - m_task->inf("Modem ready (settings successfully set)."); + m_task->inf("Modem ready."); state = EntityStates::STA_IDLE; } From 59ebaaec9392655cd46c9bcf8f297815a8dfca54 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 29 Mar 2023 16:22:35 +0100 Subject: [PATCH 19/35] Transports/Seatrac: Fixed verification of velocity of sound setting. --- src/Transports/Seatrac/Driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index 22176548a1..578e467d81 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -309,7 +309,7 @@ namespace Transports && (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) + && (m_data_beacon.cid_settings_msg.env_vos == sspeed_def * 10) && ahrs); } From f78269af63e133520e8251d353a6acdda432e108 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 29 Mar 2023 21:22:01 +0100 Subject: [PATCH 20/35] Transports/Seatrac: Improved handling of communication errors. --- src/Transports/Seatrac/Constants.hpp | 4 ++-- src/Transports/Seatrac/Driver.hpp | 5 +++++ src/Transports/Seatrac/Task.cpp | 28 ++++++++++++++++++++++------ 3 files changed, 29 insertions(+), 8 deletions(-) diff --git a/src/Transports/Seatrac/Constants.hpp b/src/Transports/Seatrac/Constants.hpp index 3fb90e1858..7326cb154c 100644 --- a/src/Transports/Seatrac/Constants.hpp +++ b/src/Transports/Seatrac/Constants.hpp @@ -62,9 +62,9 @@ namespace Transports //! 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. diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index 578e467d81..7ba81d4206 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -79,6 +79,8 @@ namespace Transports //! @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. @@ -216,6 +218,9 @@ namespace Transports 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) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 54a3e64272..6628b5b8fd 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -349,9 +349,18 @@ namespace Transports if (m_addr < 1 || m_addr > 15) throw std::runtime_error(String::str(DTR("modem address for agent '%s' is invalid"), getSystemName())); - // 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]); + try + { + // 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]); + } + catch (std::runtime_error& e) + { + err("%s: %s", DTR(Status::getString(CODE_COM_ERROR)), e.what()); + setState(STA_ERR_COM); + throw std::runtime_error(m_states[m_state_entity].description); + } try { @@ -497,6 +506,10 @@ namespace Transports bool hasConnection(void) { + // Throw runtime error if connection problem persists + if (Clock::get() > (m_last_input + c_input_tout + 30)) + throw std::runtime_error(m_states[STA_ERR_COM].description); + return (Clock::get() < (m_last_input + c_input_tout)); } @@ -1010,7 +1023,7 @@ namespace Transports processNewData(); checkTxOWAY(); - if (m_state_entity != STA_ERR_STP) + if ((m_state_entity != STA_ERR_STP) && hasConnection()) { if (!isRestricted()) setState(STA_ACTIVE); @@ -1101,8 +1114,8 @@ namespace Transports while (!stopping()) { // Wait for modem configuration - if (!m_config_status) - continue; + if(!m_config_status) + break; // Check for incoming data. processInput(); @@ -1110,7 +1123,10 @@ namespace Transports // Check modem connection if (!hasConnection()) + { setState(STA_ERR_COM); + err("%s", DTR(Status::getString(CODE_COM_ERROR))); + } } } }; From 5748400bdab8d1253dd37f3556386f83fb303ed0 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 5 Apr 2023 09:30:08 +0100 Subject: [PATCH 21/35] Transports/Seatrac: Use attitude from system in transponder if 'AHRS mode' disabled. --- src/Transports/Seatrac/Constants.hpp | 5 ++- src/Transports/Seatrac/DebugMsg.hpp | 66 ++++++++++++++-------------- src/Transports/Seatrac/Driver.hpp | 7 ++- src/Transports/Seatrac/MsgTypes.hpp | 2 +- src/Transports/Seatrac/Task.cpp | 45 ++++++++++++++++--- 5 files changed, 83 insertions(+), 42 deletions(-) diff --git a/src/Transports/Seatrac/Constants.hpp b/src/Transports/Seatrac/Constants.hpp index 7326cb154c..b4e193e8d0 100644 --- a/src/Transports/Seatrac/Constants.hpp +++ b/src/Transports/Seatrac/Constants.hpp @@ -71,6 +71,8 @@ namespace Transports static const int c_bfr_size = 256; //! Sound speed update window (m/s). static const uint16_t c_sspeed_window = 1; + //! Time between attitude updates for transponder. + static const float c_att_interval = 1.0; //! Entity states. enum EntityStates @@ -367,7 +369,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) @@ -376,6 +378,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 fad1cec867..dda824dafd 100644 --- a/src/Transports/Seatrac/DebugMsg.hpp +++ b/src/Transports/Seatrac/DebugMsg.hpp @@ -65,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. @@ -96,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); } } @@ -139,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", diff --git a/src/Transports/Seatrac/Driver.hpp b/src/Transports/Seatrac/Driver.hpp index 7ba81d4206..e95d03b7f1 100644 --- a/src/Transports/Seatrac/Driver.hpp +++ b/src/Transports/Seatrac/Driver.hpp @@ -111,7 +111,12 @@ namespace Transports uint8_t xcvr_flags = XCVR_FIX_MSGS_FLAG | XCVR_POSFLT_ENABLE_FLAG; if (usbl_receiver) - xcvr_flags |= USBL_USE_AHRS_FLAG | XCVR_USBL_MSGS_FLAG; + { + 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; diff --git a/src/Transports/Seatrac/MsgTypes.hpp b/src/Transports/Seatrac/MsgTypes.hpp index 9421d5e3c5..cfbed74271 100644 --- a/src/Transports/Seatrac/MsgTypes.hpp +++ b/src/Transports/Seatrac/MsgTypes.hpp @@ -69,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; diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 6628b5b8fd..d65be44e36 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -151,6 +151,8 @@ namespace Transports 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. @@ -271,6 +273,7 @@ namespace Transports // Initialize medium. m_medium.medium = IMC::VehicleMedium::VM_UNKNOWN; + bind(this); bind(this); bind(this); bind(this); @@ -374,6 +377,8 @@ namespace Transports setState(STA_ERR_STP); throw std::runtime_error(m_states[m_state_entity].description); } + + m_timer.setTop(c_att_interval); } //! Update parameters. @@ -407,6 +412,34 @@ namespace Transports Memory::clear(m_driver); } + void + consume(const IMC::EulerAngles* msg) + { + if (m_args.ahrs_mode || !m_timer.overflow()) + 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) { @@ -667,6 +700,9 @@ namespace Transports usblPosition.n = aco_fix.position_northing / 10.0; usblPosition.d = aco_fix.position_depth / 10.0; + if (aco_fix.outputflags_list[4]) + debug("Position received with filter error flag set!"); + dispatch(usblPosition); } else // Mimics Evologic: Only if position is not computed, compute angles. @@ -776,10 +812,10 @@ namespace Transports 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; //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.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); @@ -1004,9 +1040,6 @@ namespace Transports handleAhrsData(); if(m_args.pressure_sensor_mode) handlePressureSensor(); - - //TODO: send environment_supply - //m_data_beacon.cid_status_msg.environment_supply; //uint16_t } } From 792f14a04ff5e57adbadd6c917abb03d7e0f8e3e Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 5 Apr 2023 09:40:10 +0100 Subject: [PATCH 22/35] Transports/Seatrac: Removed 'Log AHRS Data' parameter and changed scope of 'Active' parameter. --- etc/hardware/lctr-a6xx/seatrac.ini | 2 +- src/Transports/Seatrac/Task.cpp | 10 ++-------- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/etc/hardware/lctr-a6xx/seatrac.ini b/etc/hardware/lctr-a6xx/seatrac.ini index cb671789fd..b5c5ca932b 100644 --- a/etc/hardware/lctr-a6xx/seatrac.ini +++ b/etc/hardware/lctr-a6xx/seatrac.ini @@ -32,6 +32,7 @@ [Transports.Seatrac] Enabled = Hardware Debug Level = None +Active = true Entity Label = Acoustic Modem Serial Port - Device = /dev/uart/10 Serial Port - Baud Rate = 115200 @@ -42,7 +43,6 @@ Address Section = Seatrac Addresses Honour Medium = true Max Range = 1000 Turn Around Time = 10 -Log AHRS Data = false Pressure Sensor Mode = true Use Internal Pressure Sensor as Medium = false USBL Mode = true diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index d65be44e36..0b4425eeb6 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -79,8 +79,6 @@ namespace Transports std::vector hard_iron; //! Enhanced usbl information will be requested. bool enhanced_usbl; - //! Log AHRS data. - bool log_ahrs; //! Rotation matrix values. std::vector rotation_mx; //! Calibration threshold. @@ -170,7 +168,7 @@ namespace Transports 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) @@ -216,10 +214,6 @@ namespace Transports "information in transmissions. This parameter is useful only when " "the beacon has an USBL receiver."); - param("Log AHRS Data", m_args.log_ahrs) - .defaultValue("false") - .description("Enable to log AHRS data when 'AHRS Mode' disabled."); - param("AHRS Rotation Matrix", m_args.rotation_mx) .defaultValue("") .size(9) @@ -1036,7 +1030,7 @@ namespace Transports if(m_data_beacon.newDataAvailable(CID_STATUS)) { - if(m_args.ahrs_mode || m_args.log_ahrs) + if(m_args.ahrs_mode) handleAhrsData(); if(m_args.pressure_sensor_mode) handlePressureSensor(); From 178e3fa977a54f2af73775bd29a47863158157cd Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Thu, 6 Apr 2023 12:43:54 +0100 Subject: [PATCH 23/35] Transports/Seatrac: Incresead attitude sending rate. --- src/Transports/Seatrac/Constants.hpp | 2 +- src/Transports/Seatrac/DebugMsg.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Transports/Seatrac/Constants.hpp b/src/Transports/Seatrac/Constants.hpp index b4e193e8d0..6a0fc12645 100644 --- a/src/Transports/Seatrac/Constants.hpp +++ b/src/Transports/Seatrac/Constants.hpp @@ -72,7 +72,7 @@ namespace Transports //! Sound speed update window (m/s). static const uint16_t c_sspeed_window = 1; //! Time between attitude updates for transponder. - static const float c_att_interval = 1.0; + static const float c_att_interval = 0.05; //! Entity states. enum EntityStates diff --git a/src/Transports/Seatrac/DebugMsg.hpp b/src/Transports/Seatrac/DebugMsg.hpp index dda824dafd..581e62af97 100644 --- a/src/Transports/Seatrac/DebugMsg.hpp +++ b/src/Transports/Seatrac/DebugMsg.hpp @@ -420,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; From e5780b1166416ddee35d7fd24dc8d8ca4bac30dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Pinto?= Date: Mon, 10 Apr 2023 09:44:58 +0100 Subject: [PATCH 24/35] Transports/Seatrac: Calculate relative position of the other modem when USBL ranges are received. --- src/Transports/Seatrac/Task.cpp | 91 ++++++++++++++++++++++++++++++--- 1 file changed, 84 insertions(+), 7 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 0b4425eeb6..93c02c7ef9 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -56,7 +56,7 @@ namespace Transports //! Task arguments. struct Arguments - { + {\ //! Serial port device. std::string uart_dev; //! Serial port baud rate. @@ -91,6 +91,8 @@ namespace Transports double sound_speed_def; //! Entity label of sound speed provider. std::string sound_speed_elabel; + //! Calculate NED offsets + bool calc_ned_locally; }; struct Task: public DUNE::Tasks::Task @@ -151,6 +153,8 @@ namespace Transports Math::Matrix m_rotation; //! Timer. Time::Counter m_timer; + //! Last euler angles. + IMC::EulerAngles* m_last_angles; //! Constructor. //! @param[in] name task name. @@ -165,7 +169,8 @@ namespace Transports m_addr(0), m_tstamp(0), m_ticket(nullptr), - m_sound_speed_eid(-1) + m_sound_speed_eid(-1), + m_last_angles(nullptr) { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_GLOBAL, @@ -252,6 +257,10 @@ namespace Transports param("Sound Speed - Entity Label", m_args.sound_speed_elabel) .description("Entity label of sound speed provider"); + param("Calculate NED offsets locally", m_args.calc_ned_locally) + .defaultValue("true") + .description("Calculate NED offsets when USBL range is received."); + // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); @@ -409,9 +418,10 @@ namespace Transports void consume(const IMC::EulerAngles* msg) { + Memory::replace(m_last_angles, msg->clone()); + if (m_args.ahrs_mode || !m_timer.overflow()) return; - // Rotate attitude values to transponder reference frame. Math::Matrix data(3, 1); data(0) = msg->phi; @@ -650,6 +660,50 @@ namespace Transports handleAcousticInformation(m_data_beacon.cid_dat_receive_msg.aco_fix); } + // Convert AER to rotated XYZ position + //! @param[in] aer AER position. + //! @param[in] euler Euler angles. + //! @param[out] xyz XYZ position. + void + aer2xyz(double aer[3], double euler[3], double xyz[3]) + { + // Compute intermediate variables + double sinA = std::sin(aer[0]), cosA = std::cos(aer[0]); + double sinE = std::sin(aer[1]), cosE = std::cos(aer[1]); + + // Convert to local XYZ + xyz[0] = -aer[2]*sinE*cosA; // X + xyz[1] = aer[2]*sinE*sinA; // Y + xyz[2] = -aer[2]*cosE; // Z + + // Apply Euler rotation + double Rz[3][3] = {{std::cos(euler[0]), -std::sin(euler[0]), 0}, + {std::sin(euler[0]), std::cos(euler[0]), 0}, + {0, 0, 1}}; + double Ry[3][3] = {{std::cos(euler[1]), 0, std::sin(euler[1])}, + {0, 1, 0}, + {-std::sin(euler[1]), 0, std::cos(euler[1])}}; + double Rx[3][3] = {{1, 0, 0}, + {0, std::cos(euler[2]), -std::sin(euler[2])}, + {0, std::sin(euler[2]), std::cos(euler[2])}}; + double R[3][3] = {{0}}; + + // Compute overall rotation matrix + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + for (int k = 0; k < 3; k++) { + R[i][j] += Rz[i][k] * Ry[k][j] * Rx[j][k]; + } + } + } + + // Rotate XYZ vector + double x = xyz[0], y = xyz[1], z = xyz[2]; + xyz[0] = R[0][0]*x + R[0][1]*y + R[0][2]*z; + xyz[1] = R[1][0]*x + R[1][1]*y + R[1][2]*z; + xyz[2] = R[2][0]*x + R[2][1]*y + R[2][2]*z; + } + //! Handle acoustic information from received data. //! param[in] aco_fix Acoustic information field of the received message. void @@ -690,10 +744,33 @@ namespace Transports usblPosition.phi = Angles::radians(aco_fix.attitude_roll / 10.0); usblPosition.theta = Angles::radians(aco_fix.attitude_pitch / 10.0); usblPosition.psi = Angles::radians(aco_fix.attitude_yaw / 10.0); - usblPosition.e = aco_fix.position_easting / 10.0; - usblPosition.n = aco_fix.position_northing / 10.0; - usblPosition.d = aco_fix.position_depth / 10.0; - + + if (!m_args.calc_ned_locally) + { + usblPosition.e = aco_fix.position_easting / 10.0; + usblPosition.n = aco_fix.position_northing / 10.0; + usblPosition.d = aco_fix.position_depth / 10.0; + } + else // calculate the NED offsets locally + { + if (m_last_angles == nullptr) + { + war("Cannot calculate NED offsets internally because no euler angles have been received."); + return; + } + double xyz[3], aer[3], euler[3]; + aer[0] = Angles::radians(aco_fix.usbl_azimuth / 10.0); + aer[1] = Angles::radians(aco_fix.usbl_elevation / 10.0); + aer[2] = aco_fix.range_dist / 10.0; + euler[0] = m_last_angles->phi; + euler[1] = m_last_angles->theta; + euler[2] = m_last_angles->psi; + aer2xyz(aer, euler, xyz); + usblPosition.n = xyz[0]; + usblPosition.e = xyz[1]; + usblPosition.d = xyz[2]; + } + if (aco_fix.outputflags_list[4]) debug("Position received with filter error flag set!"); From 403a5f323a8bf727c8d314d15ba532a146776cc1 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Mon, 10 Apr 2023 10:45:29 +0100 Subject: [PATCH 25/35] Transports/Seatrac: Fixed typo. --- src/Transports/Seatrac/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 93c02c7ef9..10e33a872a 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -56,7 +56,7 @@ namespace Transports //! Task arguments. struct Arguments - {\ + { //! Serial port device. std::string uart_dev; //! Serial port baud rate. From 744e7de8e597b4ab0f1c7b4289b5b11901b7efdc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Pinto?= Date: Mon, 10 Apr 2023 14:34:58 +0100 Subject: [PATCH 26/35] Transports/Seatrac: Reformatted code. --- src/Transports/Seatrac/Task.cpp | 73 +++++++++++++++++---------------- 1 file changed, 38 insertions(+), 35 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 10e33a872a..57507a560a 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -663,45 +663,48 @@ namespace Transports // Convert AER to rotated XYZ position //! @param[in] aer AER position. //! @param[in] euler Euler angles. - //! @param[out] xyz XYZ position. - void + //! @param[out] xyz XYZ position. + void aer2xyz(double aer[3], double euler[3], double xyz[3]) { - // Compute intermediate variables - double sinA = std::sin(aer[0]), cosA = std::cos(aer[0]); - double sinE = std::sin(aer[1]), cosE = std::cos(aer[1]); - - // Convert to local XYZ - xyz[0] = -aer[2]*sinE*cosA; // X - xyz[1] = aer[2]*sinE*sinA; // Y - xyz[2] = -aer[2]*cosE; // Z - - // Apply Euler rotation - double Rz[3][3] = {{std::cos(euler[0]), -std::sin(euler[0]), 0}, - {std::sin(euler[0]), std::cos(euler[0]), 0}, - {0, 0, 1}}; - double Ry[3][3] = {{std::cos(euler[1]), 0, std::sin(euler[1])}, - {0, 1, 0}, - {-std::sin(euler[1]), 0, std::cos(euler[1])}}; - double Rx[3][3] = {{1, 0, 0}, - {0, std::cos(euler[2]), -std::sin(euler[2])}, - {0, std::sin(euler[2]), std::cos(euler[2])}}; - double R[3][3] = {{0}}; - - // Compute overall rotation matrix - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - for (int k = 0; k < 3; k++) { - R[i][j] += Rz[i][k] * Ry[k][j] * Rx[j][k]; - } - } + // Compute intermediate variables + double sinA = std::sin(aer[0]), cosA = std::cos(aer[0]); + double sinE = std::sin(aer[1]), cosE = std::cos(aer[1]); + + // Convert to local XYZ + xyz[0] = -aer[2] * sinE * cosA;// X + xyz[1] = aer[2] * sinE * sinA; // Y + xyz[2] = -aer[2] * cosE; // Z + + // Apply Euler rotation + double Rz[3][3] = { { std::cos(euler[0]), -std::sin(euler[0]), 0 }, + { std::sin(euler[0]), std::cos(euler[0]), 0 }, + { 0, 0, 1 } }; + double Ry[3][3] = { { std::cos(euler[1]), 0, std::sin(euler[1]) }, + { 0, 1, 0 }, + { -std::sin(euler[1]), 0, std::cos(euler[1]) } }; + double Rx[3][3] = { { 1, 0, 0 }, + { 0, std::cos(euler[2]), -std::sin(euler[2]) }, + { 0, std::sin(euler[2]), std::cos(euler[2]) } }; + double R[3][3] = { { 0 } }; + + // Compute overall rotation matrix + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + for (int k = 0; k < 3; k++) + { + R[i][j] += Rz[i][k] * Ry[k][j] * Rx[j][k]; + } } + } - // Rotate XYZ vector - double x = xyz[0], y = xyz[1], z = xyz[2]; - xyz[0] = R[0][0]*x + R[0][1]*y + R[0][2]*z; - xyz[1] = R[1][0]*x + R[1][1]*y + R[1][2]*z; - xyz[2] = R[2][0]*x + R[2][1]*y + R[2][2]*z; + // Rotate XYZ vector + double x = xyz[0], y = xyz[1], z = xyz[2]; + xyz[0] = R[0][0] * x + R[0][1] * y + R[0][2] * z; + xyz[1] = R[1][0] * x + R[1][1] * y + R[1][2] * z; + xyz[2] = R[2][0] * x + R[2][1] * y + R[2][2] * z; } //! Handle acoustic information from received data. From 681ce5af0f8eaa328a870d8a90b7b0589a1de859 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Mon, 10 Apr 2023 15:42:27 +0100 Subject: [PATCH 27/35] Transports/Seatrac: Frequency at which attitude is manually set is now a parameter. --- src/Transports/Seatrac/Constants.hpp | 2 -- src/Transports/Seatrac/Task.cpp | 15 +++++++++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/Transports/Seatrac/Constants.hpp b/src/Transports/Seatrac/Constants.hpp index 6a0fc12645..e477421e1e 100644 --- a/src/Transports/Seatrac/Constants.hpp +++ b/src/Transports/Seatrac/Constants.hpp @@ -71,8 +71,6 @@ namespace Transports static const int c_bfr_size = 256; //! Sound speed update window (m/s). static const uint16_t c_sspeed_window = 1; - //! Time between attitude updates for transponder. - static const float c_att_interval = 0.05; //! Entity states. enum EntityStates diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 57507a560a..c4053846c7 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -93,6 +93,8 @@ namespace Transports std::string sound_speed_elabel; //! Calculate NED offsets bool calc_ned_locally; + //! Attitude update frequency when manually set. + float freq_attitude; }; struct Task: public DUNE::Tasks::Task @@ -261,6 +263,13 @@ namespace Transports .defaultValue("true") .description("Calculate NED offsets when USBL range is received."); + 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."); + // Initialize state messages. m_states[STA_BOOT].state = IMC::EntityState::ESTA_BOOT; m_states[STA_BOOT].description = DTR("initializing"); @@ -381,7 +390,8 @@ namespace Transports throw std::runtime_error(m_states[m_state_entity].description); } - m_timer.setTop(c_att_interval); + if (m_args.freq_attitude != 0) + m_timer.setTop(1/m_args.freq_attitude); } //! Update parameters. @@ -420,8 +430,9 @@ namespace Transports { Memory::replace(m_last_angles, msg->clone()); - if (m_args.ahrs_mode || !m_timer.overflow()) + 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; From 67aeb3805725fd0256a629a1891a8ffd7dd0307a Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Mon, 10 Apr 2023 15:57:04 +0100 Subject: [PATCH 28/35] Transports/Seatrac: Added parameter to define threshold to discard positions if fit error is too high. --- src/Transports/Seatrac/Task.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index c4053846c7..a973668bf0 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -95,6 +95,8 @@ namespace Transports bool calc_ned_locally; //! Attitude update frequency when manually set. float freq_attitude; + //! Fit error threshold + float fit_threshold; }; struct Task: public DUNE::Tasks::Task @@ -270,6 +272,11 @@ namespace Transports .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; m_states[STA_BOOT].description = DTR("initializing"); @@ -786,7 +793,17 @@ namespace Transports } if (aco_fix.outputflags_list[4]) - debug("Position received with filter error flag set!"); + { + 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); } From 0585ec9637b4f448ebc35e3b12ba13d54a284939 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Tue, 11 Apr 2023 16:12:48 +0100 Subject: [PATCH 29/35] Transports/Seatrac: Avoid repeated error output messages when modem not connected. --- src/Transports/Seatrac/Task.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index a973668bf0..a13aded943 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -381,7 +381,7 @@ namespace Transports { err("%s: %s", DTR(Status::getString(CODE_COM_ERROR)), e.what()); setState(STA_ERR_COM); - throw std::runtime_error(m_states[m_state_entity].description); + throw RestartNeeded(m_states[m_state_entity].description, 30); } try @@ -563,7 +563,7 @@ namespace Transports { // Throw runtime error if connection problem persists if (Clock::get() > (m_last_input + c_input_tout + 30)) - throw std::runtime_error(m_states[STA_ERR_COM].description); + throw RestartNeeded(m_states[STA_ERR_COM].description, 30); return (Clock::get() < (m_last_input + c_input_tout)); } @@ -789,7 +789,7 @@ namespace Transports aer2xyz(aer, euler, xyz); usblPosition.n = xyz[0]; usblPosition.e = xyz[1]; - usblPosition.d = xyz[2]; + usblPosition.d = xyz[2]; } if (aco_fix.outputflags_list[4]) @@ -1258,10 +1258,7 @@ namespace Transports // Check modem connection if (!hasConnection()) - { setState(STA_ERR_COM); - err("%s", DTR(Status::getString(CODE_COM_ERROR))); - } } } }; From 8b10f88296efe3b16e5493fdfcfea8b169060cd8 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Tue, 11 Apr 2023 18:59:01 +0100 Subject: [PATCH 30/35] Transports/Seatrac: Accuracy field on UsblPosition and UsblAngles messages now being filled with fit error value. --- src/Transports/Seatrac/Task.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index a13aded943..b83df632e6 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -791,7 +791,8 @@ namespace Transports usblPosition.e = xyz[1]; usblPosition.d = xyz[2]; } - + 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!"); @@ -821,6 +822,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); } From 402d2a2d2182942fbac97eae698f9eb5023a0ff4 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Wed, 19 Apr 2023 10:59:38 +0100 Subject: [PATCH 31/35] Transports/Seatrac: Fix usage of 'Active' parameter - if disabled modem doesn't communicate nor processes requests except for range auto-reply (intended for manta systems). --- etc/hardware/lctr-a6xx/seatrac.ini | 2 +- src/Transports/Seatrac/Task.cpp | 34 ++++++++++++++++++++++-------- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/etc/hardware/lctr-a6xx/seatrac.ini b/etc/hardware/lctr-a6xx/seatrac.ini index b5c5ca932b..898d7e0637 100644 --- a/etc/hardware/lctr-a6xx/seatrac.ini +++ b/etc/hardware/lctr-a6xx/seatrac.ini @@ -32,7 +32,7 @@ [Transports.Seatrac] Enabled = Hardware Debug Level = None -Active = true +Active = true # keep always active for LAUVs Entity Label = Acoustic Modem Serial Port - Device = /dev/uart/10 Serial Port - Baud Rate = 115200 diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index b83df632e6..0044af2b15 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -112,7 +112,7 @@ namespace Transports //! c_preamble detected bool m_preamble; //! Current state. - EntityStates m_state_entity; + EntityStates m_state; //! Entity states. IMC::EntityState m_states[STA_MAX]; //! True if the beacon has an USBL receiver. @@ -304,9 +304,9 @@ namespace Transports void setState(EntityStates state) { - m_state_entity = state; - setEntityState((IMC::EntityState::StateEnum) m_states[m_state_entity].state, - m_states[m_state_entity].description); + m_state = state; + setEntityState((IMC::EntityState::StateEnum) m_states[m_state].state, + m_states[m_state].description); } //! Open TCP socket. @@ -381,7 +381,7 @@ namespace Transports { err("%s: %s", DTR(Status::getString(CODE_COM_ERROR)), e.what()); setState(STA_ERR_COM); - throw RestartNeeded(m_states[m_state_entity].description, 30); + throw RestartNeeded(m_states[m_state].description, 30); } try @@ -394,7 +394,7 @@ namespace Transports { err("%s", DTR(Status::getString(CODE_COM_ERROR))); setState(STA_ERR_STP); - throw std::runtime_error(m_states[m_state_entity].description); + throw std::runtime_error(m_states[m_state].description); } if (m_args.freq_attitude != 0) @@ -435,6 +435,9 @@ namespace Transports void consume(const IMC::EulerAngles* msg) { + if (m_state != STA_ACTIVE) + return; + Memory::replace(m_last_angles, msg->clone()); if (m_args.ahrs_mode || !m_timer.overflow() || !m_args.freq_attitude) @@ -465,6 +468,9 @@ namespace Transports void consume(const IMC::MagneticField* msg) { + if (m_state != STA_ACTIVE) + return; + if(!m_args.ahrs_mode) return; @@ -496,7 +502,7 @@ namespace Transports void consume(const IMC::SoundSpeed* msg) { - if (m_state_entity != STA_ACTIVE) + if (m_state != STA_ACTIVE) return; if ((int)msg->getSourceEntity() != m_sound_speed_eid) @@ -561,6 +567,10 @@ namespace Transports bool hasConnection(void) { + // 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)) throw RestartNeeded(m_states[STA_ERR_COM].description, 30); @@ -982,6 +992,9 @@ namespace Transports void consume(const IMC::UamTxFrame* msg) { + if (m_state != STA_ACTIVE) + return; + debug(DTR("Received UamTxFrame with dst=0x%04X. Msg for system '%s'"), msg->getDestination(), msg->sys_dst.c_str()); if (msg->getDestination() != getSystemId()) @@ -1152,6 +1165,9 @@ namespace Transports void processInput(double timeout = 1.0) { + if (!isActive()) + return; + double deadline = Clock::get() + timeout; do { @@ -1160,7 +1176,7 @@ namespace Transports processNewData(); checkTxOWAY(); - if ((m_state_entity != STA_ERR_STP) && hasConnection()) + if ((m_state != STA_ERR_STP) && hasConnection()) { if (!isRestricted()) setState(STA_ACTIVE); @@ -1259,7 +1275,7 @@ namespace Transports waitForMessages(1.0); // Check modem connection - if (!hasConnection()) + if (!hasConnection() && isActive()) setState(STA_ERR_COM); } } From 47d58bec547c8d8a10ffea7ac1ca5247398feee2 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Thu, 20 Apr 2023 11:52:11 +0100 Subject: [PATCH 32/35] Transports/Seatrac: Improve handling communication error on boot. --- src/Transports/Seatrac/Task.cpp | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 0044af2b15..847f258b50 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -379,9 +379,12 @@ namespace Transports } catch (std::runtime_error& e) { - err("%s: %s", DTR(Status::getString(CODE_COM_ERROR)), e.what()); - setState(STA_ERR_COM); - throw RestartNeeded(m_states[m_state].description, 30); + if (m_state != STA_ERR_COM) + { + err("%s: %s", DTR(Status::getString(CODE_COM_ERROR)), e.what()); + setState(STA_ERR_COM); + } + return; } try @@ -572,7 +575,7 @@ namespace Transports m_last_input = Clock::get(); // Throw runtime error if connection problem persists - if (Clock::get() > (m_last_input + c_input_tout + 30)) + 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)); @@ -1165,7 +1168,8 @@ namespace Transports void processInput(double timeout = 1.0) { - if (!isActive()) + // Wait for modem configuration + if (!isActive() || !m_config_status) return; double deadline = Clock::get() + timeout; @@ -1266,9 +1270,11 @@ namespace Transports { while (!stopping()) { - // Wait for modem configuration - if(!m_config_status) - break; + // 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(); From 8ccf471f47ea043cff867339bb9fbc23763a8956 Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Thu, 20 Apr 2023 11:59:52 +0100 Subject: [PATCH 33/35] Revert "Transports/Seatrac: Calculate relative position of the other modem when USBL ranges are received." This reverts commit fd70b34c35b4bddc0966b859af69e3e9a097e49e. --- src/Transports/Seatrac/Task.cpp | 83 ++------------------------------- 1 file changed, 4 insertions(+), 79 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 847f258b50..584654518b 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -91,8 +91,6 @@ namespace Transports double sound_speed_def; //! Entity label of sound speed provider. std::string sound_speed_elabel; - //! Calculate NED offsets - bool calc_ned_locally; //! Attitude update frequency when manually set. float freq_attitude; //! Fit error threshold @@ -173,8 +171,7 @@ namespace Transports m_addr(0), m_tstamp(0), m_ticket(nullptr), - m_sound_speed_eid(-1), - m_last_angles(nullptr) + m_sound_speed_eid(-1) { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_GLOBAL, @@ -441,8 +438,6 @@ namespace Transports if (m_state != STA_ACTIVE) return; - Memory::replace(m_last_angles, msg->clone()); - if (m_args.ahrs_mode || !m_timer.overflow() || !m_args.freq_attitude) return; @@ -691,53 +686,6 @@ namespace Transports handleAcousticInformation(m_data_beacon.cid_dat_receive_msg.aco_fix); } - // Convert AER to rotated XYZ position - //! @param[in] aer AER position. - //! @param[in] euler Euler angles. - //! @param[out] xyz XYZ position. - void - aer2xyz(double aer[3], double euler[3], double xyz[3]) - { - // Compute intermediate variables - double sinA = std::sin(aer[0]), cosA = std::cos(aer[0]); - double sinE = std::sin(aer[1]), cosE = std::cos(aer[1]); - - // Convert to local XYZ - xyz[0] = -aer[2] * sinE * cosA;// X - xyz[1] = aer[2] * sinE * sinA; // Y - xyz[2] = -aer[2] * cosE; // Z - - // Apply Euler rotation - double Rz[3][3] = { { std::cos(euler[0]), -std::sin(euler[0]), 0 }, - { std::sin(euler[0]), std::cos(euler[0]), 0 }, - { 0, 0, 1 } }; - double Ry[3][3] = { { std::cos(euler[1]), 0, std::sin(euler[1]) }, - { 0, 1, 0 }, - { -std::sin(euler[1]), 0, std::cos(euler[1]) } }; - double Rx[3][3] = { { 1, 0, 0 }, - { 0, std::cos(euler[2]), -std::sin(euler[2]) }, - { 0, std::sin(euler[2]), std::cos(euler[2]) } }; - double R[3][3] = { { 0 } }; - - // Compute overall rotation matrix - for (int i = 0; i < 3; i++) - { - for (int j = 0; j < 3; j++) - { - for (int k = 0; k < 3; k++) - { - R[i][j] += Rz[i][k] * Ry[k][j] * Rx[j][k]; - } - } - } - - // Rotate XYZ vector - double x = xyz[0], y = xyz[1], z = xyz[2]; - xyz[0] = R[0][0] * x + R[0][1] * y + R[0][2] * z; - xyz[1] = R[1][0] * x + R[1][1] * y + R[1][2] * z; - xyz[2] = R[2][0] * x + R[2][1] * y + R[2][2] * z; - } - //! Handle acoustic information from received data. //! param[in] aco_fix Acoustic information field of the received message. void @@ -778,32 +726,9 @@ namespace Transports usblPosition.phi = Angles::radians(aco_fix.attitude_roll / 10.0); usblPosition.theta = Angles::radians(aco_fix.attitude_pitch / 10.0); usblPosition.psi = Angles::radians(aco_fix.attitude_yaw / 10.0); - - if (!m_args.calc_ned_locally) - { - usblPosition.e = aco_fix.position_easting / 10.0; - usblPosition.n = aco_fix.position_northing / 10.0; - usblPosition.d = aco_fix.position_depth / 10.0; - } - else // calculate the NED offsets locally - { - if (m_last_angles == nullptr) - { - war("Cannot calculate NED offsets internally because no euler angles have been received."); - return; - } - double xyz[3], aer[3], euler[3]; - aer[0] = Angles::radians(aco_fix.usbl_azimuth / 10.0); - aer[1] = Angles::radians(aco_fix.usbl_elevation / 10.0); - aer[2] = aco_fix.range_dist / 10.0; - euler[0] = m_last_angles->phi; - euler[1] = m_last_angles->theta; - euler[2] = m_last_angles->psi; - aer2xyz(aer, euler, xyz); - usblPosition.n = xyz[0]; - usblPosition.e = xyz[1]; - usblPosition.d = xyz[2]; - } + 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]) From 83f9896f848bef09f42c075e8785856b07fc5f2e Mon Sep 17 00:00:00 2001 From: Maria Costa Date: Thu, 20 Apr 2023 12:09:58 +0100 Subject: [PATCH 34/35] Transports/Seatrac: Complete revert of local relative position calculations for USBL commit. --- src/Transports/Seatrac/Task.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/Transports/Seatrac/Task.cpp b/src/Transports/Seatrac/Task.cpp index 584654518b..2e298b2856 100644 --- a/src/Transports/Seatrac/Task.cpp +++ b/src/Transports/Seatrac/Task.cpp @@ -155,8 +155,6 @@ namespace Transports Math::Matrix m_rotation; //! Timer. Time::Counter m_timer; - //! Last euler angles. - IMC::EulerAngles* m_last_angles; //! Constructor. //! @param[in] name task name. @@ -258,10 +256,6 @@ namespace Transports param("Sound Speed - Entity Label", m_args.sound_speed_elabel) .description("Entity label of sound speed provider"); - param("Calculate NED offsets locally", m_args.calc_ned_locally) - .defaultValue("true") - .description("Calculate NED offsets when USBL range is received."); - param("Attitude update frequency", m_args.freq_attitude) .units(DUNE::Units::Hertz) .defaultValue("20") From 9ff0beb0c4e3d908d81e5437fc7a40c2786c21a6 Mon Sep 17 00:00:00 2001 From: Ricardo Martins Date: Wed, 25 Jan 2017 17:09:29 +0000 Subject: [PATCH 35/35] DUNE/Network: added utility class NodeMap (import from OMST). --- src/DUNE/Network.hpp | 1 + src/DUNE/Network/NodeMap.hpp | 112 +++++++++++++++++++++++++++++++++++ 2 files changed, 113 insertions(+) create mode 100644 src/DUNE/Network/NodeMap.hpp 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