From 35f50a9003dd4fbf2091154ee7ccee4fd59ba8dd Mon Sep 17 00:00:00 2001 From: anaezes Date: Thu, 1 Nov 2018 17:00:46 +0000 Subject: [PATCH 01/20] Sensors/Imagenex872: Log raw data into a file. --- src/Sensors/Imagenex872/Task.cpp | 48 +++++++++++++++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 888c2e5e86..4bb6a9b577 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -98,6 +98,8 @@ namespace Sensors static const int c_rdata_dat_size = 1000; // Return data footer size. static const int c_rdata_ftr_size = 1; + // Raw log file name + static const std::string c_file_name = "data.872"; struct Task: public Tasks::Periodic { @@ -113,10 +115,15 @@ namespace Sensors IMC::SonarData m_ping; // Configuration parameters. Arguments m_args; + // Dune context + Context& m_ctx; + // Raw log file + std::ofstream m_data_file; Task(const std::string& name, Tasks::Context& ctx): Tasks::Periodic(name, ctx), - m_sock(NULL) + m_sock(NULL), + m_ctx(ctx) { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_MANEUVER, @@ -174,6 +181,8 @@ namespace Sensors m_ping.type = IMC::SonarData::ST_SIDESCAN; m_ping.bits_per_point = 8; m_ping.scale_factor = 1.0f; + + bind(this); } void @@ -318,9 +327,45 @@ namespace Sensors setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); } + void + consume(const IMC::LoggingControl* msg) + { + + if (!isActive()) + return; + + 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(); + + std::string data_path = m_ctx.dir_log.c_str() + m_ctx.dir_log.extension() + "/" + msg->name +"/" + c_file_name; + m_data_file.open(data_path.c_str(), std::ofstream::app | std::ios::binary); + + spew("Openning raw log: %s", data_path.c_str()); + + } + } + + void + logRawData() + { + for (std::vector::iterator it = m_ping.data.begin() ; it != m_ping.data.end(); ++it) + m_data_file << *it; + } + void task(void) { + + consumeMessages(); + if (!isActive()) return; @@ -328,6 +373,7 @@ namespace Sensors { pingBoth(); dispatch(m_ping); + logRawData(); } catch (std::exception& e) { From 7f7a4b4af0fa361de28de70bc75c43d68170cf88 Mon Sep 17 00:00:00 2001 From: anaezes Date: Mon, 5 Nov 2018 12:04:31 +0000 Subject: [PATCH 02/20] Sensors/Imagenex872: Open file when task is activated after IMC log is open as well. --- src/Sensors/Imagenex872/Task.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 4bb6a9b577..eecb5deca1 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -119,11 +119,14 @@ namespace Sensors Context& m_ctx; // Raw log file std::ofstream m_data_file; + //Raw log path + std::string m_data_path; Task(const std::string& name, Tasks::Context& ctx): Tasks::Periodic(name, ctx), m_sock(NULL), - m_ctx(ctx) + m_ctx(ctx), + m_data_path("") { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_MANEUVER, @@ -331,9 +334,6 @@ namespace Sensors consume(const IMC::LoggingControl* msg) { - if (!isActive()) - return; - if (msg->getSource() != getSystemId()) return; @@ -345,10 +345,12 @@ namespace Sensors if(m_data_file.is_open()) m_data_file.close(); - std::string data_path = m_ctx.dir_log.c_str() + m_ctx.dir_log.extension() + "/" + msg->name +"/" + c_file_name; - m_data_file.open(data_path.c_str(), std::ofstream::app | std::ios::binary); + m_data_path = m_ctx.dir_log.c_str() + m_ctx.dir_log.extension() + "/" + msg->name +"/" + c_file_name; - spew("Openning raw log: %s", data_path.c_str()); + 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()); } } @@ -373,6 +375,10 @@ namespace Sensors { pingBoth(); dispatch(m_ping); + + 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) From 777a643e52fcae7fe64323b761eef812960ae4f0 Mon Sep 17 00:00:00 2001 From: anaezes Date: Mon, 5 Nov 2018 12:05:32 +0000 Subject: [PATCH 03/20] Sensors/Imagenex872: Ensure that file is closed when DUNE stops. --- src/Sensors/Imagenex872/Task.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index eecb5deca1..7b494acfce 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -214,6 +214,9 @@ namespace Sensors onResourceRelease(void) { Memory::clear(m_sock); + + if(m_data_file.is_open()) + m_data_file.close(); } void From edbfbfd12dc5f801b2e665463fede3032576f1c9 Mon Sep 17 00:00:00 2001 From: anaezes Date: Mon, 5 Nov 2018 12:07:02 +0000 Subject: [PATCH 04/20] Sensors/Imagenex872: Improve data writing to file. --- src/Sensors/Imagenex872/Task.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 7b494acfce..3aced5bc2a 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -358,13 +358,17 @@ namespace Sensors } } + void logRawData() { - for (std::vector::iterator it = m_ping.data.begin() ; it != m_ping.data.end(); ++it) - m_data_file << *it; + char data[m_ping.data.size()]; + std::copy(m_ping.data.begin(), m_ping.data.end(), data); + + m_data_file.write(data, m_ping.data.size()); } + void task(void) { From 7d31eab9a98edd14549ba74388db05887c8562a0 Mon Sep 17 00:00:00 2001 From: anaezes Date: Fri, 7 Dec 2018 17:53:49 +0000 Subject: [PATCH 05/20] Sensors/Imagenex872: Log 872 into file (WIP). --- src/Sensors/Imagenex872/Task.cpp | 954 ++++++++++++++++++++----------- 1 file changed, 607 insertions(+), 347 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 3aced5bc2a..74c553af73 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -35,368 +35,628 @@ 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; - // Raw log file name - static const std::string c_file_name = "data.872"; - - 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; - // Dune context - Context& m_ctx; - // Raw log file - std::ofstream m_data_file; - //Raw log path - std::string m_data_path; - - Task(const std::string& name, Tasks::Context& ctx): - Tasks::Periodic(name, ctx), - m_sock(NULL), - m_ctx(ctx), - m_data_path("") - { - // 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; - - 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); - } - - void - onResourceAcquisition(void) - { - m_sock = new TCPSocket(); - m_sock->setNoDelay(true); - } - - void - onResourceRelease(void) - { - Memory::clear(m_sock); - - if(m_data_file.is_open()) - m_data_file.close(); - } - - 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) + SIDE_PORT, + SIDE_STARBOARD + }; + + enum Index { - 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) + // 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 { - 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 - consume(const IMC::LoggingControl* msg) - { - - if (msg->getSource() != getSystemId()) - return; - - switch (msg->op) + // 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 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"; + // Total bytes to each ping + static const uint16_t c_total_bytes_ping = 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 = 3000; + // Event/annotation counter + static const uint16_t c_event_annotation_counter = 0; + // Number of bytes to previous ping + static const uint16_t c_bytes_previous_ping = 8192; + + struct Task: public Tasks::Periodic { - case IMC::LoggingControl::COP_STARTED: - case IMC::LoggingControl::COP_CURRENT_NAME: - - if(m_data_file.is_open()) - m_data_file.close(); + // 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; + // Header to 872 file + uint8_t m_header[c_total_bytes_ping]; + // Ping data + uint8_t m_ping_data[2000]; + // Ping number counter + uint32_t m_ping_number; + // Estimated state + IMC::EstimatedState m_estate; + // Last valid sound speed value + uint16_t m_sound_speed; + // Time stamp of vehicle + time_t m_time_stamp; + // Receive sound speed message + bool m_receive_sound_speed; + // 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; + + Task(const std::string& name, Tasks::Context& ctx): + Tasks::Periodic(name, ctx), + m_sock(NULL), + m_ctx(ctx), + m_data_path(""), + m_ping_number(0), + m_receive_sound_speed(false), + m_receive_estimated_state(false), + m_receive_dev_data_text(false), + m_receive_log_control(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")); + + // 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); + } + + void + onResourceAcquisition(void) + { + m_sock = new TCPSocket(); + m_sock->setNoDelay(true); + } + + void + onResourceRelease(void) + { + Memory::clear(m_sock); + + if(m_data_file.is_open()) + m_data_file.close(); + } + + 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")); + + if (side == SIDE_PORT) + { + inf("SIDE_PORT ..."); + std::memcpy(&m_ping_data[0], &m_ping.data.at(0), c_rdata_dat_size); + } + else { + inf("SIDE_STARBOARD ..."); + std::memcpy(&m_ping_data[c_rdata_dat_size], &m_ping.data.at(c_rdata_dat_size), c_rdata_dat_size); + } + + // 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); + ++m_ping_number; + } + + 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; + m_estate = *msg; + } + + void + consume(const IMC::SoundSpeed* msg) + { + if (msg->getSource() != getSystemId()) + return; + + m_receive_sound_speed = true; + m_sound_speed = msg->value * 10; + } + + void + consume(const IMC::DevDataText* msg) + { + if (msg->getSource() != getSystemId()) + return; + + m_receive_dev_data_text = true; + m_time_stamp = msg->getTimeStamp(); + + } + + std::string + createRMC() + { + double time_reference = Math::round(m_time_stamp); + 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 = m_estate.lat; + double lon = m_estate.lon; + + Coordinates::toWGS84(m_estate, lat, lon); + + std::string lat_nmea = latitudeToNMEA(lat); + std::string lon_nmea = longitudeToNMEA(lon); + + double vel = Math::norm(m_estate.vx, m_estate.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 + getFileData() { + + memset(m_header, '\0', c_total_bytes_ping); + + m_header[0] = '8'; + m_header[1] = '7'; + m_header[2] = '2'; - m_data_path = m_ctx.dir_log.c_str() + m_ctx.dir_log.extension() + "/" + msg->name +"/" + c_file_name; + //File version + m_header[3] = '0'; - 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()); + //Ping number + m_header[4] = m_ping_number >> 24; + m_header[5] = m_ping_number >> 16; + m_header[6] = m_ping_number >> 8; + m_header[7] = m_ping_number & 0xff; - } - } + //Number of bytes that are written to the disk + m_header[8] = c_total_bytes_ping >> 8; + m_header[9] = c_total_bytes_ping & 0xff; - void - logRawData() - { - char data[m_ping.data.size()]; - std::copy(m_ping.data.begin(), m_ping.data.end(), data); - m_data_file.write(data, m_ping.data.size()); - } + //Data points per channel + m_header[10] = c_data_points_channel >> 8; + m_header[11] = c_data_points_channel & 0xff; + + //Bytes per data point - always 1 + m_header[12] = 1; + + //Data point bit depth - always 8 + m_header[13] = 8; + + //Gps type (GPRMC) and number of strings (1) - 00100001 + m_header[14] = 0x21; + + //GPS string file offset - 3000 + m_header[15] = c_gps_string_file_offset >> 8; + m_header[16] = c_gps_string_file_offset & 0xff; + + //Event/Annotation counter + // TODO check if we need this + m_header[17] = 0; + m_header[18] = 0; + + // date + struct tm *tm = localtime(&m_time_stamp); + char date[12]; + strftime(date, sizeof(date), "%d-%b-%Y", tm); + std::memcpy(&m_header[19], &date, 12); + m_header[30] = '\0'; + inf("date %s", date); + + // time + char time[9]; + strftime(time, sizeof(time), "%H:%M:%S", tm); + std::memcpy(&m_header[31], &time, 9); + m_header[39] = '\0'; + inf("time %s", time); + + // Thousandths of seconds + float value = m_time_stamp/1000.0; + uint8_t *bytes; + bytes = reinterpret_cast(&value); + m_header[40] = bytes[3]; + m_header[41] = bytes[2]; + m_header[42] = bytes[1]; + m_header[43] = bytes[0]; + m_header[44] = '\0'; - void - task(void) - { - - consumeMessages(); - - if (!isActive()) - return; - - try - { - pingBoth(); - dispatch(m_ping); - - 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); - } - } - }; - } + inf("seconds: %f",value); + inf("seconds 0: %02x", bytes[0]); + inf("seconds 1: %02x", bytes[1]); + inf("seconds 2: %02x", bytes[2]); + inf("seconds 3: %02x", bytes[3]); + + //Operating frequency + m_header[45] = 0x02; //TODO don't hardcode + + //Range index + m_header[46] = 0x07; //TODO don't hardcode + + //Data gain + m_header[47] = 0x14; //TODO don't hardcode + + //Channel balance + m_header[48] = 0x1e; //TODO don't hardcode + + //Repetition rate (time between pings) + m_header[49] = m_args.frequency >> 8; + m_header[50] = m_args.frequency & 0xff; + + //Sound velocity + m_header[51] = m_sound_speed >> 8; + m_header[52] = m_sound_speed & 0xff ; + + // 12 Byte file header + std::memcpy(&m_header[53], &m_rdata_hdr, 12); + + // Reserved always 0 + m_header[66] = 0; + m_header[67] = 0; + m_header[68] = 0; + m_header[69] = 0; + + //Sonar type + m_header[70] = 1; + + //Real range + m_header[71] = 0x07; //TODO don't hardcode + + // 0's fill 1 + char zeroFill1[928]; + memset(zeroFill1, '\0', 928); + std::memcpy(&m_header[72], &zeroFill1, 928); + + // PORT & STARBOARD channel echo data fixme + std::memcpy(&m_header[1000], &m_ping_data, c_rdata_dat_size * 2); + + + // GPS Strings + char gpsString[100]; + memset(gpsString, 0, 100); + std::string sentence = createRMC(); + sprintf(gpsString, "%s", sentence.c_str()); + std::memcpy(&m_header[3000], &gpsString, 100); + + // 0's fill 2 + char zeroFill2[993]; + memset(zeroFill2, '\0' , 993); + std::memcpy(&m_header[993], &zeroFill2, 993); + + // previous ping + //m_header[4094] = c_bytes_previous_ping; + m_header[4094] = c_bytes_previous_ping >> 8; + m_header[4095] = c_bytes_previous_ping & 0xFF; + } + + void + logRawData() + { + if(!m_receive_dev_data_text || + !m_receive_sound_speed || + !m_receive_log_control || + !m_receive_estimated_state) { + debug("Missing some message..."); + return; + } + + getFileData(); + m_data_file.write((const char*)m_header, c_total_bytes_ping); + } + + + void + task(void) + { + consumeMessages(); + + if (!isActive()) + return; + + try + { + pingBoth(); + dispatch(m_ping); + + 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 From 6ea80645bec43f472fa2aa55d306e45cd466fcf5 Mon Sep 17 00:00:00 2001 From: anaezes Date: Tue, 11 Dec 2018 10:14:00 +0000 Subject: [PATCH 06/20] Sensors/Imagenex872: Write zero fill in the correct position. --- src/Sensors/Imagenex872/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 74c553af73..8529e73ff9 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -606,7 +606,7 @@ namespace Sensors // 0's fill 2 char zeroFill2[993]; memset(zeroFill2, '\0' , 993); - std::memcpy(&m_header[993], &zeroFill2, 993); + std::memcpy(&m_header[3100], &zeroFill2, 993); // previous ping //m_header[4094] = c_bytes_previous_ping; From 47ed11ee482ba23bd043a31beb565cb867e0081b Mon Sep 17 00:00:00 2001 From: anaezes Date: Tue, 11 Dec 2018 10:14:36 +0000 Subject: [PATCH 07/20] Sensors/Imagenex872: Improve writing of data pings. --- src/Sensors/Imagenex872/Task.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 8529e73ff9..9bab8a76e0 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -351,16 +351,6 @@ namespace Sensors if (rv != c_rdata_ftr_size) throw std::runtime_error(DTR("failed to read footer")); - if (side == SIDE_PORT) - { - inf("SIDE_PORT ..."); - std::memcpy(&m_ping_data[0], &m_ping.data.at(0), c_rdata_dat_size); - } - else { - inf("SIDE_STARBOARD ..."); - std::memcpy(&m_ping_data[c_rdata_dat_size], &m_ping.data.at(c_rdata_dat_size), c_rdata_dat_size); - } - // Correct port imagery. if (side == SIDE_PORT) { @@ -380,6 +370,7 @@ namespace Sensors ping(SIDE_STARBOARD); setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); ++m_ping_number; + std::memcpy(&m_ping_data[0], &m_ping.data.at(0), c_rdata_dat_size*2); } void From f207bea28e94799fe323b9601b549d468a2d040f Mon Sep 17 00:00:00 2001 From: anaezes Date: Mon, 21 Jan 2019 18:04:27 +0000 Subject: [PATCH 08/20] Sensors/Imagenex872: Added sound speed by default. --- src/Sensors/Imagenex872/Task.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 9bab8a76e0..2d192eed62 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -143,8 +143,6 @@ namespace Sensors uint16_t m_sound_speed; // Time stamp of vehicle time_t m_time_stamp; - // Receive sound speed message - bool m_receive_sound_speed; // Receive estimated state message bool m_receive_estimated_state; // Receive dev data text message @@ -158,7 +156,7 @@ namespace Sensors m_ctx(ctx), m_data_path(""), m_ping_number(0), - m_receive_sound_speed(false), + m_sound_speed(1500), m_receive_estimated_state(false), m_receive_dev_data_text(false), m_receive_log_control(false) @@ -414,7 +412,6 @@ namespace Sensors if (msg->getSource() != getSystemId()) return; - m_receive_sound_speed = true; m_sound_speed = msg->value * 10; } @@ -609,7 +606,6 @@ namespace Sensors logRawData() { if(!m_receive_dev_data_text || - !m_receive_sound_speed || !m_receive_log_control || !m_receive_estimated_state) { debug("Missing some message..."); From 5dc505df4859ca6a7449ad26bd1991dd7c4562e0 Mon Sep 17 00:00:00 2001 From: anaezes Date: Mon, 21 Jan 2019 18:06:30 +0000 Subject: [PATCH 09/20] Sensors/Imagenex872: Added execution frequency param. --- src/Sensors/Imagenex872/Task.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 2d192eed62..a96408dd47 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -74,6 +74,8 @@ namespace Sensors unsigned frequency; // Default range. unsigned range; + //Execution frequency + unsigned exec_freq; }; // List of available ranges. @@ -206,6 +208,13 @@ namespace Sensors .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")); + // Initialize switch data. std::memset(m_sdata, 0, sizeof(m_sdata)); m_sdata[0] = 0xfe; @@ -541,7 +550,7 @@ namespace Sensors inf("seconds 3: %02x", bytes[3]); //Operating frequency - m_header[45] = 0x02; //TODO don't hardcode + m_header[45] = m_args.exec_freq; //Range index m_header[46] = 0x07; //TODO don't hardcode From 70dee8a0911d60c8d07075145c021369f003b12b Mon Sep 17 00:00:00 2001 From: anaezes Date: Mon, 21 Jan 2019 18:08:17 +0000 Subject: [PATCH 10/20] Sensors/Imagenex872: Non-hardcoded parameters. --- src/Sensors/Imagenex872/Task.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index a96408dd47..5d81065e3b 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -553,13 +553,13 @@ namespace Sensors m_header[45] = m_args.exec_freq; //Range index - m_header[46] = 0x07; //TODO don't hardcode + m_header[46] = 0x07; //Data gain - m_header[47] = 0x14; //TODO don't hardcode + m_header[47] = m_args.dat_gain; - //Channel balance - m_header[48] = 0x1e; //TODO don't hardcode + //Channel balance (balance gain) + m_header[48] = m_args.bal_gain; //Repetition rate (time between pings) m_header[49] = m_args.frequency >> 8; @@ -579,20 +579,19 @@ namespace Sensors m_header[69] = 0; //Sonar type - m_header[70] = 1; + m_header[70] = 0; //Real range - m_header[71] = 0x07; //TODO don't hardcode + m_header[71] = m_rdata_hdr[4]; // 0's fill 1 char zeroFill1[928]; memset(zeroFill1, '\0', 928); std::memcpy(&m_header[72], &zeroFill1, 928); - // PORT & STARBOARD channel echo data fixme + // PORT & STARBOARD channel echo data std::memcpy(&m_header[1000], &m_ping_data, c_rdata_dat_size * 2); - // GPS Strings char gpsString[100]; memset(gpsString, 0, 100); From bcfca3b4332379667b896cb22b728b09d1bfba6f Mon Sep 17 00:00:00 2001 From: anaezes Date: Mon, 21 Jan 2019 18:10:12 +0000 Subject: [PATCH 11/20] Sensors/Imagenex872: Milliseconds recorded according to specification. --- src/Sensors/Imagenex872/Task.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 5d81065e3b..f0cab04f73 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -534,20 +534,19 @@ namespace Sensors inf("time %s", time); // Thousandths of seconds - float value = m_time_stamp/1000.0; - uint8_t *bytes; - bytes = reinterpret_cast(&value); - m_header[40] = bytes[3]; - m_header[41] = bytes[2]; - m_header[42] = bytes[1]; - m_header[43] = bytes[0]; - m_header[44] = '\0'; + float n = m_time_stamp / 1000.0; + double whole; + + char buffer[3]; + int ms = modf(n, &whole) * 1000; - inf("seconds: %f",value); - inf("seconds 0: %02x", bytes[0]); - inf("seconds 1: %02x", bytes[1]); - inf("seconds 2: %02x", bytes[2]); - inf("seconds 3: %02x", bytes[3]); + sprintf(buffer,"%03d", ms); + + m_header[40] = '.'; + m_header[41] = buffer[0]; + m_header[42] = buffer[1]; + m_header[43] = buffer[2]; + m_header[44] = '\0'; //Operating frequency m_header[45] = m_args.exec_freq; From a49d5bdcf48828437083129206f8b346ceda8208 Mon Sep 17 00:00:00 2001 From: anaezes Date: Tue, 29 Jan 2019 14:32:06 +0000 Subject: [PATCH 12/20] Sensors/Imagenex872: Fix GPS offset. --- src/Sensors/Imagenex872/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index f0cab04f73..95dc1e92f9 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -107,7 +107,7 @@ namespace Sensors // 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 = 3000; + static const uint16_t c_gps_string_file_offset = 3200; // Event/annotation counter static const uint16_t c_event_annotation_counter = 0; // Number of bytes to previous ping From 37cdaafb488abbdf0c1fd6964b14dc18c15c552a Mon Sep 17 00:00:00 2001 From: anaezes Date: Tue, 29 Jan 2019 14:32:31 +0000 Subject: [PATCH 13/20] Sensors/Imagenex872: Code cleanup. --- src/Sensors/Imagenex872/Task.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 95dc1e92f9..1aa29342e3 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -413,6 +413,7 @@ namespace Sensors m_receive_estimated_state = true; m_estate = *msg; + } void @@ -482,7 +483,6 @@ namespace Sensors //File version m_header[3] = '0'; - //Ping number m_header[4] = m_ping_number >> 24; m_header[5] = m_ping_number >> 16; @@ -604,7 +604,6 @@ namespace Sensors std::memcpy(&m_header[3100], &zeroFill2, 993); // previous ping - //m_header[4094] = c_bytes_previous_ping; m_header[4094] = c_bytes_previous_ping >> 8; m_header[4095] = c_bytes_previous_ping & 0xFF; } @@ -637,10 +636,12 @@ namespace Sensors pingBoth(); dispatch(m_ping); + 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) { From ea863430b054a14aa6bc5dab3fc6ec3ac71584c8 Mon Sep 17 00:00:00 2001 From: anaezes Date: Tue, 29 Jan 2019 15:08:39 +0000 Subject: [PATCH 14/20] Sensors/Imagenex872: Get system time instead time of dev_data_text msg. --- src/Sensors/Imagenex872/Task.cpp | 25 +++++-------------------- 1 file changed, 5 insertions(+), 20 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 1aa29342e3..1086b22540 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -144,7 +144,7 @@ namespace Sensors // Last valid sound speed value uint16_t m_sound_speed; // Time stamp of vehicle - time_t m_time_stamp; + long m_time_stamp; // Receive estimated state message bool m_receive_estimated_state; // Receive dev data text message @@ -229,7 +229,6 @@ namespace Sensors bind(this); bind(this); - bind(this); bind(this); } @@ -425,17 +424,6 @@ namespace Sensors m_sound_speed = msg->value * 10; } - void - consume(const IMC::DevDataText* msg) - { - if (msg->getSource() != getSystemId()) - return; - - m_receive_dev_data_text = true; - m_time_stamp = msg->getTimeStamp(); - - } - std::string createRMC() { @@ -519,6 +507,7 @@ namespace Sensors m_header[18] = 0; // date + m_time_stamp = Clock::getSinceEpochMsec(); struct tm *tm = localtime(&m_time_stamp); char date[12]; strftime(date, sizeof(date), "%d-%b-%Y", tm); @@ -534,13 +523,10 @@ namespace Sensors inf("time %s", time); // Thousandths of seconds - float n = m_time_stamp / 1000.0; - double whole; + int n = m_time_stamp % 1000; char buffer[3]; - int ms = modf(n, &whole) * 1000; - - sprintf(buffer,"%03d", ms); + sprintf(buffer,"%03d", n); m_header[40] = '.'; m_header[41] = buffer[0]; @@ -611,8 +597,7 @@ namespace Sensors void logRawData() { - if(!m_receive_dev_data_text || - !m_receive_log_control || + if(!m_receive_log_control || !m_receive_estimated_state) { debug("Missing some message..."); return; From b1da59a637a1cbf60680b2469544f90fe8d9b17f Mon Sep 17 00:00:00 2001 From: anaezes Date: Wed, 30 Jan 2019 19:47:40 +0000 Subject: [PATCH 15/20] Sensors/Imagenex872: Create a frame to write to a 872 file. --- src/Sensors/Imagenex872/Frame872.hpp | 269 +++++++++++++++++++++++++++ src/Sensors/Imagenex872/Task.cpp | 237 +++++------------------ 2 files changed, 312 insertions(+), 194 deletions(-) create mode 100644 src/Sensors/Imagenex872/Frame872.hpp diff --git a/src/Sensors/Imagenex872/Frame872.hpp b/src/Sensors/Imagenex872/Frame872.hpp new file mode 100644 index 0000000000..1c76b0fdd8 --- /dev/null +++ b/src/Sensors/Imagenex872/Frame872.hpp @@ -0,0 +1,269 @@ +//*************************************************************************** +// 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; + + //Range index + m_data[46] = 0x07; + + // 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]; + } + + //todo m_ping_number + 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 frequency){ + m_data[49] = frequency >> 8; + m_data[50] = frequency & 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(); + } + + 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 1086b22540..5920ad1c45 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -32,6 +32,7 @@ // DUNE headers. #include +#include "Frame872.hpp" namespace Sensors { @@ -101,17 +102,8 @@ namespace Sensors // 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"; - // Total bytes to each ping - static const uint16_t c_total_bytes_ping = 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; - // Event/annotation counter - static const uint16_t c_event_annotation_counter = 0; - // Number of bytes to previous ping - static const uint16_t c_bytes_previous_ping = 8192; + static const std::string c_file_name = "Data.872"; + struct Task: public Tasks::Periodic { @@ -133,18 +125,14 @@ namespace Sensors std::ofstream m_data_file; // Raw log path std::string m_data_path; - // Header to 872 file - uint8_t m_header[c_total_bytes_ping]; - // Ping data - uint8_t m_ping_data[2000]; - // Ping number counter + //! 872 Frame. + Frame872* m_frame872; + //! Ping counter uint32_t m_ping_number; - // Estimated state - IMC::EstimatedState m_estate; - // Last valid sound speed value + + //! Last valid sound speed value uint16_t m_sound_speed; - // Time stamp of vehicle - long m_time_stamp; + // Receive estimated state message bool m_receive_estimated_state; // Receive dev data text message @@ -157,6 +145,7 @@ namespace Sensors m_sock(NULL), m_ctx(ctx), m_data_path(""), + m_frame872(NULL), m_ping_number(0), m_sound_speed(1500), m_receive_estimated_state(false), @@ -245,6 +234,14 @@ namespace Sensors 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); + m_frame872->setRepetitionRate(m_args.frequency); } void @@ -252,6 +249,12 @@ namespace Sensors { 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); + m_frame872->setRepetitionRate(m_args.frequency); } void @@ -261,6 +264,8 @@ namespace Sensors if(m_data_file.is_open()) m_data_file.close(); + + Memory::clear(m_frame872); } void @@ -375,8 +380,15 @@ namespace Sensors ping(SIDE_PORT); ping(SIDE_STARBOARD); setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); - ++m_ping_number; - std::memcpy(&m_ping_data[0], &m_ping.data.at(0), c_rdata_dat_size*2); + + 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]); + } + } void @@ -411,7 +423,9 @@ namespace Sensors return; m_receive_estimated_state = true; - m_estate = *msg; + + if(m_frame872 != NULL) + m_frame872->setGpsString(*msg); } @@ -422,177 +436,11 @@ namespace Sensors return; m_sound_speed = msg->value * 10; - } - std::string - createRMC() - { - double time_reference = Math::round(m_time_stamp); - 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 = m_estate.lat; - double lon = m_estate.lon; - - Coordinates::toWGS84(m_estate, lat, lon); - - std::string lat_nmea = latitudeToNMEA(lat); - std::string lon_nmea = longitudeToNMEA(lon); - - double vel = Math::norm(m_estate.vx, m_estate.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(); + if(m_frame872 != NULL) + m_frame872->setSoundSpeed(m_sound_speed); } - void - getFileData() { - - memset(m_header, '\0', c_total_bytes_ping); - - m_header[0] = '8'; - m_header[1] = '7'; - m_header[2] = '2'; - - //File version - m_header[3] = '0'; - - //Ping number - m_header[4] = m_ping_number >> 24; - m_header[5] = m_ping_number >> 16; - m_header[6] = m_ping_number >> 8; - m_header[7] = m_ping_number & 0xff; - - - //Number of bytes that are written to the disk - m_header[8] = c_total_bytes_ping >> 8; - m_header[9] = c_total_bytes_ping & 0xff; - - - //Data points per channel - m_header[10] = c_data_points_channel >> 8; - m_header[11] = c_data_points_channel & 0xff; - - - //Bytes per data point - always 1 - m_header[12] = 1; - - //Data point bit depth - always 8 - m_header[13] = 8; - - //Gps type (GPRMC) and number of strings (1) - 00100001 - m_header[14] = 0x21; - - //GPS string file offset - 3000 - m_header[15] = c_gps_string_file_offset >> 8; - m_header[16] = c_gps_string_file_offset & 0xff; - - //Event/Annotation counter - // TODO check if we need this - m_header[17] = 0; - m_header[18] = 0; - - // date - m_time_stamp = Clock::getSinceEpochMsec(); - struct tm *tm = localtime(&m_time_stamp); - char date[12]; - strftime(date, sizeof(date), "%d-%b-%Y", tm); - std::memcpy(&m_header[19], &date, 12); - m_header[30] = '\0'; - inf("date %s", date); - - // time - char time[9]; - strftime(time, sizeof(time), "%H:%M:%S", tm); - std::memcpy(&m_header[31], &time, 9); - m_header[39] = '\0'; - inf("time %s", time); - - // Thousandths of seconds - int n = m_time_stamp % 1000; - - char buffer[3]; - sprintf(buffer,"%03d", n); - - m_header[40] = '.'; - m_header[41] = buffer[0]; - m_header[42] = buffer[1]; - m_header[43] = buffer[2]; - m_header[44] = '\0'; - - //Operating frequency - m_header[45] = m_args.exec_freq; - - //Range index - m_header[46] = 0x07; - - //Data gain - m_header[47] = m_args.dat_gain; - - //Channel balance (balance gain) - m_header[48] = m_args.bal_gain; - - //Repetition rate (time between pings) - m_header[49] = m_args.frequency >> 8; - m_header[50] = m_args.frequency & 0xff; - - //Sound velocity - m_header[51] = m_sound_speed >> 8; - m_header[52] = m_sound_speed & 0xff ; - - // 12 Byte file header - std::memcpy(&m_header[53], &m_rdata_hdr, 12); - - // Reserved always 0 - m_header[66] = 0; - m_header[67] = 0; - m_header[68] = 0; - m_header[69] = 0; - - //Sonar type - m_header[70] = 0; - - //Real range - m_header[71] = m_rdata_hdr[4]; - - // 0's fill 1 - char zeroFill1[928]; - memset(zeroFill1, '\0', 928); - std::memcpy(&m_header[72], &zeroFill1, 928); - - // PORT & STARBOARD channel echo data - std::memcpy(&m_header[1000], &m_ping_data, c_rdata_dat_size * 2); - - // GPS Strings - char gpsString[100]; - memset(gpsString, 0, 100); - std::string sentence = createRMC(); - sprintf(gpsString, "%s", sentence.c_str()); - std::memcpy(&m_header[3000], &gpsString, 100); - - // 0's fill 2 - char zeroFill2[993]; - memset(zeroFill2, '\0' , 993); - std::memcpy(&m_header[3100], &zeroFill2, 993); - - // previous ping - m_header[4094] = c_bytes_previous_ping >> 8; - m_header[4095] = c_bytes_previous_ping & 0xFF; - } void logRawData() @@ -603,8 +451,7 @@ namespace Sensors return; } - getFileData(); - m_data_file.write((const char*)m_header, c_total_bytes_ping); + m_data_file.write((const char*)m_frame872->getData(), c_ping_size); } @@ -621,6 +468,8 @@ namespace Sensors pingBoth(); dispatch(m_ping); + if(m_frame872 == NULL) + return; if(!m_data_file.is_open()) m_data_file.open(m_data_path.c_str(), std::ofstream::app | std::ios::binary); From 80bd3252c3d042bbc94bb374f6ec12bd64551a46 Mon Sep 17 00:00:00 2001 From: anaezes Date: Wed, 6 Feb 2019 14:04:48 +0000 Subject: [PATCH 16/20] Sensors/Imagenex872: Fixed bug on parameter of time between pings. --- src/Sensors/Imagenex872/Frame872.hpp | 62 +++++++++++++++++++++++----- src/Sensors/Imagenex872/Task.cpp | 11 +++-- 2 files changed, 60 insertions(+), 13 deletions(-) diff --git a/src/Sensors/Imagenex872/Frame872.hpp b/src/Sensors/Imagenex872/Frame872.hpp index 1c76b0fdd8..39420455ae 100644 --- a/src/Sensors/Imagenex872/Frame872.hpp +++ b/src/Sensors/Imagenex872/Frame872.hpp @@ -88,16 +88,13 @@ namespace Sensors m_data[17] = 0; m_data[18] = 0; - //Range index - m_data[46] = 0x07; - // Reserved always 0 m_data[66] = 0; m_data[67] = 0; m_data[68] = 0; m_data[69] = 0; - //Sonar type + // Sonar type m_data[70] = 0; // 0's fill 1 @@ -110,7 +107,7 @@ namespace Sensors memset(zeroFill2, '\0' , 993); std::memcpy(&m_data[3100], &zeroFill2, 993); - // previous ping + // Previous ping m_data[4094] = c_bytes_previous_ping >> 8; m_data[4095] = c_bytes_previous_ping & 0xFF; @@ -127,9 +124,8 @@ namespace Sensors return &m_data[0]; } - //todo m_ping_number void - setPingNumber(uint32_t ping_number){ + (uint32_t ping_number){ m_data[4] = ping_number >> 24; m_data[5] = ping_number >> 16; m_data[6] = ping_number >> 8; @@ -184,9 +180,9 @@ namespace Sensors // Repetition rate (time between pings) void - setRepetitionRate(unsigned frequency){ - m_data[49] = frequency >> 8; - m_data[50] = frequency & 0xff; + setRepetitionRate(unsigned time_between_pings){ + m_data[49] = time_between_pings >> 8; + m_data[50] = time_between_pings & 0xff; } // Sound velocity @@ -259,6 +255,52 @@ namespace Sensors return stn.sentence(); } + void + setRangeIndex(const uint32_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; diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 5920ad1c45..079b474d8a 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -129,9 +129,10 @@ namespace Sensors 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; @@ -148,6 +149,7 @@ namespace Sensors 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) @@ -241,7 +243,6 @@ namespace Sensors m_frame872->setOperationFrequency(m_args.exec_freq); m_frame872->setDataGain(m_args.dat_gain); m_frame872->setBalanceGain(m_args.bal_gain); - m_frame872->setRepetitionRate(m_args.frequency); } void @@ -254,7 +255,6 @@ namespace Sensors m_frame872->setOperationFrequency(m_args.exec_freq); m_frame872->setDataGain(m_args.dat_gain); m_frame872->setBalanceGain(m_args.bal_gain); - m_frame872->setRepetitionRate(m_args.frequency); } void @@ -387,6 +387,11 @@ namespace Sensors 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; } } From ddf51e3f1daa4cbeee6fb9121e07ca990fd18ca2 Mon Sep 17 00:00:00 2001 From: anaezes Date: Wed, 6 Feb 2019 14:15:08 +0000 Subject: [PATCH 17/20] Sensors/Imagenex872: Fixed bug of last commit. --- src/Sensors/Imagenex872/Frame872.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Sensors/Imagenex872/Frame872.hpp b/src/Sensors/Imagenex872/Frame872.hpp index 39420455ae..ea4c43ce94 100644 --- a/src/Sensors/Imagenex872/Frame872.hpp +++ b/src/Sensors/Imagenex872/Frame872.hpp @@ -125,7 +125,7 @@ namespace Sensors } void - (uint32_t ping_number){ + setPingNumber(uint32_t ping_number){ m_data[4] = ping_number >> 24; m_data[5] = ping_number >> 16; m_data[6] = ping_number >> 8; @@ -256,7 +256,7 @@ namespace Sensors } void - setRangeIndex(const uint32_t range) { + setRangeIndex(const uint8_t range) { uint8_t index = 0x07; From e7bcd7e036040557211f1112e281e6cc8c1d7d96 Mon Sep 17 00:00:00 2001 From: anaezes Date: Wed, 13 Mar 2019 16:43:15 +0000 Subject: [PATCH 18/20] Sensors/Imagenex872: Stop logging while the vehicle is transmitting acoustically. --- src/Sensors/Imagenex872/Task.cpp | 57 ++++++++++++++++++++++++++++++-- 1 file changed, 55 insertions(+), 2 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 079b474d8a..8f293ea97e 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -75,8 +75,12 @@ namespace Sensors unsigned frequency; // Default range. unsigned range; - //Execution frequency + // 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. @@ -140,6 +144,10 @@ namespace Sensors bool m_receive_dev_data_text; // Receive logging control message bool m_receive_log_control; + // Vehicle transmission uam + bool m_transmission_uam; + // Timer to break pings between acoustic operations + Time::Counter m_timer_counter; Task(const std::string& name, Tasks::Context& ctx): Tasks::Periodic(name, ctx), @@ -152,7 +160,8 @@ namespace Sensors m_time_last_ping(Clock::getSinceEpochMsec() * 1000), m_receive_estimated_state(false), m_receive_dev_data_text(false), - m_receive_log_control(false) + m_receive_log_control(false), + m_transmission_uam(false) { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_MANEUVER, @@ -206,6 +215,19 @@ namespace Sensors .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; @@ -221,6 +243,7 @@ namespace Sensors bind(this); bind(this); bind(this); + bind(this); } void @@ -446,6 +469,33 @@ namespace Sensors m_frame872->setSoundSpeed(m_sound_speed); } + void + consume(const IMC::UamTxStatus* msg) + { + + if(!m_args.filter_uam) + return; + + if (msg->getSource() != getSystemId()) + return; + + debug("Transmission of acoustic message..."); + + if (msg->value == IMC::UamTxStatus::UTS_IP) + { + //Stop + debug("Acoustic communication start"); + debug("Stop writing pings between %f s", m_args.time_filter_uam); + m_transmission_uam = true; + m_timer_counter.setTop(m_args.time_filter_uam); + } + else { + //Restart + debug("Acoustic communication stop."); + m_transmission_uam = false; + } + } + void logRawData() @@ -476,6 +526,9 @@ namespace Sensors if(m_frame872 == NULL) return; + if(!m_timer_counter.overflow() || m_transmission_uam) + return; + if(!m_data_file.is_open()) m_data_file.open(m_data_path.c_str(), std::ofstream::app | std::ios::binary); From 8c932a0550463403c893de43c48a5bbe05c1ded7 Mon Sep 17 00:00:00 2001 From: anaezes Date: Thu, 14 Mar 2019 12:57:40 +0000 Subject: [PATCH 19/20] Sensors/Imagenex872: Improvements in debug messages. --- src/Sensors/Imagenex872/Task.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 8f293ea97e..05c0e28eec 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -473,25 +473,24 @@ namespace Sensors consume(const IMC::UamTxStatus* msg) { + if (!isActive()) + return; + if(!m_args.filter_uam) return; if (msg->getSource() != getSystemId()) return; - debug("Transmission of acoustic message..."); - if (msg->value == IMC::UamTxStatus::UTS_IP) { //Stop - debug("Acoustic communication start"); - debug("Stop writing pings between %f s", m_args.time_filter_uam); m_transmission_uam = true; m_timer_counter.setTop(m_args.time_filter_uam); } else { //Restart - debug("Acoustic communication stop."); + debug("Transmission of acoustic message stop."); m_transmission_uam = false; } } @@ -526,8 +525,11 @@ namespace Sensors if(m_frame872 == NULL) return; - if(!m_timer_counter.overflow() || m_transmission_uam) + if(!m_timer_counter.overflow() || m_transmission_uam) { + debug("Transmission of acoustic message"); + debug("Stop write pings in file"); return; + } if(!m_data_file.is_open()) m_data_file.open(m_data_path.c_str(), std::ofstream::app | std::ios::binary); From 8b425feb80d3daf68a50752f4ec9f4a085d14984 Mon Sep 17 00:00:00 2001 From: anaezes Date: Fri, 15 Mar 2019 11:04:51 +0000 Subject: [PATCH 20/20] Sensors/Imagenex872: Cleanups. --- src/Sensors/Imagenex872/Task.cpp | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 05c0e28eec..98ff761ea1 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -144,8 +144,8 @@ namespace Sensors bool m_receive_dev_data_text; // Receive logging control message bool m_receive_log_control; - // Vehicle transmission uam - bool m_transmission_uam; + // If an acoustic message is being transmitted + bool m_acoustic_transmission; // Timer to break pings between acoustic operations Time::Counter m_timer_counter; @@ -161,7 +161,7 @@ namespace Sensors m_receive_estimated_state(false), m_receive_dev_data_text(false), m_receive_log_control(false), - m_transmission_uam(false) + m_acoustic_transmission(false) { // Define configuration parameters. paramActive(Tasks::Parameter::SCOPE_MANEUVER, @@ -473,25 +473,23 @@ namespace Sensors consume(const IMC::UamTxStatus* msg) { - if (!isActive()) - return; - - if(!m_args.filter_uam) + if (msg->getSource() != getSystemId()) return; - if (msg->getSource() != getSystemId()) + if (!isActive() || + !m_args.filter_uam) return; if (msg->value == IMC::UamTxStatus::UTS_IP) { //Stop - m_transmission_uam = true; + m_acoustic_transmission = true; m_timer_counter.setTop(m_args.time_filter_uam); } else { //Restart - debug("Transmission of acoustic message stop."); - m_transmission_uam = false; + debug("Finished acoustic transmission."); + m_acoustic_transmission = false; } } @@ -525,9 +523,9 @@ namespace Sensors if(m_frame872 == NULL) return; - if(!m_timer_counter.overflow() || m_transmission_uam) { - debug("Transmission of acoustic message"); - debug("Stop write pings in file"); + if(!m_timer_counter.overflow() || + m_acoustic_transmission) { + debug("Not writting 872 data; acoustic transmission in progress."); return; }