diff --git a/src/Sensors/Imagenex872/Frame872.hpp b/src/Sensors/Imagenex872/Frame872.hpp new file mode 100644 index 0000000000..ea4c43ce94 --- /dev/null +++ b/src/Sensors/Imagenex872/Frame872.hpp @@ -0,0 +1,311 @@ +//*************************************************************************** +// Copyright 2007-2015 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Ana Santos * +//*************************************************************************** +#ifndef DUNE_FRAME872_HPP +#define DUNE_FRAME872_HPP + +// ISO C++ 98 headers. +#include +#include + +// DUNE headers. +#include + +namespace Sensors +{ + namespace Imagenex872 { + using DUNE_NAMESPACES; + + //! Size of 872 frame. + const unsigned c_ping_size = 4096; + //! Data points channel + static const uint16_t c_data_points_channel = 1000; + //! GPS string file offset + static const uint16_t c_gps_string_file_offset = 3200; + //! Number of bytes to previous ping + static const uint16_t c_bytes_previous_ping = 8192; + + //! Data logger base frame for Imagenex files. + class Frame872 { + public: + + //! Constructor. + Frame872(void) { + + m_data.resize(c_ping_size, 0); + m_data[0] = '8'; + m_data[1] = '7'; + m_data[2] = '2'; + m_data[3] = '0'; + + //Number of bytes that are written to the disk + m_data[8] = c_ping_size >> 8; + m_data[9] = c_ping_size & 0xff; + + //Data points per channel + m_data[10] = c_data_points_channel >> 8; + m_data[11] = c_data_points_channel & 0xff; + + //Bytes per data point - always 1 + m_data[12] = 1; + + //Data point bit depth - always 8 + m_data[13] = 8; + + //Gps type (GPRMC) and number of strings (1) - 00100001 + m_data[14] = 0x21; + + //GPS string file offset - 3200 + m_data[15] = c_gps_string_file_offset >> 8; + m_data[16] = c_gps_string_file_offset & 0xff; + + //Event/Annotation counter + m_data[17] = 0; + m_data[18] = 0; + + // Reserved always 0 + m_data[66] = 0; + m_data[67] = 0; + m_data[68] = 0; + m_data[69] = 0; + + // Sonar type + m_data[70] = 0; + + // 0's fill 1 + char zeroFill1[928]; + memset(zeroFill1, '\0', 928); + std::memcpy(&m_data[72], &zeroFill1, 928); + + // 0's fill 2 + char zeroFill2[993]; + memset(zeroFill2, '\0' , 993); + std::memcpy(&m_data[3100], &zeroFill2, 993); + + // Previous ping + m_data[4094] = c_bytes_previous_ping >> 8; + m_data[4095] = c_bytes_previous_ping & 0xFF; + + } + + //! Destructor + ~Frame872(void) {} + + //! Get frame start address. + //! @return pointer to address. + const uint8_t* + getData(void) + { + return &m_data[0]; + } + + void + setPingNumber(uint32_t ping_number){ + m_data[4] = ping_number >> 24; + m_data[5] = ping_number >> 16; + m_data[6] = ping_number >> 8; + m_data[7] = ping_number & 0xff; + } + + void + setTimeInfo(){ + + uint64_t epoch_time = Time::Clock::getSinceEpochMsec(); + + // date + time_t time_stamp = epoch_time / 1000; + struct tm *tm = localtime(&time_stamp); + char date[12]; + strftime(date, sizeof(date), "%d-%b-%Y", tm); + std::memcpy(&m_data[19], &date, 12); + m_data[30] = '\0'; + + // time + char time[9]; + strftime(time, sizeof(time), "%H:%M:%S", tm); + std::memcpy(&m_data[31], &time, 9); + m_data[39] = '\0'; + + // Thousandths of seconds + char buffer[3]; + sprintf(buffer,"%03d", (int) (epoch_time % 1000)); + + m_data[40] = '.'; + m_data[41] = buffer[0]; + m_data[42] = buffer[1]; + m_data[43] = buffer[2]; + m_data[44] = '\0'; + } + + void + setOperationFrequency(unsigned exec_freq){ + m_data[45] = exec_freq; + } + + void + setDataGain(unsigned data_gain){ + m_data[47] = data_gain; + } + + // Channel balance (balance gain) + void + setBalanceGain(unsigned balance_gain){ + m_data[48] = balance_gain; + } + + // Repetition rate (time between pings) + void + setRepetitionRate(unsigned time_between_pings){ + m_data[49] = time_between_pings >> 8; + m_data[50] = time_between_pings & 0xff; + } + + // Sound velocity + void + setSoundSpeed(uint16_t sound_speed){ + m_data[51] = sound_speed >> 8; + m_data[52] = sound_speed & 0xff ; + } + + // 12 Byte file header + void + setFileHeader(const uint8_t* data_header) { + std::memcpy(&m_data[53], &data_header[0], 12); + } + + //Real range + void + setRealRange(uint8_t range) { + m_data[71] = range; + } + + // PORT & STARBOARD channel echo data + void + setData(const std::vector ping_data) { + std::memcpy(&m_data[1000], &ping_data.at(0), c_data_points_channel * 2); + } + + // GPS Strings + void + setGpsString(const IMC::EstimatedState state) { + char gpsString[100]; + memset(gpsString, 0, 100); + std::string sentence = createRMC(state); + sprintf(gpsString, "%s", sentence.c_str()); + std::memcpy(&m_data[3000], &gpsString, 100); + } + + std::string + createRMC(const IMC::EstimatedState state) + { + double time_reference = Math::round(Clock::getSinceEpochMsec()); + time_t secs = (time_t)time_reference; + double fraction = time_reference - secs; + unsigned fsec = fraction * 100; + Time::BrokenDown bdt(secs); + std::string stn_str; + + double lat = state.lat; + double lon = state.lon; + + Coordinates::toWGS84(state, lat, lon); + + std::string lat_nmea = latitudeToNMEA(lat); + std::string lon_nmea = longitudeToNMEA(lon); + + double vel = Math::norm(state.vx, state.vy); + + NMEAWriter stn("GPRMC"); + stn << String::str("%02u%02u%02u.%02u", bdt.hour, bdt.minutes, bdt.seconds, fsec) + << "A" + << lat_nmea + << lon_nmea + << vel * DUNE::Units::c_ms_to_knot + << 0 // azimuth. + << String::str("%02u%02u%02u", bdt.day, bdt.month, bdt.year - 2000) + << "" + << "" + << "A"; + + return stn.sentence(); + } + + void + setRangeIndex(const uint8_t range) { + + uint8_t index = 0x07; + + switch (range) + { + case 10: + index = 0x05; + break; + case 20: + index = 0x06; + break; + case 30: + index = 0x07; + break; + case 40: + index = 0x08; + break; + case 50: + index = 0x09; + break; + case 60: + index = 0x0a; + break; + case 80: + index = 0x0b; + break; + case 100: + index = 0x0c; + break; + case 125: + index = 0x0d; + break; + case 150: + index = 0x0f; + break; + case 200: + index = 0x10; + break; + } + + //Range index + m_data[46] = index; + } + + private: + //! Message data. + std::vector m_data; + }; + } +} + +#endif //DUNE_FRAME872_HPP diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 888c2e5e86..98ff761ea1 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -32,312 +32,518 @@ // DUNE headers. #include +#include "Frame872.hpp" namespace Sensors { - //! Device driver for the Imagenex 872 "YellowFin" Sidescan Sonar. - namespace Imagenex872 - { - using DUNE_NAMESPACES; - - enum Side + //! Device driver for the Imagenex 872 "YellowFin" Sidescan Sonar. + namespace Imagenex872 { - SIDE_PORT, - SIDE_STARBOARD - }; + using DUNE_NAMESPACES; - enum Index - { - // Range index. - SD_RANGE = 3, - // Mode/Frequency index. - SD_MODE_FREQ = 7, - // Data Gain index. - SD_DAT_GAIN = 8, - // Balance Gain index. - SD_BAL_GAIN = 10, - // TCP Packet index. - SD_TCP_PKT = 18 - }; - - struct Arguments - { - // IPv4 address. - Address addr; - // TCP port. - unsigned port; - // Data gain. - unsigned dat_gain; - // Balance gain. - unsigned bal_gain; - // Default frequency. - unsigned frequency; - // Default range. - unsigned range; - }; - - // List of available ranges. - static const unsigned c_ranges[] = {10, 20, 30, 40, 50, 60, 80, 100, 125, 150, 200}; - // Count of available ranges. - static const unsigned c_ranges_size = sizeof(c_ranges) / sizeof(c_ranges[0]); - // List of range rates in ms. - static const unsigned c_range_rates[] = {57, 57, 63, 76, 90, 102, 129, 156, 190, 223, 290}; - // List of available frequencies. - static const unsigned c_freqs[] = {260, 330, 770}; - // Count of available frequencies. - static const unsigned c_freqs_size = sizeof(c_freqs) / sizeof(c_freqs[0]); - // List of beam width configurations. - static const float c_beam_width[] = {75, 60, 30}; - // List of beam height configurations. - static const float c_beam_height[] = {2.2, 1.8, 0.7}; - // Switch data size. - static const int c_sdata_size = 27; - // Return data header size. - static const int c_rdata_hdr_size = 12; - // Return data payload size. - static const int c_rdata_dat_size = 1000; - // Return data footer size. - static const int c_rdata_ftr_size = 1; - - struct Task: public Tasks::Periodic - { - // TCP socket. - TCPSocket* m_sock; - // Output switch data. - uint8_t m_sdata[c_sdata_size]; - // Return data. - uint8_t m_rdata_hdr[c_rdata_hdr_size]; - // Return data. - uint8_t m_rdata_ftr[c_rdata_ftr_size]; - // Single sidescan ping. - IMC::SonarData m_ping; - // Configuration parameters. - Arguments m_args; - - Task(const std::string& name, Tasks::Context& ctx): - Tasks::Periodic(name, ctx), - m_sock(NULL) - { - // Define configuration parameters. - paramActive(Tasks::Parameter::SCOPE_MANEUVER, - Tasks::Parameter::VISIBILITY_USER); - - param("IPv4 Address", m_args.addr) - .defaultValue("192.168.0.5") - .description("IP address of the sonar"); - - param("TCP Port", m_args.port) - .defaultValue("4040") - .minimumValue("0") - .maximumValue("65535") - .description("TCP port"); - - param("Data Gain", m_args.dat_gain) - .defaultValue("40") - .units(Units::Percentage) - .minimumValue("0") - .maximumValue("100") - .description("Data gain"); - - param("Balance Gain", m_args.bal_gain) - .defaultValue("30") - .minimumValue("0") - .maximumValue("60") - .description("Balance gain"); - - param(DTR_RT("Frequency"), m_args.frequency) - .visibility(Tasks::Parameter::VISIBILITY_USER) - .scope(Tasks::Parameter::SCOPE_MANEUVER) - .defaultValue("770") - .values("260, 330, 770") - .units(Units::Kilohertz) - .description(DTR("Operating frequency")); - - param(DTR_RT("Range"), m_args.range) - .visibility(Tasks::Parameter::VISIBILITY_USER) - .scope(Tasks::Parameter::SCOPE_MANEUVER) - .defaultValue("30") - .units(Units::Meter) - .valuesIf("Frequency", "260", "10, 20, 30, 40, 50, 60, 80, 100, 150, 200") - .valuesIf("Frequency", "330", "10, 20, 30, 40, 50, 60, 80, 100, 150, 200") - .valuesIf("Frequency", "770", "10, 20, 30, 40, 50") - .description(DTR("Operating range")); - - // Initialize switch data. - std::memset(m_sdata, 0, sizeof(m_sdata)); - m_sdata[0] = 0xfe; - m_sdata[1] = 0x44; - m_sdata[26] = 0xfd; - - // Initialize return data. - m_ping.data.resize(c_rdata_dat_size * 2); - m_ping.type = IMC::SonarData::ST_SIDESCAN; - m_ping.bits_per_point = 8; - m_ping.scale_factor = 1.0f; - } - - void - onUpdateParameters(void) - { - setFrequency(m_args.frequency); - setRange(m_args.range); - setDataGain(m_args.dat_gain); - setBalanceGain(m_args.bal_gain); - - if (paramChanged(m_args.addr) && m_sock != NULL) - throw RestartNeeded(DTR("restarting to change IPv4 address"), 1); - - if (paramChanged(m_args.port) && m_sock != NULL) - throw RestartNeeded(DTR("restarting to change TCP port"), 1); - } - - void - onResourceAcquisition(void) - { - m_sock = new TCPSocket(); - m_sock->setNoDelay(true); - } - - void - onResourceRelease(void) - { - Memory::clear(m_sock); - } - - void - onResourceInitialization(void) - { - try + enum Side { - m_sock->connect(m_args.addr, m_args.port); - pingBoth(); - setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); - } - catch (std::runtime_error& e) - { - throw RestartNeeded(e.what(), 10.0, false); - } - } - - void - onActivation(void) - { - setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); - } - - void - onDeactivation(void) - { - setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); - } - - unsigned - getIndex(unsigned value, const unsigned* table, unsigned table_size) - { - for (unsigned i = 0; i < table_size; ++i) - { - if (value <= table[i]) - return i; - } - - return table_size - 1; - } - - void - setFrequency(unsigned value) - { - unsigned idx = getIndex(value, c_freqs, c_freqs_size); - m_sdata[SD_MODE_FREQ] = (uint8_t)idx; - m_ping.frequency = c_freqs[idx] * 1000; - - m_ping.beam_config.clear(); - IMC::BeamConfig bc; - bc.beam_width = Math::Angles::radians(c_beam_width[idx]); - bc.beam_height = Math::Angles::radians(c_beam_height[idx]); - m_ping.beam_config.push_back(bc); - } - - void - setRange(unsigned value) - { - unsigned idx = getIndex(value, c_ranges, c_ranges_size); - m_sdata[SD_RANGE] = (uint8_t)c_ranges[idx]; - m_ping.min_range = 0; - m_ping.max_range = c_ranges[idx]; - Periodic::setFrequency(1.0 / (c_range_rates[idx] / 1000.0)); - } - - void - setDataGain(unsigned value) - { - m_sdata[SD_DAT_GAIN] = (uint8_t) Math::trimValue(value, 0u, 100u); - } - - void - setBalanceGain(unsigned value) - { - m_sdata[SD_BAL_GAIN] = (uint8_t) Math::trimValue(value, 0u, 60u); - } - - void - ping(Side side) - { - m_sdata[SD_TCP_PKT] = (side == SIDE_STARBOARD) ? 0x02 : 0x00; - m_sock->write((char*)m_sdata, c_sdata_size); - - int rv = m_sock->read((char*)m_rdata_hdr, c_rdata_hdr_size); - if (rv != c_rdata_hdr_size) - throw std::runtime_error(DTR("failed to read header")); - - unsigned dat_idx = ((side == SIDE_STARBOARD) ? 1 : 0) * c_rdata_dat_size; - rv = m_sock->read(&m_ping.data[dat_idx], c_rdata_dat_size); - if (rv != c_rdata_dat_size) - throw std::runtime_error(DTR("failed to read data")); - - rv = m_sock->read((char*)m_rdata_ftr, c_rdata_ftr_size); - if (rv != c_rdata_ftr_size) - throw std::runtime_error(DTR("failed to read footer")); - - // Correct port imagery. - if (side == SIDE_PORT) + SIDE_PORT, + SIDE_STARBOARD + }; + + enum Index { - for (unsigned i = 0; i < c_rdata_dat_size / 2; ++i) - { - char tmp = m_ping.data[i]; - m_ping.data[i] = m_ping.data[c_rdata_dat_size - 1 - i]; - m_ping.data[c_rdata_dat_size - 1 - i] = tmp; - } - } - } - - void - pingBoth(void) - { - ping(SIDE_PORT); - ping(SIDE_STARBOARD); - setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); - } - - void - task(void) - { - if (!isActive()) - return; - - try + // Range index. + SD_RANGE = 3, + // Mode/Frequency index. + SD_MODE_FREQ = 7, + // Data Gain index. + SD_DAT_GAIN = 8, + // Balance Gain index. + SD_BAL_GAIN = 10, + // TCP Packet index. + SD_TCP_PKT = 18 + }; + + struct Arguments { - pingBoth(); - dispatch(m_ping); - } - catch (std::exception& e) + // IPv4 address. + Address addr; + // TCP port. + unsigned port; + // Data gain. + unsigned dat_gain; + // Balance gain. + unsigned bal_gain; + // Default frequency. + unsigned frequency; + // Default range. + unsigned range; + // Execution frequency + unsigned exec_freq; + // Filter pings between UAM communication + bool filter_uam; + // Time stop write pings to file + float time_filter_uam; + }; + + // List of available ranges. + static const unsigned c_ranges[] = {10, 20, 30, 40, 50, 60, 80, 100, 125, 150, 200}; + // Count of available ranges. + static const unsigned c_ranges_size = sizeof(c_ranges) / sizeof(c_ranges[0]); + // List of range rates in ms. + static const unsigned c_range_rates[] = {57, 57, 63, 76, 90, 102, 129, 156, 190, 223, 290}; + // List of available frequencies. + static const unsigned c_freqs[] = {260, 330, 770}; + // Count of available frequencies. + static const unsigned c_freqs_size = sizeof(c_freqs) / sizeof(c_freqs[0]); + // List of beam width configurations. + static const float c_beam_width[] = {75, 60, 30}; + // List of beam height configurations. + static const float c_beam_height[] = {2.2, 1.8, 0.7}; + // Switch data size. + static const uint32_t c_sdata_size = 27; + // Return data header size. + static const uint32_t c_rdata_hdr_size = 12; + // Return data payload size. + static const uint32_t c_rdata_dat_size = 1000; + // Return data footer size. + static const uint32_t c_rdata_ftr_size = 1; + // Raw log file name + static const std::string c_file_name = "Data.872"; + + + struct Task: public Tasks::Periodic { - err("%s", e.what()); - setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); - throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); - } - } - }; - } + // TCP socket. + TCPSocket* m_sock; + // Output switch data. + uint8_t m_sdata[c_sdata_size]; + // Return data. + uint8_t m_rdata_hdr[c_rdata_hdr_size]; + // Return data. + uint8_t m_rdata_ftr[c_rdata_ftr_size]; + // Single sidescan ping. + IMC::SonarData m_ping; + // Configuration parameters. + Arguments m_args; + // Dune context + Context& m_ctx; + // Raw log file + std::ofstream m_data_file; + // Raw log path + std::string m_data_path; + //! 872 Frame. + Frame872* m_frame872; + //! Ping counter + uint32_t m_ping_number; + //! Last valid sound speed value + uint16_t m_sound_speed; + //! Time of last ping + uint64_t m_time_last_ping; + + // Receive estimated state message + bool m_receive_estimated_state; + // Receive dev data text message + bool m_receive_dev_data_text; + // Receive logging control message + bool m_receive_log_control; + // If an acoustic message is being transmitted + bool m_acoustic_transmission; + // Timer to break pings between acoustic operations + Time::Counter m_timer_counter; + + Task(const std::string& name, Tasks::Context& ctx): + Tasks::Periodic(name, ctx), + m_sock(NULL), + m_ctx(ctx), + m_data_path(""), + m_frame872(NULL), + m_ping_number(0), + m_sound_speed(1500), + m_time_last_ping(Clock::getSinceEpochMsec() * 1000), + m_receive_estimated_state(false), + m_receive_dev_data_text(false), + m_receive_log_control(false), + m_acoustic_transmission(false) + { + // Define configuration parameters. + paramActive(Tasks::Parameter::SCOPE_MANEUVER, + Tasks::Parameter::VISIBILITY_USER); + + param("IPv4 Address", m_args.addr) + .defaultValue("192.168.0.5") + .description("IP address of the sonar"); + + param("TCP Port", m_args.port) + .defaultValue("4040") + .minimumValue("0") + .maximumValue("65535") + .description("TCP port"); + + param("Data Gain", m_args.dat_gain) + .defaultValue("40") + .units(Units::Percentage) + .minimumValue("0") + .maximumValue("100") + .description("Data gain"); + + param("Balance Gain", m_args.bal_gain) + .defaultValue("30") + .minimumValue("0") + .maximumValue("60") + .description("Balance gain"); + + param(DTR_RT("Frequency"), m_args.frequency) + .visibility(Tasks::Parameter::VISIBILITY_USER) + .scope(Tasks::Parameter::SCOPE_MANEUVER) + .defaultValue("770") + .values("260, 330, 770") + .units(Units::Kilohertz) + .description(DTR("Operating frequency")); + + param(DTR_RT("Range"), m_args.range) + .visibility(Tasks::Parameter::VISIBILITY_USER) + .scope(Tasks::Parameter::SCOPE_MANEUVER) + .defaultValue("30") + .units(Units::Meter) + .valuesIf("Frequency", "260", "10, 20, 30, 40, 50, 60, 80, 100, 150, 200") + .valuesIf("Frequency", "330", "10, 20, 30, 40, 50, 60, 80, 100, 150, 200") + .valuesIf("Frequency", "770", "10, 20, 30, 40, 50") + .description(DTR("Operating range")); + + param(DTR_RT("Execution Frequency"), m_args.exec_freq) + .visibility(Tasks::Parameter::VISIBILITY_USER) + .scope(Tasks::Parameter::SCOPE_MANEUVER) + .defaultValue("1.0") + .values("0.0, 1.0, 2.0") + .description(DTR("Execution frequency")); + + param(DTR_RT("Filter UAM"), m_args.filter_uam) + .visibility(Tasks::Parameter::VISIBILITY_USER) + .scope(Tasks::Parameter::SCOPE_MANEUVER) + .defaultValue("false") + .values("true, false") + .description(DTR("Filter UAM")); + + param(DTR_RT("Time filter UAM"), m_args.time_filter_uam) + .visibility(Tasks::Parameter::VISIBILITY_USER) + .scope(Tasks::Parameter::SCOPE_MANEUVER) + .defaultValue("0.4") + .description(DTR("Time filter UAM")); + + // Initialize switch data. + std::memset(m_sdata, 0, sizeof(m_sdata)); + m_sdata[0] = 0xfe; + m_sdata[1] = 0x44; + m_sdata[26] = 0xfd; + + // Initialize return data. + m_ping.data.resize(c_rdata_dat_size * 2); + m_ping.type = IMC::SonarData::ST_SIDESCAN; + m_ping.bits_per_point = 8; + m_ping.scale_factor = 1.0f; + + bind(this); + bind(this); + bind(this); + bind(this); + } + + void + onUpdateParameters(void) + { + setFrequency(m_args.frequency); + setRange(m_args.range); + setDataGain(m_args.dat_gain); + setBalanceGain(m_args.bal_gain); + + if (paramChanged(m_args.addr) && m_sock != NULL) + throw RestartNeeded(DTR("restarting to change IPv4 address"), 1); + + if (paramChanged(m_args.port) && m_sock != NULL) + throw RestartNeeded(DTR("restarting to change TCP port"), 1); + + if (m_frame872 == NULL) + m_frame872 = new Frame872(); + + m_frame872->setOperationFrequency(m_args.exec_freq); + m_frame872->setDataGain(m_args.dat_gain); + m_frame872->setBalanceGain(m_args.bal_gain); + } + + void + onResourceAcquisition(void) + { + m_sock = new TCPSocket(); + m_sock->setNoDelay(true); + + m_frame872 = new Frame872(); + m_frame872->setOperationFrequency(m_args.exec_freq); + m_frame872->setDataGain(m_args.dat_gain); + m_frame872->setBalanceGain(m_args.bal_gain); + } + + void + onResourceRelease(void) + { + Memory::clear(m_sock); + + if(m_data_file.is_open()) + m_data_file.close(); + + Memory::clear(m_frame872); + } + + void + onResourceInitialization(void) + { + try + { + m_sock->connect(m_args.addr, m_args.port); + pingBoth(); + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); + } + catch (std::runtime_error& e) + { + throw RestartNeeded(e.what(), 10.0, false); + } + } + + void + onActivation(void) + { + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); + } + + void + onDeactivation(void) + { + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); + } + + unsigned + getIndex(unsigned value, const unsigned* table, unsigned table_size) + { + for (unsigned i = 0; i < table_size; ++i) + { + if (value <= table[i]) + return i; + } + + return table_size - 1; + } + + void + setFrequency(unsigned value) + { + unsigned idx = getIndex(value, c_freqs, c_freqs_size); + m_sdata[SD_MODE_FREQ] = (uint8_t)idx; + m_ping.frequency = c_freqs[idx] * 1000; + + m_ping.beam_config.clear(); + IMC::BeamConfig bc; + bc.beam_width = Math::Angles::radians(c_beam_width[idx]); + bc.beam_height = Math::Angles::radians(c_beam_height[idx]); + m_ping.beam_config.push_back(bc); + } + + void + setRange(unsigned value) + { + unsigned idx = getIndex(value, c_ranges, c_ranges_size); + m_sdata[SD_RANGE] = (uint8_t)c_ranges[idx]; + m_ping.min_range = 0; + m_ping.max_range = c_ranges[idx]; + Periodic::setFrequency(1.0 / (c_range_rates[idx] / 1000.0)); + } + + void + setDataGain(unsigned value) + { + m_sdata[SD_DAT_GAIN] = (uint8_t) Math::trimValue(value, 0u, 100u); + } + + void + setBalanceGain(unsigned value) + { + m_sdata[SD_BAL_GAIN] = (uint8_t) Math::trimValue(value, 0u, 60u); + } + + void + ping(Side side) + { + m_sdata[SD_TCP_PKT] = (side == SIDE_STARBOARD) ? 0x02 : 0x00; + m_sock->write((char*)m_sdata, c_sdata_size); + + int rv = m_sock->read((char*)m_rdata_hdr, c_rdata_hdr_size); + if (rv != c_rdata_hdr_size) + throw std::runtime_error(DTR("failed to read header")); + + unsigned dat_idx = ((side == SIDE_STARBOARD) ? 1 : 0) * c_rdata_dat_size; + rv = m_sock->read(&m_ping.data[dat_idx], c_rdata_dat_size); + if (rv != c_rdata_dat_size) + throw std::runtime_error(DTR("failed to read data")); + + rv = m_sock->read((char*)m_rdata_ftr, c_rdata_ftr_size); + if (rv != c_rdata_ftr_size) + throw std::runtime_error(DTR("failed to read footer")); + + // Correct port imagery. + if (side == SIDE_PORT) + { + for (unsigned i = 0; i < c_rdata_dat_size / 2; ++i) + { + char tmp = m_ping.data[i]; + m_ping.data[i] = m_ping.data[c_rdata_dat_size - 1 - i]; + m_ping.data[c_rdata_dat_size - 1 - i] = tmp; + } + } + } + + void + pingBoth(void) + { + ping(SIDE_PORT); + ping(SIDE_STARBOARD); + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); + + if(m_frame872 != NULL) { + m_frame872->setTimeInfo(); + m_frame872->setPingNumber(++m_ping_number); + m_frame872->setFileHeader(m_rdata_hdr); + m_frame872->setData(m_ping.data); + m_frame872->setRealRange(m_rdata_hdr[4]); + m_frame872->setRangeIndex(m_rdata_hdr[4]); + + uint64_t current_time = Clock::getSinceEpochMsec() * 1000; + m_frame872->setRepetitionRate(current_time - m_time_last_ping); + m_time_last_ping = current_time; + } + + } + + void + consume(const IMC::LoggingControl* msg) + { + if (msg->getSource() != getSystemId()) + return; + + switch (msg->op) + { + case IMC::LoggingControl::COP_STARTED: + case IMC::LoggingControl::COP_CURRENT_NAME: + + if(m_data_file.is_open()) + m_data_file.close(); + + m_data_path = m_ctx.dir_log.c_str() + m_ctx.dir_log.extension() + "/" + msg->name +"/" + c_file_name; + + if(isActive()) + m_data_file.open(m_data_path.c_str(), std::ofstream::app | std::ios::binary); + + spew("Openning raw log: %s", m_data_path.c_str()); + + m_receive_log_control = true; + + } + } + void + consume(const IMC::EstimatedState* msg) + { + if (msg->getSource() != getSystemId()) + return; + + m_receive_estimated_state = true; + + if(m_frame872 != NULL) + m_frame872->setGpsString(*msg); + + } + + void + consume(const IMC::SoundSpeed* msg) + { + if (msg->getSource() != getSystemId()) + return; + + m_sound_speed = msg->value * 10; + + if(m_frame872 != NULL) + m_frame872->setSoundSpeed(m_sound_speed); + } + + void + consume(const IMC::UamTxStatus* msg) + { + + if (msg->getSource() != getSystemId()) + return; + + if (!isActive() || + !m_args.filter_uam) + return; + + if (msg->value == IMC::UamTxStatus::UTS_IP) + { + //Stop + m_acoustic_transmission = true; + m_timer_counter.setTop(m_args.time_filter_uam); + } + else { + //Restart + debug("Finished acoustic transmission."); + m_acoustic_transmission = false; + } + } + + + void + logRawData() + { + if(!m_receive_log_control || + !m_receive_estimated_state) { + debug("Missing some message..."); + return; + } + + m_data_file.write((const char*)m_frame872->getData(), c_ping_size); + } + + + void + task(void) + { + consumeMessages(); + + if (!isActive()) + return; + + try + { + pingBoth(); + dispatch(m_ping); + + if(m_frame872 == NULL) + return; + + if(!m_timer_counter.overflow() || + m_acoustic_transmission) { + debug("Not writting 872 data; acoustic transmission in progress."); + return; + } + + if(!m_data_file.is_open()) + m_data_file.open(m_data_path.c_str(), std::ofstream::app | std::ios::binary); + + logRawData(); + + } + catch (std::exception& e) + { + err("%s", e.what()); + setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); + throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); + } + } + }; + } } DUNE_TASK