diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d145b28..00f33c6 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,74 +12,74 @@ # # See https://github.com/pre-commit/pre-commit -repos: - # Standard hooks - - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 - hooks: - - id: check-added-large-files - - id: check-ast - - id: check-case-conflict - - id: check-docstring-first - - id: check-merge-conflict - - id: check-symlinks - - id: check-xml - - id: check-yaml - args: ["--allow-multiple-documents"] - - id: debug-statements - - id: end-of-file-fixer - - id: mixed-line-ending - - id: trailing-whitespace - exclude_types: [rst] - - id: fix-byte-order-marker - - id: requirements-txt-fixer - # Python hooks - - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.12.2 - hooks: - - id: ruff-format - - id: ruff - name: ruff-isort - args: [ - "--select=I", - "--fix" - ] - - id: ruff - name: ruff-pyupgrade - args: [ - "--select=UP", - "--fix" - ] - - id: ruff - name: ruff-pydocstyle - args: [ - "--select=D", - "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", - "--fix", - ] - stages: [pre-commit] - pass_filenames: true - - id: ruff - name: ruff-check - args: [ - "--select=F,PT,B,C4,T20,S,N", - "--ignore=T201,N812,B006,S101,S311,S607,S603", - "--fix" - ] - stages: [pre-commit] - pass_filenames: true - # C++ hooks - - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v20.1.7 - hooks: - - id: clang-format - args: [--style=file] - # Spellcheck in comments and docs - - repo: https://github.com/codespell-project/codespell - rev: v2.4.1 - hooks: - - id: codespell - args: ['--write-changes', '--ignore-words-list=theses,fom'] +# repos: +# # Standard hooks +# - repo: https://github.com/pre-commit/pre-commit-hooks +# rev: v5.0.0 +# hooks: +# - id: check-added-large-files +# - id: check-ast +# - id: check-case-conflict +# - id: check-docstring-first +# - id: check-merge-conflict +# - id: check-symlinks +# - id: check-xml +# - id: check-yaml +# args: ["--allow-multiple-documents"] +# - id: debug-statements +# - id: end-of-file-fixer +# - id: mixed-line-ending +# - id: trailing-whitespace +# exclude_types: [rst] +# - id: fix-byte-order-marker +# - id: requirements-txt-fixer +# # Python hooks +# - repo: https://github.com/astral-sh/ruff-pre-commit +# rev: v0.12.2 +# hooks: +# - id: ruff-format +# - id: ruff +# name: ruff-isort +# args: [ +# "--select=I", +# "--fix" +# ] +# - id: ruff +# name: ruff-pyupgrade +# args: [ +# "--select=UP", +# "--fix" +# ] +# - id: ruff +# name: ruff-pydocstyle +# args: [ +# "--select=D", +# "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", +# "--fix", +# ] +# stages: [pre-commit] +# pass_filenames: true +# - id: ruff +# name: ruff-check +# args: [ +# "--select=F,PT,B,C4,T20,S,N", +# "--ignore=T201,N812,B006,S101,S311,S607,S603", +# "--fix" +# ] +# stages: [pre-commit] +# pass_filenames: true +# # C++ hooks +# - repo: https://github.com/pre-commit/mirrors-clang-format +# rev: v20.1.7 +# hooks: +# - id: clang-format +# args: [--style=file] +# # Spellcheck in comments and docs +# - repo: https://github.com/codespell-project/codespell +# rev: v2.4.1 +# hooks: +# - id: codespell +# args: ['--write-changes', '--ignore-words-list=theses,fom'] -ci: - autoupdate_schedule: quarterly +# ci: +# autoupdate_schedule: quarterly diff --git a/acoustic_modem/CMakeLists.txt b/acoustic_modem/CMakeLists.txt new file mode 100644 index 0000000..01a01b1 --- /dev/null +++ b/acoustic_modem/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.8) +project(acoustic_modem) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(serial_driver REQUIRED) +find_package(io_context REQUIRED) +find_package(nlohmann_json REQUIRED) + + +include_directories(include) + +#set(LIB_NAME ${PROJECT_NAME}) + +add_library(${PROJECT_NAME} SHARED + src/am_driver.cpp) + + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + serial_driver + io_context + nlohmann_json +) + +# Install library and headers +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + + +#target_link_libraries() + +#install(TARGETS +#am_node +#DESTINATION lib/${PROJECT_NAME} +#) + + +install( + DIRECTORY include/ + DESTINATION include +) + + +ament_package() + +# if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# ament_add_gtest(test_driver test/test_driver.cpp) +# target_link_libraries(test_driver ${PROJECT_NAME}) +# endif() diff --git a/acoustic_modem/include/am_base_node.hpp b/acoustic_modem/include/am_base_node.hpp new file mode 100644 index 0000000..431931e --- /dev/null +++ b/acoustic_modem/include/am_base_node.hpp @@ -0,0 +1,55 @@ +#ifndef AM_BASE_NODE.HPP +#define AM_BASE_NODE .HPP + +#include +#include +#include +#include "am_driver.hpp" + +/** + * Creating two nodes, one for each system + * Initially implementing uni-directional communication: + * drone -> base station + * + * this one should have publisher and then through acoustic channel should read + * data + * + * (read_packet) + */ + +class BaseNode : public rclcpp::Node { + public: + explicit BaseNode(); + + private: + /** + * initialize the connection by creating AcousticModemDriver object + */ + void init_connection(); + + void set_publishers(); + + + void poll_and_publish_rx(); + + /** + * publish message in correct topic based by header type + */ + void publish_in_correct_topic(); + + /** + * extract header from message to publish it in the correct type + * shifts the message as before + */ + uint8 extract_type_and_shift(std::vector& msg); + + rclcpp::Publisher::SharedPtr data_1_; + rclcpp::Publisher::SharedPtr data_2_; + rclcpp::Publisher::SharedPtr data_3_; + rclcpp::Publisher::SharedPtr data_4_; + std::unique_ptr base_modem_; + // std::string latest_; + rclcpp::TimerBase::SharedPtr timer_; +} + +#endif diff --git a/acoustic_modem/include/am_driver.hpp b/acoustic_modem/include/am_driver.hpp new file mode 100644 index 0000000..6de43fa --- /dev/null +++ b/acoustic_modem/include/am_driver.hpp @@ -0,0 +1,349 @@ +#ifndef AM_DRIVER_HPP +#define AM_DRIVER_HPP + +#include +#include // per scrivere su file +#include +#include +#include +#include +#include +#include +#include +// #include +#include +#include +#include + + +/** + * we don't need last, we know the dimension of the data we are sending + * so we use the type to understand which send_data we use +*/ +// struct PacketHeader { +// uint8_t type : 2; +// uint8_t order : 3; +// //uint8_t last : 1; +// }; + +enum class TxState{ + SENDING, + WAIT_ACK, + IDLE, +}; + + +enum class MsgType : uint8_t{ + Type_def=0, + Type_1 = 1, + Type_2 = 2, + Type_3 = 3, + Type_4 = 4, +}; + +struct DiagnosticData { + uint8_t TR_BLOCK[2]; // 6 bits will be used by the packet header + uint8_t BER; + uint8_t SIGNAL_POWER; + uint8_t NOISE_POWER; + uint8_t PACKET_VALID[2]; + uint8_t PACKET_INVALID; + uint8_t GIT_REV; + uint8_t TIME[3]; + uint8_t CHIP_ID[2]; + uint8_t HW_REV : 2; + uint8_t CHANNEL : 4; + uint8_t TB_VALID : 1; + uint8_t TX_COMPLETE : 1; + uint8_t DIAGNOSTIC_MODE : 1; + uint8_t POWER_LEVEL : 2; +}; + +class AcousticModemDriver { + //friend class TestAcousticModemDriver; + public: + //default for testing + AcousticModemDriver(); + /** + close the port + */ + ~AcousticModemDriver(); + + /** + Initialize the modem connection. + Parameters: + device: (str): Serial port (e.g. "COM3" on Windows or "/dev/ttyUSB0" + on Linux). baudrate (int): Baud rate (default 9600). timeout (float): + Timeout for serial reads (default 0.5). channel (int): Channel to set (valid + values 1 to 12), (default 1). level (int): Power level to set (valid values + 1 to 4), (default 4). diagnostic (bool): If True, set the modem to + diagnostic mode; if False, set transparent mode, (default 1). + **/ + AcousticModemDriver(const std::string& device, + int baudrate, + int channel, + int level, + bool diagnostic, + float timeout); + + uint16_t make_handshake(MsgType type); + + // receiver will use these functions to implement the logic + bool is_handshake(uint16_t packet); + MsgType type(uint16_t packet); + uint16_t id(uint16_t packet); + + // distinguish each type of messsage and associate a number of float with it + static size_t floats_for_type(MsgType t); + + // divide the float in 2 uint16 + static void float_to_word(float v, uint16_t &w0, uint16_t &w1); + + static float word_to_float(uint16_t w0, uint16_t w1); + + // convert float to string and use send_two_bytes + void send_word(uint16_t w); + + // send whole messagge with handshake and chunks + void send_message(MsgType type, const float* data); + + + void rx_start_handshake(uint16_t hs_word); + + void rx_reset(); + + bool rx_rebuild_word(uint16_t w, MsgType &out_type, uint16_t &out_msg_id, float *out_floats, uint8_t &inout_capacity, std::chrono::milliseconds timeout); + + bool try_pop_rx(std::vector &out_w); + /** + Send ASCII data to the modem. + + Parameters: + data (str): The data to be sent. + + Returns: + int: Number of characters written. + **/ + size_t send_data(std::string data); + + /** + Send ASCII data to the modem. + + Parameters: + data (char): The data to be sent. + + Returns: + int: Number of characters written. + **/ + size_t send_data(char data); + + /** + Send two bytes of data to the modem. + + Parameters: + data (str): A string (at least two characters) representing the + data. + + Returns: + int: Number of characters written + **/ + size_t send_two_bytes(std::string data); + + /** + Send a longer message (more than 2 bytes) in 2-byte chunks. + If in diagnostic mode, after sending each chunk, wait for a diagnostic + report that indicates the transmission is complete (TX_COMPLETE == 1). If + in transparent mode, simply wait 2 seconds between chunks. + + Parameters: + data (str): The message to be sent. + timeout(float): Maximum time (in seconds) to wait for TX_COMPLETE + after sending each 2-byte chunk. + */ + size_t send_msg(std::string data, float timeout = 5.0f); + + /** + Read data from the serial port and search for a valid diagnostic packet. + A valid packet starts with '$' (0x24) and ends with '\\n' (0x0A) and is + exactly 18 bytes long. + + Returns: + Optional[bytes]: The valid packet if found, otherwise the buffer if + it is not empty. + */ + + // to set channel of communication + bool set_channel(int channel); + + // to set power level + bool set_level(int level); + + // to set diagnostic mode + bool set_diagnostic_mode(); + + bool reset_diagnostic_mode(); + + /** + Request a diagnostic report, decode it, update member variables from the + report, + + TODO: and optionally save the report as a JSON file. + + This function sends the report request command and then listens for a + valid packet until overall_timeout seconds have elapsed. + + Parameters: + TODO: filename (str, optional): If provided, the report is saved to + this file. overall_timeout (float): Maximum time (in seconds) to wait for + a valid report. + + Returns: + DiagnosticData: The decoded report if successful; otherwise, None. + */ + std::optional request_report( + float overall_timeout = 5.0f, + std::optional filename = std::nullopt); + + /** + Update internal state modem configuration + Parameters: + DiagnosticData report: Decoded report from the modem containing + configuration info. + */ + void update_state_from_report(DiagnosticData report); + + /** + Request a diagnostic report from the modem. + */ + void get_report(); + + /** + Read data from the serial port and search for a valid diagnostic packet. + A valid packet starts with '$' (0x24) and ends with '\\n' (0x0A) and is + exactly 18 bytes long. + + Returns: + Optional[bytes]: The valid packet if found, otherwise the buffer if + it is not empty. + */ + + std::optional> read_packet(); + + /** + * read Data asynchronously + */ + void start_async_read(); + + /** + * callback for async_receive(), saves data in queue + */ + void read_callback(std::vector& data); + + /** + * ideally in this we should order the different packet and decode the + * message then it will be used in the ros2 node, how to decide which type + * of msg? header? + */ + std::optional> process_packet(); + + /** + Decode a diagnostic packet received from the modem. + + The packet should be 18 bytes long, starting with '$' (0x24) and ending + with '\\n' (0x0A). The bytes between contain the data in the following + format: + - Byte 0: '$' + - Bytes 1-16: Data fields (see modem documentation) + - Byte 17: '\\n' + + Returns: + A DiagnosticData of decoded values if the packet is valid, + otherwise None. + */ + + std::optional decode_packet(std::vector& packet); + + /** + Close the serial connection. + */ + void close(); + + private: + + // TODO: could be done by using class BitWriter + void append_bits(std::vector& buffer, + uint16_t bit_to_append, + int count, + int& bit_position); + + struct DiagnosticPacket { + uint16_t TR_BLOCK; + uint8_t BER; + uint8_t SIGNAL_POWER; + uint8_t NOISE_POWER; + uint16_t PACKET_VALID; + uint8_t PACKET_INVALID; + uint8_t GIT_REV; + uint8_t TIME_L; + uint8_t TIME_M; + uint8_t TIME_H; + uint16_t CHIP_ID; + uint8_t HW_CH_FLAGS; // 14 (contains HW_REV, CHANNEL, TB_VALID, + // TX_COMPLETE) + uint8_t MODE_LEVEL_FLAGS; // 15 (contains DIAGNOSTIC_MODE, LEVEL) + }; + + drivers::common::IoContext io_; + + // function called to get the port returns shared pointer: drv_.port() + drivers::serial_driver::SerialDriver drv_; + drivers::serial_driver::SerialPortConfig cfg_; + std::shared_ptr port_; + std::string device_; + + + // 4 bits to recognize handshake + static constexpr uint8_t HANDSHAKE_SYNC = 0xA; + // 10 bit for msg id, to avoid error caused by lag or delay (TODO: to check if it is necessary) + uint16_t msg_id; + + static constexpr uint16_t RX_MAX_WORDS = 20; + //static constexpr uint8_t RX_MAX_FLOATS = 10; + + struct RxState{ + bool rx_receiving= false; + MsgType rx_type=MsgType::Type_def; + uint16_t rx_msg_id= 0; + uint16_t rx_expected_words = 0; + uint16_t rx_received_words = 0; + uint16_t rx_words[RX_MAX_WORDS]{}; + uint8_t rx_n_floats= 0; + std::chrono::steady_clock::time_point rx_last_rx{}; + }; + + RxState rx; + + // to save what we receive asynchronously + /** + * needed because of async_read_some inserial lib + * it could be called when reading data and when writing + * so race dondition + */ + std::queue> queue; + std::map> map; + std::mutex queue_mutex; + + //std::queue pending_msg; + // keeps count on what bit are we are at when we recreate the message + int bit_pos_; + std::vector full_message; + + int channel_; + int level_; + bool diagnostic_; + + // Timeout per chunk, + // static constexpr float default_timeout = 0.5f; +}; + +#endif diff --git a/acoustic_modem/include/am_drone_node.hpp b/acoustic_modem/include/am_drone_node.hpp new file mode 100644 index 0000000..8dad50c --- /dev/null +++ b/acoustic_modem/include/am_drone_node.hpp @@ -0,0 +1,68 @@ +#ifndef AM_ROS_HPP +#define AM_ROS_HPP + +#include +#include +#include +#include "am_driver.hpp" + +/** + * Creating two nodes, one for each system + * Initially implementing uni-directional communication: + * drone -> base station + * + * this one should have subscriber and then through acoustic channel should send + *data to base station + * + *(send_msg) + */ + +class DroneNode : public rclcpp::Node { + public: + explicit DroneNode(); + + private: + /** + * initialize the connection by creating AcousticModemDriver object + */ + void init_connection(); + + /** + * Create the subscriber to the topic + */ + void set_subscriber(); + + /** + * Called every # second to send different messages to the other modem + * through acoustic communication + * + * TODO: fix logic, probably seconds not correct + */ + void acoustic_callback(); + + /** + * the idea is this but it depends on what the data are and how we get them + * from the topic: + * + * we save te last string received and then send it using the timer with an + * interval + * + * To fix because we will probably lose information + * + * idea: queue for each new message in the topic and send them in order + * by giving each of them a sending time based on the length of the message + * ? + */ + void data_callback(); + + /** + * subscriber to the data-topic, the type is now string so that + * it's easier with send_msg(string), can be changed + */ + rclcpp::Subscription::SharedPtr subscription_; + std::unique_ptr drone_modem_; + std::string latest_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +#endif diff --git a/acoustic_modem/include/tdma_manager.hpp b/acoustic_modem/include/tdma_manager.hpp new file mode 100644 index 0000000..c11811e --- /dev/null +++ b/acoustic_modem/include/tdma_manager.hpp @@ -0,0 +1,29 @@ +#include "am_driver.hpp" + +/** + * Idea: use hybrid TDMA, to try to avoid collisions + * + */ + +struct TDMAConfig{ + uint8_t num_slots; + uint8_t my_slot; + std::chrono::seconds slot=std::chrono::seconds(25); + std::chrono::milliseconds guard=std::chrono::milliseconds(1000); + std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); +}; + +class TDMAManager{ + public: + TDMAManager(TDMAConfig config); + + // who is transmitting? + uint8_t current_slot(std::chrono::steady_clock::time_point now); + + // am i allowed to transmit? + bool tx_allowed(std::chrono::steady_clock::time_point now); + + private: + TDMAConfig cfg; + +} \ No newline at end of file diff --git a/acoustic_modem/package.xml b/acoustic_modem/package.xml new file mode 100644 index 0000000..5e3838b --- /dev/null +++ b/acoustic_modem/package.xml @@ -0,0 +1,20 @@ + + acoustic_modem + 1.0.0 + Acoustic modem implementation + vortex + MIT + + ament_cmake + ament_cmake_auto + + rclcpp + std_msgs + serial_driver + io_context + nlohmann_json + + + ament_cmake + + diff --git a/acoustic_modem/src/am_base_node.cpp b/acoustic_modem/src/am_base_node.cpp new file mode 100644 index 0000000..0408050 --- /dev/null +++ b/acoustic_modem/src/am_base_node.cpp @@ -0,0 +1,114 @@ +#include "am_driver.hpp" +#include "am_drone_node.hpp" + +BaseNode::BaseNode() : Node("base_node") { + // TODO + set_publishers(); + init_connection(); + + timer_ = this->create_wall_timer( + 1000ms, std::bind(&BaseNode::publish_in_correct_topic(), this)); +} + +void BaseNode::init_connection() { + this->declare_parameter("device"); + this->declare_parameter("baudrate", 9600); + this->declare_parameter("channel", 1); + this->declare_parameter("level", 4); + this->declare_parameter("diagnostic", false); + this->declare_parameter("timeout", 0.5); + + std::string device = this->get_parameter("device").as_string(); + int baudrate = this->get_parameter("baudrate").as_int(); + int channel = this->get_parameter("channel").as_int(); + int level = this->get_parameter("level").as_int(); + bool diagnostic = this->get_parameter("diagnostic").as_bool(); + float timeout = + static_cast(this->get_parameter("timeout").as_double()); + + base_modem_ = AcousticModemDriver(device, baudrate, channel, level, + diagnostic, timeout); +} + +void BaseNode::set_publishers() { + // we reserved 2 bit for the type of data so we'll have 4 types of data + data_0_ = this->create_publisher("topic_0", 10); + data_1_ = this->create_publisher("topic_1", 10); + data_2_ = this->create_publisher("topic_2", 10); + data_3_ = this->create_publisher("topic_3", 10); +} + + +void BaseNode::poll_and_publish_rx(){ + std::vector bytes; + while (base_modem_.try_pop_rx(bytes)) { + for (size_t i = 0; i < bytes.size(); i += 2) { + uint16_t w =static_cast(bytes[i]) | (static_cast(bytes[i + 1]) << 8); + MsgType out_type{}; + uint16_t out_msg_id; + float floats[10]; + uint8_t inout_capacity=0; + bool complete=base_modem_.rx_rebuild_word(w, out_type, out_msg_id, out_floats, inout_capacity, std::chrono::milliseconds(2000)); + if (complete) { + //PUBLISH HERE BASED ON TYPE, TO CHECK HOW + switch (out_type) { + case MsgType::Type_1: + data_0_->publish(out_floats); + case MsgType::Type_2: + data_1_->publish(out_floats); + case MsgType::Type_3: + data_2_->publish(out_floats); + default: + data_3_->publish(out_floats); + } + } + } + } +} + + + +void BaseNode::publish_in_correct_topic() { + std::optional> message = base_modem_.process_packet(); + + if (!message.has_value()) { + return; + } + uint8_t type = extract_type_and_shift(message); + + switch (type) { + case 0: + data_0_->publish(message); + case 1: + data_1_->publish(message); + case 2: + data_2_->publish(message); + case 3: + data_3_->publish(message); + } +} + +// remove the type bit and shifts the message as before +uint8_t BaseNode::extract_type_and_shift(std::vector& msg) { + uint8_t header = msg[0]; + uint8_t type = (header & 0xC0) >> 6; + + uint8_t carry = 0; + for (size_t i = 0; i < msg.size(); ++i) { + uint8_t current = msg[i]; + msg[i] = static_cast((current << 2) | (carry >> 6)); + carry = current; + } + // remove non necessary bits at the end of the messages + msg.pop_back(); + + return type(); +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/acoustic_modem/src/am_driver.cpp b/acoustic_modem/src/am_driver.cpp new file mode 100644 index 0000000..5c66e18 --- /dev/null +++ b/acoustic_modem/src/am_driver.cpp @@ -0,0 +1,613 @@ +#include "am_driver.hpp" +#include + +AcousticModemDriver::AcousticModemDriver(const std::string& device, + int baudrate, + int channel, + int level, + bool diagnostic, + float timeout) + : io_(1), // I/O context, required by the library + drv_(io_), // SerialDriver object + cfg_(baudrate, // configuration how UART should operate + drivers::serial_driver::FlowControl::NONE, + drivers::serial_driver::Parity::NONE, + drivers::serial_driver::StopBits::ONE), + // saves the device path to a member variable + device_(device), + channel_(channel), + level_(level), + diagnostic_(diagnostic) { + // initialization of the port in device_ + msg_id=0; + drv_.init_port(device_, cfg_); + + // creation of the actual SerialPort object (create to use open(), close() + // ...) + port_ = drv_.port(); + + // actually open physical serial port + port_->open(); + + if (!(port_->is_open())) { + std::cerr + << "[Error] Serial port not open. Communication will not start." + << std::endl; + return; + } + + this->set_channel(channel); + this->set_level(level); + + if (diagnostic) { + this->set_diagnostic_mode(); + } else { + this->reset_diagnostic_mode(); + } + + // start waiting for data + this->start_async_read(); + + std::cout << "[INFO] Modem initialized on device " << this->device_ + << ", channel " << channel << ", level " << level + << ", diagnostic mode = " << std::boolalpha << diagnostic + << std::endl; +} + +AcousticModemDriver::~AcousticModemDriver() { + port_->close(); +} + +// AcousticModemDriver::AcousticModemDriver() +// : io_(1), +// drv_(io_), +// cfg_(9600, +// drivers::serial_driver::FlowControl::NONE, +// drivers::serial_driver::Parity::NONE, +// drivers::serial_driver::StopBits::ONE), +// channel_(0), +// level_(0), +// diagnostic_(false) { + +// } + +uint16_t AcousticModemDriver::make_handshake(MsgType t){ + const uint16_t id = (msg_id++ & 0x03FF); // 10-bit rolling counter + const uint16_t type = (uint16_t(t) & 0x0003); // keep only 2 bits + // [SYNC(4) | TYPE(2) | MSG_ID(10)] + return uint16_t((uint16_t(HANDSHAKE_SYNC) << 12) | + (type << 10) | + id); +} + +bool AcousticModemDriver::is_handshake(uint16_t packet){ + return ((packet>>12 & 0xF)==HANDSHAKE_SYNC); +} +MsgType AcousticModemDriver::type(uint16_t packet){ + return MsgType((packet>>10 & 0x3)); +} +uint16_t AcousticModemDriver::id(uint16_t packet){ + return ((packet & 0x3FF)); +} + +size_t AcousticModemDriver::floats_for_type(MsgType t) { + switch (t) { + case MsgType::Type_1: return 2; + case MsgType::Type_2: return 4; + case MsgType::Type_3: return 5; + default: return 0; + } +} + +void AcousticModemDriver::float_to_word(float v, uint16_t &w0, uint16_t &w1){ + uint8_t b[4]; + std::memcpy(b,&v,4); + + w0 = uint16_t(b[0]) | (uint16_t(b[1]) << 8); // low part of w1 = b[0], high part=b[1] + w1 = uint16_t(b[2]) | (uint16_t(b[3]) << 8); +} + +float AcousticModemDriver::word_to_float(uint16_t w0, uint16_t w1){ + uint8_t b[4]; + b[0]=uint8_t(w0 & 0xFF); + b[1]=uint8_t((w0 >> 8) & 0xFF); + b[2]=uint8_t(w1 & 0xFF); + b[3]=uint8_t((w1 >> 8) & 0xFF); + float v; + std::memcpy(&v, b, 4); + return v; +} + +void AcousticModemDriver::send_word(uint16_t w){ + std::string s(2, '\0'); // string of 2 bytes + s[0]=static_cast(w & 0xFF); + s[1]=static_cast((w>>8) & 0xFF); + send_two_bytes(s); +} + +void AcousticModemDriver::send_message(MsgType type, const float* data){ + const size_t n= floats_for_type(type); + // TODO: implement for fourth type of data or default case (return;) + if(n==0 || data==nullptr){ + return; + } + send_word(make_handshake(type)); // send the first packet of 16 bit containing [SYNC(4) | TYPE(2) | MSG_ID(10)] + for(int i=0;iRX_MAX_WORDS){ + rx.rx_receiving=false; + return; + } + rx.rx_receiving=true; +} + +bool AcousticModemDriver::rx_rebuild_word(uint16_t w, MsgType &out_type, uint16_t &out_msg_id,float *out_floats, uint8_t &inout_capacity, std::chrono::milliseconds timeout){ + auto now = std::chrono::steady_clock::now(); + + // timeout for incomplete message (the timeout is to be defined and if it is needed) + if (rx.rx_receiving && (now - rx.rx_last_rx > timeout)) { + rx_reset(); + } + rx.rx_last_rx= now; + + if(is_handshake(w)){ + // if the word is a handshake we start receiveing from the start + rx_start_handshake(w); + return false; + } + if(!rx.rx_receiving){ + return false; + } + if(rx.rx_received_words>rx.rx_expected_words){ + rx_reset(); + return false; + } + rx.rx_words[rx.rx_received_words++]=w; + const size_t n_floats=floats_for_type(rx.rx_type); + if(rx.rx_received_words==rx.rx_expected_words){ + // maybe add check for floats capacity but i don't think it's necessary + + for(size_t i=0; i &out_w){ + std::lock_guard lock(queue_mutex); + if(queue.empty()){ + return false; + } + out_w = queue.front(); + queue.pop(); + return true; +} + +size_t AcousticModemDriver::send_data(std::string data) { + // send data using serial driver's send(msg) + std::vector msg(data.begin(), data.end()); + size_t bytes = port_->send(msg); + return bytes; +} + +// overload send_data(char) +size_t AcousticModemDriver::send_data(char data) { + // send data using serial driver's send(msg) + std::vector msg = {static_cast(data)}; + size_t bytes = port_->send(msg); + return bytes; +} + +size_t AcousticModemDriver::send_two_bytes(std::string data) { + // only send 2 bytes waiting 1 second after sending them + if (data.length() != 2) { + return 0; + } else { + size_t bytes = this->send_data(data); + + // 10bps + std::this_thread::sleep_for(std::chrono::seconds(2)); + return bytes; + } +} + +size_t AcousticModemDriver::send_msg(std::string data, float timeout) { + size_t sum_sent_char = 0; + + if (data.length() % 2 != 0) { + data += ' '; + } + for (int i = 0; i < data.length(); i += 2) { + std::string chunk = data.substr(i, 2); + size_t sent_chunk = this->send_two_bytes(chunk); + if (sent_chunk != 0) { + sum_sent_char += sent_chunk; + } + + if (!(this->diagnostic_)) { + // wait for transmission + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + auto start_time = std::chrono::steady_clock::now(); + + while ((std::chrono::steady_clock::now() - start_time) < + std::chrono::duration(timeout)) { + std::optional> packet = this->read_packet(); + if (!packet.has_value()) { + break; + } + // std::vector + // packet_cast=static_cast>(*packet); + std::optional report = this->decode_packet(*packet); + if (report.has_value() && report->TX_COMPLETE == 1) { + std::cout << "Transmission complete for chunk: " << chunk + << std::endl; + break; + } + } + } + return sum_sent_char; +} + +// to set channel of communication +bool AcousticModemDriver::set_channel(int channel) { + // check channel is correct number + if (channel < 1 || channel > 12) { + std::cout << "Warning: Channel " << channel + << " is not a valid channel, needs to be between 1 and 12." + << std::endl; + return false; + } + // wait 1 sec between c and c to go in command mode + this->send_data('c'); + std::this_thread::sleep_for(std::chrono::seconds(1)); + this->send_data('c'); + + switch (channel) { + case 10: + this->send_data('a'); + break; + case 11: + this->send_data('b'); + break; + case 12: + this->send_data('c'); + break; + default: + this->send_data(std::to_string(channel)); + break; + } + channel_ = channel; + std::this_thread::sleep_for(std::chrono::seconds(1)); + return true; +} + +// to set power level +bool AcousticModemDriver::set_level(int level) { + if (level < 1 || level > 4) { + std::cout << "Warning: Level " << level + << " is not a valid Level, needs to be between 1 and 4." + << std::endl; + return false; + } + this->send_data('l'); + std::this_thread::sleep_for(std::chrono::seconds(1)); + this->send_data('l'); + + this->send_data(std::to_string(level)); + level_ = level; + std::this_thread::sleep_for(std::chrono::seconds(1)); + return true; +} + +// to set diagnostic mode +bool AcousticModemDriver::set_diagnostic_mode() { + this->send_data('d'); + std::this_thread::sleep_for(std::chrono::seconds(1)); + this->send_data('d'); + diagnostic_ = true; + std::this_thread::sleep_for(std::chrono::seconds(1)); + return true; +} + +bool AcousticModemDriver::reset_diagnostic_mode() { + this->send_data('t'); + std::this_thread::sleep_for(std::chrono::seconds(1)); + this->send_data('t'); + diagnostic_ = false; + std::this_thread::sleep_for(std::chrono::seconds(1)); + return false; +} + +std::optional AcousticModemDriver::request_report( + float overall_timeout, + std::optional filename) { + this->get_report(); + + std::optional> packet = this->read_packet(); + if (!(packet.has_value())) { + std::cout << "Packet is empty" << std::endl; + return std::nullopt; + } + // std::vector + // packet_cast=static_cast>(*packet); + std::cout << "Returning packet of length: " + << std::string((*packet).begin(), (*packet).end()).length() + << std::endl; + + std::optional report = this->decode_packet(*packet); + if (!(report.has_value())) { + std::cout << "Failed to decode the packet." << std::endl; + return std::nullopt; + } + // DiagnosticData report_cast=static_cast(*report); + this->update_state_from_report(*report); + + // TODO? implement saving report in json file + if (filename.has_value()) { + // TODO + } + + return *report; +} + +void AcousticModemDriver::update_state_from_report(DiagnosticData report) { + this->channel_ = static_cast(report.CHANNEL); + this->level_ = static_cast(report.POWER_LEVEL); + this->diagnostic_ = report.DIAGNOSTIC_MODE; + std::cout << "Updated channel: " << this->channel_ + << "Updated level: " << this->level_ + << "Updated diagnostic: " << this->diagnostic_ << std::endl; +} + +void AcousticModemDriver::get_report() { + this->send_data('r'); + std::this_thread::sleep_for(std::chrono::seconds(1)); + this->send_data('r'); + std::this_thread::sleep_for(std::chrono::seconds(1)); +} + +void AcousticModemDriver::start_async_read() { + port_->async_receive( + [this](std::vector& buffer, const size_t& bytes_transferred) { + std::vector data(buffer.begin(), + buffer.begin() + bytes_transferred); + this->read_callback(data); + this->start_async_read(); + }); +} + +/** + * mutex probably needed because of async_receive + * asio lib creates a new thread, if we access that both when some data arrive + * (push) and when we need to extract (pop) + */ +void AcousticModemDriver::read_callback(std::vector& data) { + std::lock_guard lock(queue_mutex); + queue.push(data); +} + +/** + * re-build packet based on header bits + * checking for code, order and last + */ +std::optional> AcousticModemDriver::process_packet() { + std::vector fragment; + { + std::lock_guard lock(queue_mutex); + if (queue.empty()) { + return std::nullopt; + } + fragment = std::move(queue.front()); + queue.pop(); + // mutex end + } + + // to fix: using header? + uint8_t byte0 = fragment[0]; + uint8_t byte1 = fragment[1]; + uint8_t type = (byte0 & 0xC0) >> 6; + uint8_t order = (byte0 & 0x38) >> 3; + bool last = (byte0 & 0x04) >> 2; + uint16_t data = ((byte0 & 0x03) << 8) | byte1; + map[type][order] = data; + + if (!last) { + return std::nullopt; + } + + bool complete = true; + for (int i = 0; i <= order; ++i) { + if (map[type].count(i) == 0) { + complete = false; + // last arrived but not every fragment is inside the map + break; + } + } + + if (!complete) { + // TODO: how to behave if not every fragment is present + // for now it just return null and the message is lost, + // goal is to implement a way to retrieve the piece we lose + full_message.clear(); + map.erase(type); + bit_pos_ = 0; + return std::nullopt; + } + + // adding two bit at the start of the full message to understand which type + // of message is + append_bits(full_message, type, 2, bit_pos_); + + // need to concatenate 10 data bit for each fragment + // we use order as number of package because we are working with the last + // package order + for (int i = 0; i <= order; i++) { + append_bits(full_message, map[type][i], 10, bit_pos_); + } + + auto message = full_message; + + // deleting data for next message + full_message.clear(); + map.erase(type); + bit_pos_ = 0; + + return message; +} + +void AcousticModemDriver::append_bits(std::vector& buffer, + uint16_t bit_to_append, + int count, + int& bit_position) { + for (int i = count - 1; i >= 0; --i) { + // extract the bit + bool bit = (bit_to_append >> i) & 1; + + // if we completed the previous element of the buffer, we create a new + // one + if (bit == 0) { + buffer.push_back(0); + } + + if (bit) { + buffer.back() |= (1 << (7 - bit_position)); + } + // increase the position we are adding the bit + bit_position = (bit_position + 1) % 8; + } +} + +std::optional> AcousticModemDriver::read_packet() { + // time duration to wait for a valid packet + const auto time_duration = std::chrono::seconds(2); + auto start_time = std::chrono::steady_clock::now(); + + std::vector buffer(64); // 64 can be modified, didn't put 18 to + // avoid cutting diagnostic packet in half + + while ((std::chrono::steady_clock::now() - start_time) < time_duration) { + std::vector temp_buffer( + 64); // use this to store temporanealy buffer data, to add them to + // buffer + size_t bytes_read = port_->receive(temp_buffer); + temp_buffer.resize( + bytes_read); // resize the temp_buffer to the actual byte read + + if (bytes_read == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + + buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end()); + + std::cout << "Buffer size: " << bytes_read << " bytes, " + << "Buffer: " << std::string(buffer.begin(), buffer.end()) + << std::endl; + + if (bytes_read <= 17) { // look for diagnostic packet + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + auto begin = + find(buffer.begin(), buffer.end(), '$'); // returns iterator + auto end = find(buffer.begin(), buffer.end(), '\n'); + + if (begin != buffer.end() && + end != buffer.end()) { // create and return diagnostic packet + std::vector packet(begin, end + 1); + std::cout << "Returning packet: " + << std::string(packet.begin(), packet.end()) << std::endl; + return packet; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if (buffer.size() == 0) { + std::cout << "Returning no data"; + return std::nullopt; + } else { + std::cout << "Returning buffer: " + << std::string(buffer.begin(), buffer.end()) << std::endl; + return buffer; + } +} + +std::optional AcousticModemDriver::decode_packet( + std::vector& packet) { + std::string packet_str(packet.begin(), packet.end()); + + if (packet_str.size() != 18 || packet_str.front() != '$' || + packet_str.back() != '\n') { // check if the packet is 18 bytes with + // the start and end character + return std::nullopt; + } + + std::vector data_bytes( + packet.begin() + 1, + packet.begin() + 17); // remove start and end character + + DiagnosticPacket raw{}; + std::memcpy( + &raw, data_bytes.data(), + sizeof(data_bytes)); // copy raw bytes into struct DiagnosticPacket + + DiagnosticData data{}; + // Decode packet in DiagnosticData Struct + data.TR_BLOCK[0] = static_cast(raw.TR_BLOCK & 0xFF); + data.TR_BLOCK[1] = static_cast((raw.TR_BLOCK) >> 8 & 0xFF); + data.BER = raw.BER; + data.SIGNAL_POWER = raw.SIGNAL_POWER; + data.NOISE_POWER = raw.SIGNAL_POWER; + data.PACKET_VALID[0] = static_cast(raw.PACKET_VALID & 0xFF); + data.PACKET_VALID[1] = static_cast((raw.PACKET_VALID) >> 8 & 0xFF); + data.PACKET_INVALID = raw.PACKET_INVALID; + data.GIT_REV = raw.GIT_REV; + data.TIME[0] = raw.TIME_L; + data.TIME[1] = raw.TIME_M; + data.TIME[2] = raw.TIME_H; + data.CHIP_ID[0] = static_cast(raw.CHIP_ID & 0xFF); + data.CHIP_ID[0] = static_cast((raw.CHIP_ID) >> 8 & 0xFF); + + uint8_t hw = raw.HW_CH_FLAGS; + data.HW_REV = hw & 0x03; + data.CHANNEL = hw & 0x3C; + data.TB_VALID = hw & 0x40; + data.TX_COMPLETE = hw & 0x80; + + uint8_t ml = raw.MODE_LEVEL_FLAGS; + data.DIAGNOSTIC_MODE = ml & 0x01; + data.POWER_LEVEL = ml & 0x0C; + + return data; +} + +void AcousticModemDriver::close() { + port_->close(); +} diff --git a/acoustic_modem/src/am_drone_node.cpp b/acoustic_modem/src/am_drone_node.cpp new file mode 100644 index 0000000..6b85f63 --- /dev/null +++ b/acoustic_modem/src/am_drone_node.cpp @@ -0,0 +1,58 @@ +#include "am_drone_node.hpp" + +DroneNode::DroneNode() : Node("drone_node") { + // TODO + set_subscriber(); + init_connection(); + + timer_ = this->create_wall_timer( + 5000ms, std::bind(&DroneNode::acoustic_callback, this)); +} + +void DroneNode::init_connection() { + this->declare_parameter("device"); + this->declare_parameter("baudrate", 9600); + this->declare_parameter("channel", 1); + this->declare_parameter("level", 4); + this->declare_parameter("diagnostic", false); + this->declare_parameter("timeout", 0.5); + + std::string device = this->get_parameter("device").as_string(); + int baudrate = this->get_parameter("baudrate").as_int(); + int channel = this->get_parameter("channel").as_int(); + int level = this->get_parameter("level").as_int(); + bool diagnostic = this->get_parameter("diagnostic").as_bool(); + float timeout = + static_cast(this->get_parameter("timeout").as_double()); + + drone_modem_ = AcousticModemDriver(device, baudrate, channel, level, + diagnostic, timeout); +} + +void DroneNode::set_subscriber() { + // Depends on the topic in which we will read the data + subscription_ = this->create_subscription( + "data_topic", 10, std::bind(&DroneNode::data_callback), this, + std::placeholders::_1); +} + +void DroneNode::acoustic_callback() { + // TODO + if (!latest_.empty()) { + drone_modem_.send_msg(latest_); + } +} + +void DroneNode::data_callback( + const std_msgs::msg::String::SharedPtr msg) const { + // TODO + latest_ = msg->data; +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/acoustic_modem/src/tdma_manager.cpp b/acoustic_modem/src/tdma_manager.cpp new file mode 100644 index 0000000..e69de29