From fe0f70607a5bbaff5d00992ee835468f1c216433 Mon Sep 17 00:00:00 2001 From: Riley Tuttle Date: Tue, 14 Apr 2026 08:05:48 -0400 Subject: [PATCH 1/2] updating ping protocol library for surveyor --- lib/ping-protocol | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/ping-protocol b/lib/ping-protocol index ac67fcb..0385552 160000 --- a/lib/ping-protocol +++ b/lib/ping-protocol @@ -1 +1 @@ -Subproject commit ac67fcb72ae22cf9cec2fa2fe1a3061b947b72d0 +Subproject commit 0385552b3774c834e0c3b4159aefa165cb78df50 From 00eaf9a7c41a2d477591e408fc1cd944faca73a2 Mon Sep 17 00:00:00 2001 From: Riley Tuttle Date: Tue, 14 Apr 2026 08:08:25 -0400 Subject: [PATCH 2/2] minimally working surveyor communication --- CMakeLists.txt | 18 + src/device/CMakeLists.txt | 2 + src/device/ping-device-surveyor240.cpp | 154 +++++ src/device/ping-device-surveyor240.h | 203 ++++++ src/device/ping-device.cpp | 24 +- src/generate/generate-device.py | 10 +- src/generate/generate-message.py | 12 +- .../templates/ping-device-surveyor240.cpp.in | 70 ++ .../templates/ping-device-surveyor240.h.in | 77 +++ src/generate/templates/ping-message-.h.in | 6 +- src/generate/templates/ping-message-all.h.in | 1 + .../templates/ping-message-surveyor240.h.in | 148 ++++ src/message/ping-message-surveyor240.h | 635 ++++++++++++++++++ test/test-device-surveyor240.cpp | 58 ++ test/test-device.cpp | 2 +- 15 files changed, 1410 insertions(+), 10 deletions(-) create mode 100644 src/device/ping-device-surveyor240.cpp create mode 100644 src/device/ping-device-surveyor240.h create mode 100644 src/generate/templates/ping-device-surveyor240.cpp.in create mode 100644 src/generate/templates/ping-device-surveyor240.h.in create mode 100644 src/generate/templates/ping-message-surveyor240.h.in create mode 100644 src/message/ping-message-surveyor240.h create mode 100644 test/test-device-surveyor240.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8498a9d..9a6298d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,24 @@ if (PING_CPP_BUILD_TESTS) fmt::fmt ) + # test surveyor240 + add_executable(test-device-surveyor240) + target_include_directories(test-device-surveyor240 PRIVATE lib/fmt/include/) + target_sources( + test-device-surveyor240 + PRIVATE + test/test-device-surveyor240.cpp + ) + target_link_libraries( + test-device-surveyor240 + PRIVATE + DEVICE + HAL + PING_MESSAGES + ${Boost_LIBRARIES} + fmt::fmt + ) + # test-message add_executable(test-message) target_include_directories(test-message PRIVATE lib/fmt/include/) diff --git a/src/device/CMakeLists.txt b/src/device/CMakeLists.txt index 7d54806..92f3bfd 100644 --- a/src/device/CMakeLists.txt +++ b/src/device/CMakeLists.txt @@ -8,6 +8,7 @@ add_custom_target(generate-devices BYPRODUCTS ${CMAKE_CURRENT_SOURCE_DIR}/ping-device-ping360.cpp ${CMAKE_CURRENT_SOURCE_DIR}/ping-device-ping1d.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/ping-device-surveyor240.cpp ) add_library( @@ -16,6 +17,7 @@ STATIC ping-device-ping1d.cpp ping-device-ping360.cpp ping-device.cpp + ping-device-surveyor240.cpp ) include_directories(${HAL_SOURCE_DIR}) diff --git a/src/device/ping-device-surveyor240.cpp b/src/device/ping-device-surveyor240.cpp new file mode 100644 index 0000000..9c90523 --- /dev/null +++ b/src/device/ping-device-surveyor240.cpp @@ -0,0 +1,154 @@ +#include +#include "ping-message-surveyor240.h" +#include "ping-device-surveyor240.h" + +Surveyor240::~Surveyor240() +{ + if (atof_point_data_data.atof_point_data) { + delete[] atof_point_data_data.atof_point_data; + } + if (yz_point_data_data.yz_point_data) { + delete[] yz_point_data_data.yz_point_data; + } +} + +bool Surveyor240::initialize() +{ + if (!PingDevice::initialize()) { + return false; + } + return true; +} + +void Surveyor240::_handleMessage(const ping_message* message) +{ + switch (message->message_id()) { + case Surveyor240Id::ATOF_POINT_DATA: + { + const surveyor240_atof_point_data* message_atof_point_data = static_cast(message); + atof_point_data_data.pwr_up_msec = message_atof_point_data->pwr_up_msec(); + atof_point_data_data.utc_msec = message_atof_point_data->utc_msec(); + atof_point_data_data.listening_sec = message_atof_point_data->listening_sec(); + atof_point_data_data.sos_mps = message_atof_point_data->sos_mps(); + atof_point_data_data.ping_number = message_atof_point_data->ping_number(); + atof_point_data_data.ping_hz = message_atof_point_data->ping_hz(); + atof_point_data_data.pulse_sec = message_atof_point_data->pulse_sec(); + atof_point_data_data.flags = message_atof_point_data->flags(); + atof_point_data_data.num_points = message_atof_point_data->num_points(); + atof_point_data_data.reserved = message_atof_point_data->reserved(); + if (message_atof_point_data->num_points() > atof_point_data_data.atof_point_data_length) { + if (atof_point_data_data.atof_point_data) { + delete[] atof_point_data_data.atof_point_data; + } + atof_point_data_data.atof_point_data = new atof_t[message_atof_point_data->num_points()]; + } + atof_point_data_data.atof_point_data_length = message_atof_point_data->num_points(); + memcpy(atof_point_data_data.atof_point_data, message_atof_point_data->atof_point_data(), message_atof_point_data->num_points()); + } + break; + case Surveyor240Id::ATTITUDE_REPORT: + { + const surveyor240_attitude_report* message_attitude_report = static_cast(message); + attitude_report_data.up_vec_x = message_attitude_report->up_vec_x(); + attitude_report_data.up_vec_y = message_attitude_report->up_vec_y(); + attitude_report_data.up_vec_z = message_attitude_report->up_vec_z(); + attitude_report_data.reserved_1 = message_attitude_report->reserved_1(); + attitude_report_data.reserved_2 = message_attitude_report->reserved_2(); + attitude_report_data.reserved_3 = message_attitude_report->reserved_3(); + attitude_report_data.utc_msec = message_attitude_report->utc_msec(); + attitude_report_data.pwr_up_msec = message_attitude_report->pwr_up_msec(); + } + break; + case Surveyor240Id::WATER_STATS: + { + const surveyor240_water_stats* message_water_stats = static_cast(message); + water_stats_data.temperature = message_water_stats->temperature(); + water_stats_data.pressure = message_water_stats->pressure(); + } + break; + case Surveyor240Id::YZ_POINT_DATA: + { + const surveyor240_yz_point_data* message_yz_point_data = static_cast(message); + yz_point_data_data.timestamp_msec = message_yz_point_data->timestamp_msec(); + yz_point_data_data.ping_number = message_yz_point_data->ping_number(); + yz_point_data_data.sos_mps = message_yz_point_data->sos_mps(); + yz_point_data_data.up_vec_x = message_yz_point_data->up_vec_x(); + yz_point_data_data.up_vec_y = message_yz_point_data->up_vec_y(); + yz_point_data_data.up_vec_z = message_yz_point_data->up_vec_z(); + yz_point_data_data.mag_vec_x = message_yz_point_data->mag_vec_x(); + yz_point_data_data.mag_vec_y = message_yz_point_data->mag_vec_y(); + yz_point_data_data.mag_vec_z = message_yz_point_data->mag_vec_z(); + yz_point_data_data.reserved_0 = message_yz_point_data->reserved_0(); + yz_point_data_data.reserved_1 = message_yz_point_data->reserved_1(); + yz_point_data_data.reserved_2 = message_yz_point_data->reserved_2(); + yz_point_data_data.reserved_3 = message_yz_point_data->reserved_3(); + yz_point_data_data.reserved_4 = message_yz_point_data->reserved_4(); + yz_point_data_data.reserved_5 = message_yz_point_data->reserved_5(); + yz_point_data_data.reserved_6 = message_yz_point_data->reserved_6(); + yz_point_data_data.reserved_7 = message_yz_point_data->reserved_7(); + yz_point_data_data.reserved_8 = message_yz_point_data->reserved_8(); + yz_point_data_data.reserved_9 = message_yz_point_data->reserved_9(); + yz_point_data_data.water_degC = message_yz_point_data->water_degC(); + yz_point_data_data.water_bar = message_yz_point_data->water_bar(); + yz_point_data_data.heave_m = message_yz_point_data->heave_m(); + yz_point_data_data.start_m = message_yz_point_data->start_m(); + yz_point_data_data.end_m = message_yz_point_data->end_m(); + yz_point_data_data.unused = message_yz_point_data->unused(); + yz_point_data_data.num_points = message_yz_point_data->num_points(); + if (message_yz_point_data->num_points() > yz_point_data_data.yz_point_data_length) { + if (yz_point_data_data.yz_point_data) { + delete[] yz_point_data_data.yz_point_data; + } + yz_point_data_data.yz_point_data = new float[message_yz_point_data->num_points()]; + } + yz_point_data_data.yz_point_data_length = message_yz_point_data->num_points(); + memcpy(yz_point_data_data.yz_point_data, message_yz_point_data->yz_point_data(), message_yz_point_data->num_points()); + } + break; + + default: + break; + } + + PingDevice::_handleMessage(message); +} + +void Surveyor240::set_set_net_info(uint32_t _ntp_ip_address, uint32_t _subnet_mask, uint32_t _gateway_ip) +{ + surveyor240_set_net_info message; + message.set_ntp_ip_address(_ntp_ip_address); + message.set_subnet_mask(_subnet_mask); + message.set_gateway_ip(_gateway_ip); + writeMessage(message); +} + +void Surveyor240::set_set_ping_parameters(int32_t _start_mm, int32_t _end_mm, float _sos_mps, int16_t _gain_index, int16_t _msec_per_ping, uint16_t _deprecated, uint8_t _diagnostic_injected_signal, bool _ping_enable, bool _enable_channel_data, bool _reserved_for_raw_data, bool _enable_yz_point_data, bool _enable_atof_data, int32_t _target_ping_hz, uint16_t _n_range_steps, uint16_t _reserved, float _pulse_len_steps) +{ + surveyor240_set_ping_parameters message; + message.set_start_mm(_start_mm); + message.set_end_mm(_end_mm); + message.set_sos_mps(_sos_mps); + message.set_gain_index(_gain_index); + message.set_msec_per_ping(_msec_per_ping); + message.set_deprecated(_deprecated); + message.set_diagnostic_injected_signal(_diagnostic_injected_signal); + message.set_ping_enable(_ping_enable); + message.set_enable_channel_data(_enable_channel_data); + message.set_reserved_for_raw_data(_reserved_for_raw_data); + message.set_enable_yz_point_data(_enable_yz_point_data); + message.set_enable_atof_data(_enable_atof_data); + message.set_target_ping_hz(_target_ping_hz); + message.set_n_range_steps(_n_range_steps); + message.set_reserved(_reserved); + message.set_pulse_len_steps(_pulse_len_steps); + writeMessage(message); +} + +void Surveyor240::set_utc_response(uint64_t _utc_msec, uint32_t _accuracy_msec) +{ + surveyor240_utc_response message; + message.set_utc_msec(_utc_msec); + message.set_accuracy_msec(_accuracy_msec); + writeMessage(message); +} + diff --git a/src/device/ping-device-surveyor240.h b/src/device/ping-device-surveyor240.h new file mode 100644 index 0000000..9684722 --- /dev/null +++ b/src/device/ping-device-surveyor240.h @@ -0,0 +1,203 @@ +/* + * A device API for the Blue Robotics Ping Echosounder + * + *~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! + * THIS IS AN AUTOGENERATED FILE + * DO NOT EDIT + *~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! + * + */ + +#pragma once + +#include "ping-device.h" +#include "ping-message-surveyor240.h" + +class Surveyor240 : public PingDevice +{ +public: + /** + * @brief Constructor + * + * @param ser: The device I/O + */ + Surveyor240(PingPort& port) : PingDevice(port) {} + + /** + * @brief Destructor + */ + ~Surveyor240(); + + /** + * @brief Establish communications with the device, and initialize the update interval + * + * @param pingIntervalMs: The interval (in milliseconds) between acoustic measurements + * + * @return true if the device was initialized successfully + */ + bool initialize(); + + /** + * @brief The first octet of the IP address is placed in the least significant byte of the u32. For example, the IP address 192.168.2.2 would be packed into a u32 like this: u32 ip = (192) | (168 << 8) | (2 << 16) | (2 << 24); + * + * @param ntp_ip_address - IP address of NTP server. + * @param subnet_mask - Subnet of NTP server. + * @param gateway_ip - Gateway IP address of NTP server. Only necessary if NTP server is in different subnet than device and needs to be accessed through a gateway. Otherwise can be left as 0 + */ + void set_set_net_info(uint32_t ntp_ip_address, uint32_t subnet_mask, uint32_t gateway_ip); + + /** + * @brief Used to control the operation of Surveyor + * + * @param start_mm - Units: mm; Start range in mm at which echo data is recorded. Ignored in auto range mode. Will be forced to 5% of end range as a minimum. + * @param end_mm - Units: mm; End range in mm over which echo data is recorded. Set to 0 for Surveyor to automatically track the bottom and adjust range dynamically. + * @param sos_mps - Units: m/s; Speed of sound + * @param gain_index - Set to -1 for auto gain (recommended). Valid manual gain ranges are from 0 to 100 + * @param msec_per_ping - Units: ms/ping; pings per second = 1000 / msec_per_ping. So 100 msec_per_ping means 10 pings per second. Actual ping rate can be limited by range and speed of sound, so the value specified defines an upper limit on ping rate. + * @param deprecated - Set to 0 + * @param diagnostic_injected_signal - must be set to 0 + * @param ping_enable - true to start/continue pinging, false to stop + * @param enable_channel_data - Used by SonarView, but not necessary. If enabled log files will be much larger. + * @param reserved_for_raw_data - Must be set to false + * @param enable_yz_point_data - Angle and time of flight data is preferred for future SonarView enhancements. + * @param enable_atof_data - Enable angle and time of flight information for each detected point + * @param target_ping_hz - Set to 240000. Used for internal production testing only + * @param n_range_steps - Sets range resolution in number of range steps. Must be between 200 and 800 (inclusive) + * @param reserved - + * @param pulse_len_steps - Pulse length in number of range steps. Recommended value is 1.5 + */ + void set_set_ping_parameters(int32_t start_mm, int32_t end_mm, float sos_mps, int16_t gain_index, int16_t msec_per_ping, uint16_t deprecated, uint8_t diagnostic_injected_signal, bool ping_enable, bool enable_channel_data, bool reserved_for_raw_data, bool enable_yz_point_data, bool enable_atof_data, int32_t target_ping_hz, uint16_t n_range_steps, uint16_t reserved, float pulse_len_steps); + + /** + * @brief + * + * @param utc_msec - + * @param accuracy_msec - + */ + void set_utc_response(uint64_t utc_msec, uint32_t accuracy_msec); + + + + //! A response packet Surveyor sends to the host application containing angle and time of flight for each detected point. + struct { + // Time at start of ping, msec since power on. + uint32_t pwr_up_msec; + // Time at start of ping, utc msec (1970 epoc). Will be 0 if not avaliable. + uint64_t utc_msec; + // Time from start of ping to last range bin. + float listening_sec; + // Speed of sound (mps) used in angle calculations. + float sos_mps; + // Assigned sequentially from power on. + uint32_t ping_number; + // Frequency of the ping signal (Hz). + uint32_t ping_hz; + // Pulse length in seconds. + float pulse_sec; + // reserved + uint32_t flags; + // Number of points reported in the points field + uint16_t num_points; + // + uint16_t reserved; + // + atof_t* atof_point_data = nullptr; + uint32_t atof_point_data_length = 0; + } atof_point_data_data; + + //! A response packet Surveyor sends to the host application containing information about the attitude (pitch and roll) of the device based on it's internal sensors. + struct { + // World up vector (x, y, z) in the device coordinate system (x forward, y port, z up) + float up_vec_x; + // World up vector (x, y, z) in the device coordinate system (x forward, y port, z up) + float up_vec_y; + // World up vector (x, y, z) in the device coordinate system (x forward, y port, z up) + float up_vec_z; + // reserved for future magnetic field vector + float reserved_1; + // reserved for future magnetic field vector + float reserved_2; + // reserved for future magnetic field vector + float reserved_3; + // utc msec (1970 epoc). Will be 0 if not avaliable + uint64_t utc_msec; + // msec since power up + uint32_t pwr_up_msec; + } attitude_report_data; + + //! A response packet Surveyor sends to the host application containing information about the temperature and pressure of the water. + struct { + // + float temperature; + // + float pressure; + } water_stats_data; + + //! A response packet that Surveyor sends to the host application containing the detected depth points. + struct { + // msec since power up + uint32_t timestamp_msec; + // Assigned sequentially from power on. + uint32_t ping_number; + // Speed of sound + float sos_mps; + // World up vector (x, y, z) in the device coordinate system (x forward, y port, z up) + float up_vec_x; + // World up vector (x, y, z) in the device coordinate system (x forward, y port, z up) + float up_vec_y; + // World up vector (x, y, z) in the device coordinate system (x forward, y port, z up) + float up_vec_z; + // reserved for future magnetic field vector + float mag_vec_x; + // reserved for future magnetic field vector + float mag_vec_y; + // reserved for future magnetic field vector + float mag_vec_z; + // + uint32_t reserved_0; + // + uint32_t reserved_1; + // + uint32_t reserved_2; + // + uint32_t reserved_3; + // + uint32_t reserved_4; + // + uint32_t reserved_5; + // + uint32_t reserved_6; + // + uint32_t reserved_7; + // + uint32_t reserved_8; + // + uint32_t reserved_9; + // -1000 if sensor not installed + float water_degC; + // Water pressure in bar. -1000 if sensor not installed + float water_bar; + // Reserved for heave in meters. + float heave_m; + // Range at the beginning of the analyzed signals for this ping. + float start_m; + // Range at the end of the analyzed signals for this ping. + float end_m; + // + uint16_t unused; + // Number of points reported in the points field + uint16_t num_points; + // Y and Z value pairs for each detected point. Y is athwartships offset in meters, positive to the port side. Z is depth, positive up so all z values will be negative. + float* yz_point_data = nullptr; + uint32_t yz_point_data_length = 0; + } yz_point_data_data; + + +private: + /** + * @brief Handle an incoming message from the device. Internal values are updated according to the device data. + * + * @param message: A pointer to the message received from the device + */ + void _handleMessage(const ping_message* message) override; +}; diff --git a/src/device/ping-device.cpp b/src/device/ping-device.cpp index 71db21e..863e97f 100644 --- a/src/device/ping-device.cpp +++ b/src/device/ping-device.cpp @@ -4,8 +4,25 @@ #include