diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..383b889 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.10.2) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(PACKAGE_VERSION 1.5.5) +project(positioning_systems_api VERSION ${PACKAGE_VERSION}) + +if(${CMAKE_BUILD_TYPE} MATCHES "Debug") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O0 -g3 -pthread -fPIC") +else() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O2 -g0 -pthread -fPIC") +endif() + +option(BUILD_TESTS "Builds tests" ON) +option(BUILD_EXAMPLES "Build short example binaries" ON) + +include(GNUInstallDirs) +set(project_config_in "${CMAKE_CURRENT_LIST_DIR}/cmake/positioning_systems_apiConfig.cmake.in") +set(project_config_out "${CMAKE_CURRENT_BINARY_DIR}/positioning_systems_apiConfig.cmake") +set(config_targets_file_serial_communication "serial_communication.cmake") +set(config_targets_file_follow_me_driver "follow_me_driver.cmake") +set(config_targets_file_rtls_driver "rtls_driver.cmake") +set(version_config_file "${CMAKE_CURRENT_BINARY_DIR}/positioning_systems_apiConfigVersion.cmake") +set(export_dest_dir "lib/cmake/positioning_systems_api") + +add_subdirectory(serial_communication) +add_subdirectory(follow_me_driver) +add_subdirectory(rtls_driver) +add_subdirectory(logger) +if(BUILD_TESTS) + enable_testing() + add_subdirectory(tests) +endif() + +if(BUILD_EXAMPLES) + add_subdirectory(examples) +endif() + +include(CMakePackageConfigHelpers) +configure_file("${project_config_in}" "${project_config_out}" @ONLY) +write_basic_package_version_file("${version_config_file}" COMPATIBILITY SameMajorVersion) + +install(FILES + "${project_config_out}" + "${version_config_file}" DESTINATION "${export_dest_dir}" +) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..7203bd4 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Terabee + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..2ea56f2 --- /dev/null +++ b/README.md @@ -0,0 +1,139 @@ +# positioning_systems_api + +This package comprises following modules described below: **serial_communication**, **follow_me_driver**, **rtls_driver**, **logger**. + +## serial_communication + +General purpose serial communication + +### Features + + - support for different baudrate, parity, bytesize (5, 6, 7, or 8 bits), stopbits and flow control + - implementation for Linux and Windows + +### Usage + +In order to use serial_communication in your code, include the interface header and appropriate implementation header, e.g. + +``` +#include "serial_communication/iserial.hpp" +#include "serial_communication/serial.hpp" +``` + +Add `positioning_systems_api` to `find_package` of your `CMakeLists.txt` and link the target against `serial_communication`: + +``` +target_link_libraries(target_name + positioning_systems_api::serial_communication +) +``` + +See subdirectory `examples` for a C++ source file presenting usage of `serial_communication`. + +## follow_me_driver + +Driver dedicated for communication with Terabee Follow-Me system + +### Features + + - set configuration parameters: span manual setting and auto-calibration, output mode (text/binary), swap beacons, Exponential Moving Average filter window size, + - configuration of RS485 connection parameters (slave address, baud rate, parity) + - set configuration parameters of the remote control: enable/disable buzzer, change button mode (toggle/hold) + - data reception (distance and heading) + +### Usage + +Include the following header: +``` +#include "follow_me_driver/follow_me_driver.hpp" +``` + +and header with appropriate implementation of serial port, e.g. +``` +#include "serial_communication/serial.hpp" +``` + +Add `positioning_systems_api` to `find_package` of your `CMakeLists.txt` and link the target against `follow_me_driver`: + +``` +target_link_libraries(target_name + positioning_systems_api::follow_me_driver +) +``` + +See subdirectory `examples` for a C++ source file presenting usage of `follow_me_driver`. + +## rtls_driver + +Driver dedicated for communication with Terabee Robot Positioning System + +### Features + +- setting all configuration parameters +- reading the whole configuration of device +- reading position output of the tracker + +### Usage + +Include the following header: +``` +#include "rtls_driver/rtls_driver.hpp" +``` + +and header with appropriate implementation of serial port, e.g. +``` +#include "serial_communication/serial.hpp" +``` + +Add `positioning_systems_api` to `find_package` of your `CMakeLists.txt` and link the target against `rtls_driver`: + +``` +target_link_libraries(target_name + positioning_systems_api::rtls_driver +) +``` + +See subdirectory `examples` for a C++ source file presenting usage of `rtls_driver`. + +## Compilation + +``` +mkdir build +cd build +cmake ../ +make +``` + +**Note:** + +If using MinGW in Windows, type + +`cmake .. -G "MinGW Makefiles"` instead of `cmake ../` + +to prevent configuring for MSVC. + +Use `cmake.exe --build . --target all -- -j` to build. + +## Installation in the system +``` +sudo make install +``` + +## Run tests + +``` +cd build +make test +``` +For tests to pass, linux null modem emulator is required https://github.com/freemed/tty0tty + +## Logging +To use the logger, set the following environment variables: +``` +export LOGGER_ENABLE_LOGGING=1 +export LOGGER_PRINT_STDOUT=1 +``` + +Set `export LOGGER_ENABLE_DEBUG=1` to enable debug logs. + +Set `export LOGGER_FILENAME=rtls_output.log` to change the default log file name. diff --git a/cmake/positioning_systems_apiConfig.cmake.in b/cmake/positioning_systems_apiConfig.cmake.in new file mode 100644 index 0000000..0a71536 --- /dev/null +++ b/cmake/positioning_systems_apiConfig.cmake.in @@ -0,0 +1,4 @@ +set(config_targets_file_serial_communication @config_targets_file_serial_communication@) +include("${CMAKE_CURRENT_LIST_DIR}/${config_targets_file_serial_communication}") +set(config_targets_file_follow_me_driver @config_targets_file_follow_me_driver@) +include("${CMAKE_CURRENT_LIST_DIR}/${config_targets_file_follow_me_driver}") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..3501453 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,45 @@ +project(positioning_systems_api_examples) + +add_executable(serial_example + serial_example.cpp +) + +target_link_libraries(serial_example + serial_communication +) + +add_executable(follow_me_master_example + follow_me_master_example.cpp +) + +target_link_libraries(follow_me_master_example + follow_me_driver + serial_communication +) + +add_executable(follow_me_remote_example + follow_me_remote_example.cpp +) + +target_link_libraries(follow_me_remote_example + follow_me_driver + serial_communication +) + +add_executable(rtls_anchor_example + rtls_anchor_example.cpp +) + +target_link_libraries(rtls_anchor_example + rtls_driver + serial_communication +) + +add_executable(rtls_tracker_example + rtls_tracker_example.cpp +) + +target_link_libraries(rtls_tracker_example + rtls_driver + serial_communication +) diff --git a/examples/follow_me_master_example.cpp b/examples/follow_me_master_example.cpp new file mode 100644 index 0000000..b9168c4 --- /dev/null +++ b/examples/follow_me_master_example.cpp @@ -0,0 +1,64 @@ +#include +#include +#include +#include +#include "follow_me_driver/follow_me_driver.hpp" + +#ifdef __linux__ + #define STOP_SIGNAL SIGTSTP + #include "serial_communication/serial.hpp" + using SerialInterface = terabee::serial_communication::Serial; +#elif defined(__MINGW32__) || defined(__MINGW64__) || defined(_WIN32) || defined(_WIN64) + #define STOP_SIGNAL SIGTERM + #include "serial_communication/serial_windows.hpp" + using SerialInterface = terabee::serial_communication::SerialWindows; +#endif + +static volatile std::sig_atomic_t g_signal_status; +void signal_handler(int signal) { + g_signal_status = signal; +} + +int main(int argc, char **argv) +{ + terabee::utility::logger::Logger logger("FollowMeMasterExample"); + if (argc != 2) + { + logger->info("usage: ./follow_me_master_example PORT_NAME"); + return -1; + } + std::signal(STOP_SIGNAL, signal_handler); + + std::string portname = argv[1]; + std::shared_ptr serial_port = + std::make_shared(portname); + + serial_port->setBaudrate(115200); + serial_port->setTimeout(std::chrono::milliseconds(200)); + + serial_port->open(); + + if (!serial_port->isOpen()) + { + logger->error("Failed to open serial port!"); + return 0; + } + + terabee::FollowMeMasterBeacon master_beacon(serial_port); + terabee::PolarPoint2D point = { 0.0, 0.0 }; + + master_beacon.swapBeacons(false); + master_beacon.setEMAWindow(10); + master_beacon.setBeaconsSpan(1000); + logger->info(master_beacon.retrieveDeviceData()); + + master_beacon.printoutModeBin(); + logger->info("Press Ctrl + Z to quit"); + while (g_signal_status != STOP_SIGNAL) + { + master_beacon.process(point); + logger->info("Distance: {}\tHeading: {}", point.distance, point.heading); + } + + return serial_port->close() ? 0 : -1; +} diff --git a/examples/follow_me_remote_example.cpp b/examples/follow_me_remote_example.cpp new file mode 100644 index 0000000..f491bcd --- /dev/null +++ b/examples/follow_me_remote_example.cpp @@ -0,0 +1,59 @@ +#include +#include +#include +#include +#include "follow_me_driver/follow_me_driver.hpp" + +#ifdef __linux__ + #define STOP_SIGNAL SIGTSTP + #include "serial_communication/serial.hpp" + using SerialInterface = terabee::serial_communication::Serial; +#elif defined(__MINGW32__) || defined(__MINGW64__) || defined(_WIN32) || defined(_WIN64) + #define STOP_SIGNAL SIGTERM + #include "serial_communication/serial_windows.hpp" + using SerialInterface = terabee::serial_communication::SerialWindows; +#endif + +static volatile std::sig_atomic_t g_signal_status; +void signal_handler(int signal) { + g_signal_status = signal; +} + +int main(int argc, char **argv) +{ + terabee::utility::logger::Logger logger("FollowMeRemoteExample"); + if (argc != 2) + { + logger->info("usage: ./follow_me_remote_example PORT_NAME"); + return -1; + } + std::signal(STOP_SIGNAL, signal_handler); + + std::string portname = argv[1]; + std::shared_ptr serial_port = + std::make_shared(portname); + + serial_port->setBaudrate(115200); + serial_port->setTimeout(std::chrono::milliseconds(700)); + + serial_port->open(); + + if (!serial_port->isOpen()) + { + logger->error("Failed to open serial port!"); + return 0; + } + + terabee::FollowMeRemoteControl remote_control(serial_port); + + remote_control.setBuzzer(false); + remote_control.setButtonMode(terabee::FollowMeRemoteControl::button_mode::hold); + + terabee::FollowMeRemoteControl::button_mode button_mode; + bool buzzer; + remote_control.retrieveRemoteParameters(button_mode, buzzer); + logger->info("Button mode: {}", static_cast(button_mode)); + logger->info("Buzzer: {}", buzzer); + + return serial_port->close() ? 0 : -1; +} diff --git a/examples/rtls_anchor_example.cpp b/examples/rtls_anchor_example.cpp new file mode 100644 index 0000000..b273964 --- /dev/null +++ b/examples/rtls_anchor_example.cpp @@ -0,0 +1,52 @@ +#include +#include +#include "rtls_driver/rtls_driver.hpp" + +#ifdef __linux__ + #include "serial_communication/serial.hpp" + using SerialInterface = terabee::serial_communication::Serial; +#elif defined(__MINGW32__) || defined(__MINGW64__) || defined(_WIN32) || defined(_WIN64) + #include "serial_communication/serial_windows.hpp" + using SerialInterface = terabee::serial_communication::SerialWindows; +#endif + +int main(int argc, char **argv) +{ + terabee::utility::logger::Logger logger("AnchorExample"); + if (argc != 2) + { + logger->info("usage: ./rtls_anchor_example PORT_NAME"); + return -1; + } + + std::string portname = argv[1]; + std::shared_ptr serial_port = + std::make_shared(portname); + + serial_port->setBaudrate(115200); + serial_port->setTimeout(std::chrono::milliseconds(800)); + + serial_port->open(); + + if (!serial_port->isOpen()) + { + logger->error("Failed to open serial port!"); + return 0; + } + + terabee::RtlsDevice rtls_device(serial_port); + + rtls_device.setDevice(terabee::RtlsDevice::device_type::anchor, 0); + rtls_device.setAnchorPosition(0, 0, 0); + rtls_device.setLabel(0x1ABE); + rtls_device.setUpdateTime(1); + rtls_device.setNetworkId(0xC0FE); + rtls_device.setAnchorHeightForAutoPositioning(1320); + rtls_device.enableAnchorInitiator(); + rtls_device.disableAutoAnchorPositioning(); + rtls_device.enableLED(); + rtls_device.requestConfig(); + terabee::RtlsDevice::config_t device_configuration = rtls_device.getConfig(); + + return serial_port->close() ? 0 : -1; +} diff --git a/examples/rtls_tracker_example.cpp b/examples/rtls_tracker_example.cpp new file mode 100644 index 0000000..cd10e69 --- /dev/null +++ b/examples/rtls_tracker_example.cpp @@ -0,0 +1,87 @@ +#include +#include +#include +#include +#include "rtls_driver/rtls_driver.hpp" + +#ifdef __linux__ + #define STOP_SIGNAL SIGTSTP + #include "serial_communication/serial.hpp" + using SerialInterface = terabee::serial_communication::Serial; +#elif defined(__MINGW32__) || defined(__MINGW64__) || defined(_WIN32) || defined(_WIN64) + #define STOP_SIGNAL SIGTERM + #include "serial_communication/serial_windows.hpp" + using SerialInterface = terabee::serial_communication::SerialWindows; +#endif + +static volatile std::sig_atomic_t g_signal_status; +void signal_handler(int signal) { + g_signal_status = signal; +} + +int main(int argc, char **argv) +{ + terabee::utility::logger::Logger logger("TrackerExample"); + if (argc != 2) + { + logger->info("Usage: ./rtls_tracker_example PORT_NAME"); + return -1; + } + + std::signal(STOP_SIGNAL, signal_handler); + std::string portname = argv[1]; + std::shared_ptr serial_port = + std::make_shared(portname); + + serial_port->setBaudrate(115200); + serial_port->setTimeout(std::chrono::milliseconds(800)); + + serial_port->open(); + + if (!serial_port->isOpen()) + { + logger->error("Failed to open serial port!"); + return 0; + } + + terabee::RtlsDevice rtls_device(serial_port); + + rtls_device.disableTrackerStream(); + serial_port->flushInput(); + rtls_device.setDevice(terabee::RtlsDevice::device_type::tracker, 1); + rtls_device.setLabel(0x1ABE); + rtls_device.setUpdateTime(1); + rtls_device.setNetworkId(0xC0FE); + rtls_device.setTrackerMessageLong(); + rtls_device.enableLED(); + rtls_device.requestConfig(); + terabee::RtlsDevice::config_t device_configuration = rtls_device.getConfig(); + + rtls_device.enableTrackerStream(); + + logger->info("Press Ctrl + Z to quit"); + + terabee::RtlsDevice::tracker_msg_t tracker_data; + rtls_device.registerOnDistanceDataCaptureCallback( + [&tracker_data, &logger](const terabee::RtlsDevice::tracker_msg_t& tracker_msg) + { + logger->info("T:{}", tracker_msg.timestamp); + if (tracker_msg.is_valid_position) + { + logger->info("Tracker position: ({}, {}, {})", + tracker_msg.tracker_position_xyz.at(0), + tracker_msg.tracker_position_xyz.at(1), + tracker_msg.tracker_position_xyz.at(2)); + } + tracker_data = tracker_msg; + }); + rtls_device.startReadingStream(); + + while (g_signal_status != STOP_SIGNAL) + { + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + } + + rtls_device.stopReadingStream(); + return serial_port->close() ? 0 : -1; +} diff --git a/examples/serial_example.cpp b/examples/serial_example.cpp new file mode 100644 index 0000000..8733faf --- /dev/null +++ b/examples/serial_example.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include "serial_communication/iserial.hpp" + +#ifdef __linux__ + #define STOP_SIGNAL SIGTSTP + #include "serial_communication/serial.hpp" + using SerialInterface = terabee::serial_communication::Serial; +#elif defined(__MINGW32__) || defined(__MINGW64__) || defined(_WIN32) || defined(_WIN64) + #define STOP_SIGNAL SIGTERM + #include "serial_communication/serial_windows.hpp" + using SerialInterface = terabee::serial_communication::SerialWindows; +#endif + +static volatile std::sig_atomic_t g_signal_status; +void signal_handler(int signal) { + g_signal_status = signal; +} + +int main(int argc, char **argv) +{ + terabee::utility::logger::Logger logger("SerialExample"); + if (argc != 2) + { + logger->info("usage: ./serial_example PORT_NAME"); + return -1; + } + std::signal(STOP_SIGNAL, signal_handler); + + size_t max_buffer_size = 10; + std::string portname = argv[1]; + std::shared_ptr serial_port = + std::make_shared(portname); + + serial_port->setBaudrate(115200); + serial_port->setTimeout(std::chrono::milliseconds(200)); + + serial_port->open(); + + if (!serial_port->isOpen()) + { + logger->error("Failed to open serial port!"); + return 0; + } + + logger->info("Reading from serial port: ", portname); + + logger->info("Press Ctrl + Z to quit"); + while (g_signal_status != STOP_SIGNAL) + { + logger->info(serial_port->readline(max_buffer_size)); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + return serial_port->close() ? 0 : -1; +} diff --git a/follow_me_driver/CMakeLists.txt b/follow_me_driver/CMakeLists.txt new file mode 100644 index 0000000..56488f2 --- /dev/null +++ b/follow_me_driver/CMakeLists.txt @@ -0,0 +1,37 @@ +project(follow_me_driver) +set(LIB_NAME follow_me_driver) + +add_library(${LIB_NAME} SHARED + src/follow_me_driver.cpp +) + +target_include_directories(${LIB_NAME} PUBLIC + $ + $ +) + +target_link_libraries(${LIB_NAME} + serial_communication +) + +set_target_properties(${LIB_NAME} PROPERTIES + VERSION ${PACKAGE_VERSION} +) + +install(DIRECTORY include/ + DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" + COMPONENT headers +) + +install(TARGETS ${LIB_NAME} + EXPORT ${LIB_NAME} + DESTINATION "${CMAKE_INSTALL_LIBDIR}" + COMPONENT libraries +) + +install(EXPORT ${LIB_NAME} + DESTINATION "${export_dest_dir}" + NAMESPACE positioning_systems_api:: + FILE ${config_targets_file_follow_me_driver} + COMPONENT configs +) diff --git a/follow_me_driver/include/follow_me_driver/follow_me_driver.hpp b/follow_me_driver/include/follow_me_driver/follow_me_driver.hpp new file mode 100644 index 0000000..2882e24 --- /dev/null +++ b/follow_me_driver/include/follow_me_driver/follow_me_driver.hpp @@ -0,0 +1,121 @@ +#ifndef FOLLOW_ME_DRIVER_H +#define FOLLOW_ME_DRIVER_H + +#include +#include +#include "serial_communication/iserial.hpp" +#include "logger/logger.hpp" + +namespace terabee { + +struct PolarPoint2D +{ + float distance; + float heading; +}; + +class FollowMeMasterBeacon +{ +public: + enum class printout_mode + { + text = 1, + binary = 2 + }; + + enum class rs485_parity + { + rs485_parity_none = 0, + rs485_parity_odd = 1, + rs485_parity_even = 2 + }; + + FollowMeMasterBeacon() = delete; + explicit FollowMeMasterBeacon(std::shared_ptr serialIf); + FollowMeMasterBeacon(const FollowMeMasterBeacon &other) = delete; + FollowMeMasterBeacon(FollowMeMasterBeacon &&other) = delete; + ~FollowMeMasterBeacon() = default; + + bool process(PolarPoint2D &point); + + bool printoutModeText(); + bool printoutModeBin(); + bool spanAutoCalibrate() const; + bool swapBeacons(bool is_swapped) const; + bool setBeaconsSpan(uint16_t span_mm) const; + bool setEMAWindow(uint8_t ema_window) const; + bool setRS485_Parameters(uint32_t baudrate, rs485_parity parity) const; + bool setRS485_SlaveId(uint8_t slave_id) const; + std::string retrieveDeviceData() const; + + static uint8_t calculateCommandLength(const uint8_t frame[]) { return (frame[1] & 0x0F) + 3; } + +private: + utility::logger::Logger logger_; + std::shared_ptr serial_port_; + const float MM_TO_M_FACTOR = 0.001F; + const float DEG_TO_RAD_FACTOR = M_PI/180.0; + bool binary_mode_ = true; + + static constexpr size_t CMD_ACK_LENGTH = 4; + static constexpr size_t CMD_PRINTOUT_LENGTH = 4; + static constexpr size_t CMD_SPAN_LENGTH = 5; + static constexpr size_t CMD_MODBUS_SLAVE_ID_LENGTH = 5; + static constexpr size_t CMD_EMA_WINDOW_LENGTH = 5; + static constexpr size_t CMD_TEST_LENGTH = 4; + static constexpr size_t CMD_SWAP_BEACONS_LENGTH = 4; + static constexpr size_t CMD_RS485_PARAMS_LENGTH = 8; + + static const std::array CMD_PRINTOUT_TEXT; + static const std::array CMD_PRINTOUT_BINARY; + static const std::array CMD_SPAN_AUTOCALIBRATE; + static const std::array CMD_TEST; + + static constexpr size_t SERIAL_READ_TEST_CMD_BUFFER_SIZE = 120; + static constexpr size_t SERIAL_READ_TEXT_BUFFER_SIZE = 20; + static constexpr size_t BINARY_FRAME_BUFFER_SIZE = 6; + std::array read_buffer_; + + bool processFrameBinary(PolarPoint2D &point); + bool processFrameText(PolarPoint2D &point) const; + + bool sendCommand(const uint8_t cmd[], size_t length) const; + bool writeCommand(const uint8_t cmd[], size_t length) const; + bool readACK(const uint8_t cmd[]) const; +}; + +class FollowMeRemoteControl +{ +public: + enum class button_mode + { + hold = 0, + toggle = 1 + }; + + FollowMeRemoteControl() = delete; + explicit FollowMeRemoteControl(std::shared_ptr serialIf); + FollowMeRemoteControl(const FollowMeRemoteControl &other) = delete; + FollowMeRemoteControl(FollowMeRemoteControl &&other) = delete; + ~FollowMeRemoteControl() = default; + + bool setButtonMode(button_mode mode) const; + bool setBuzzer(bool enabled) const; + bool retrieveRemoteParameters(button_mode &mode, bool &buzzer) const; + +private: + utility::logger::Logger logger_; + std::shared_ptr serial_port_; + + static constexpr size_t CMD_ACK_LENGTH = 6; + static constexpr size_t CMD_BUTTON_MODE_LENGTH = 3; + static constexpr size_t CMD_BUZZER_MODE_LENGTH = 3; + static constexpr size_t CMD_GET_PARAMS_LENGTH = 3; + + bool writeCommandCheckACK(const uint8_t frame[], size_t size) const; + bool readACK() const; +}; + +} // namespace terabee + +#endif // FOLLOW_ME_DRIVER_H diff --git a/follow_me_driver/src/follow_me_driver.cpp b/follow_me_driver/src/follow_me_driver.cpp new file mode 100644 index 0000000..c19140d --- /dev/null +++ b/follow_me_driver/src/follow_me_driver.cpp @@ -0,0 +1,399 @@ +#include +#include "follow_me_driver/follow_me_driver.hpp" +#include "serial_communication/crc.hpp" + +namespace terabee { + +const std::array + FollowMeMasterBeacon::CMD_PRINTOUT_TEXT = {0x00, 0x11, 0x01, 0x45}; + +const std::array + FollowMeMasterBeacon::CMD_PRINTOUT_BINARY = {0x00, 0x11, 0x02, 0x4C}; + +const std::array + FollowMeMasterBeacon::CMD_SPAN_AUTOCALIBRATE = {0x00, 0x22, 0x00, 0x00, 0x95}; + +const std::array + FollowMeMasterBeacon::CMD_TEST = {0x00, 0x00, 0x00, 0x00}; + + + +FollowMeMasterBeacon::FollowMeMasterBeacon(std::shared_ptr serialIf): + logger_("FollowMeMasterBeacon"), + serial_port_(serialIf) +{ +} + +bool FollowMeMasterBeacon::process(PolarPoint2D &point) +{ + if (binary_mode_) + { + return processFrameBinary(point); + } + else + { + return processFrameText(point); + } +} + +bool FollowMeMasterBeacon::printoutModeText() +{ + if (!sendCommand(CMD_PRINTOUT_TEXT.data(), CMD_PRINTOUT_TEXT.size())) + { + logger_->error("Failed to set output mode: Text!"); + return false; + } + + binary_mode_ = false; + logger_->info("Output mode: Text"); + return true; +} + +bool FollowMeMasterBeacon::printoutModeBin() +{ + if (!sendCommand(CMD_PRINTOUT_BINARY.data(), CMD_PRINTOUT_BINARY.size())) + { + logger_->error("Failed to set output mode: Binary!"); + return false; + } + + binary_mode_ = true; + logger_->info("Output mode: Binary"); + return true; +} + +bool FollowMeMasterBeacon::spanAutoCalibrate() const +{ + if (!sendCommand(CMD_SPAN_AUTOCALIBRATE.data(), CMD_SPAN_AUTOCALIBRATE.size())) + { + logger_->error("Failed to initialize span autocalibration!"); + return false; + } + logger_->info("Span autocalibration executed"); + return true; +} + +bool FollowMeMasterBeacon::swapBeacons(bool is_swapped) const +{ + std::array write_buffer = + { 0x00, 0x31, is_swapped, 0x00 }; + + write_buffer.at(write_buffer.size() - 1) = + crc::crc8(write_buffer.data(), write_buffer.size() - 1); + + if (!sendCommand(write_buffer.data(), write_buffer.size())) + { + logger_->error("Failed to swap beacons!"); + return false; + } + logger_->info("Beacons swapped: {}", static_cast(is_swapped)); + return true; +} + +bool FollowMeMasterBeacon::setBeaconsSpan(uint16_t span_mm) const +{ + uint8_t span_mm_hi = span_mm >> 8; + uint8_t span_mm_lo = span_mm & 0x00FF; + + std::array write_buffer = + { 0x00, 0x22, span_mm_hi, span_mm_lo, 0x00 }; + + write_buffer.at(write_buffer.size() - 1) = + crc::crc8(write_buffer.data(), write_buffer.size() - 1); + + if (!sendCommand(write_buffer.data(), write_buffer.size())) + { + logger_->error("Failed to set beacons span!"); + return false; + } + logger_->info("Beacons span: {}", static_cast(span_mm)); + return true; +} + +bool FollowMeMasterBeacon::setEMAWindow(uint8_t ema_window) const +{ + std::array write_buffer = + {0x00, 0x52, 0x01, ema_window, 0x00}; + + write_buffer.at(write_buffer.size() - 1) = + crc::crc8(write_buffer.data(), write_buffer.size() - 1); + + if (!sendCommand(write_buffer.data(), write_buffer.size())) + { + logger_->error("Failed to set EMA window!"); + return false; + } + logger_->info("EMA window: {}", static_cast(ema_window)); + return true; +} + +bool FollowMeMasterBeacon::setRS485_Parameters(uint32_t baudrate, rs485_parity parity) const +{ + std::array write_buffer = + { 0x00, 0x55, 0x02, + static_cast(baudrate >> 16), + static_cast(baudrate >> 8), + static_cast(baudrate >> 0), + static_cast(parity), + 0x00 + }; + + write_buffer.at(write_buffer.size() - 1) = + crc::crc8(write_buffer.data(), write_buffer.size() - 1); + + if (!sendCommand(write_buffer.data(), write_buffer.size())) + { + logger_->error("Failed to set RS485 parameters!"); + return false; + } + + logger_->info("RS485 parameters set"); + return true; +} + +bool FollowMeMasterBeacon::setRS485_SlaveId(uint8_t slave_id) const +{ + std::array write_buffer_slave_id = + { 0x00, 0x52, 0x03, slave_id, 0x00 }; + + write_buffer_slave_id.at(write_buffer_slave_id.size() - 1) = + crc::crc8(write_buffer_slave_id.data(), write_buffer_slave_id.size() - 1); + + if (!sendCommand(write_buffer_slave_id.data(), write_buffer_slave_id.size())) + { + logger_->error("Failed to set RS485 slave id!"); + return false; + } + + logger_->info("RS485 slave id set"); + return true; +} + +bool FollowMeMasterBeacon::processFrameBinary(PolarPoint2D &point) +{ + size_t bytes_read = serial_port_->read(read_buffer_.data(), read_buffer_.size()); + + if (bytes_read <= 0) + { + return false; + } + + if (bytes_read != read_buffer_.size() || read_buffer_[0] != 'F' || read_buffer_[1] != 'M') + { + logger_->error("Frame incorrect: {} bytes; Header: {0:x}{1:x}!", bytes_read, read_buffer_[0], read_buffer_[1]); + return false; + } + + if (crc::crc8(read_buffer_.data(), read_buffer_.size() - 1) != + read_buffer_.back()) + { + logger_->error("CRC incorrect!"); + return false; + } + + point.distance = MM_TO_M_FACTOR*static_cast((read_buffer_[2] << 8) + read_buffer_[3]); + auto heading_read = static_cast(read_buffer_[4]); + point.heading = DEG_TO_RAD_FACTOR*static_cast(heading_read); + + return true; +} + +bool FollowMeMasterBeacon::processFrameText(PolarPoint2D &point) const +{ + std::string data = serial_port_->readline(SERIAL_READ_TEXT_BUFFER_SIZE, '\n'); + if (data.empty()) + { + return false; + } + + size_t distance_pos = data.find('\t') + 1; + size_t heading_pos = data.find('\t', distance_pos) + 1; + + if (data.substr(0, distance_pos) != "FM\t") + { + return false; + } + + point.distance = MM_TO_M_FACTOR*std::stof(data.substr(distance_pos, heading_pos)); + point.heading = DEG_TO_RAD_FACTOR*std::stof(data.substr(heading_pos)); + + return true; +} + +std::string FollowMeMasterBeacon::retrieveDeviceData() const +{ + if (!writeCommand(CMD_TEST.data(), CMD_TEST.size())) + { + logger_->error("Failed to retrieve device information!"); + return ""; + } + uint8_t eol; + serial_port_->read(&eol, 1); + return serial_port_->readline(SERIAL_READ_TEST_CMD_BUFFER_SIZE); +} + +bool FollowMeMasterBeacon::sendCommand(const uint8_t cmd[], size_t length) const +{ + if (!writeCommand(cmd, length)) + { + return false; + } + return readACK(cmd); +} + +bool FollowMeMasterBeacon::writeCommand(const uint8_t cmd[], size_t length) const +{ + if(!serial_port_->write(cmd, length)) + { + logger_->error("Timeout or error while writing to serial!"); + return false; + } + return true; +} + +bool FollowMeMasterBeacon::readACK(const uint8_t cmd[]) const +{ + std::array ack_buffer = {0, 0, 0, 0}; + size_t bytes_read = serial_port_->read(ack_buffer.data(), ack_buffer.size()); + if (bytes_read == ack_buffer.size() && + ack_buffer[0] == cmd[0] && + ack_buffer[1] == (cmd[1] >> 4) && + ack_buffer[2] == 0x00 && + ack_buffer[3] == crc::crc8(ack_buffer.data(), ack_buffer.size() - 1)) + { + return true; + } + + logger_->error("ACK error. Received: 0x{0:x}, 0x{1:x}, 0x{2:x}, 0x{3:x}", + static_cast(ack_buffer[0]), + static_cast(ack_buffer[1]), + static_cast(ack_buffer[2]), + static_cast(ack_buffer[3]) + ); + + return false; +} + +FollowMeRemoteControl::FollowMeRemoteControl(std::shared_ptr serialIf): + logger_("FollowMeRemoteControl"), + serial_port_(serialIf) +{ +} + +bool FollowMeRemoteControl::setButtonMode(button_mode mode) const +{ + std::array remote_write = { 0x08, 0x00, 0x00 }; + bool success = false; + + switch (mode) + { + case button_mode::toggle: + remote_write.at(1) = 0x01; + remote_write.at(2) = 0xAF; + success = writeCommandCheckACK(remote_write.data(), remote_write.size()); + break; + case button_mode::hold: + remote_write.at(1) = 0x00; + remote_write.at(2) = 0xA8; + success = writeCommandCheckACK(remote_write.data(), remote_write.size()); + break; + default: + logger_->error("Unsupported remote control button mode!"); + success = false; + break; + } + + if (success) + { + logger_->info("Button mode changed to {}", (mode == button_mode::toggle ? "toggle" : "hold")); + } + return success; +} + +bool FollowMeRemoteControl::setBuzzer(bool enabled) const +{ + std::array remote_write = { 0x09, 0x00, 0x00 }; + if (enabled) + { + remote_write.at(1) = 0x01; + remote_write.at(2) = 0xBA; + } + else + { + remote_write.at(1) = 0x00; + remote_write.at(2) = 0xBD; + } + + bool success = writeCommandCheckACK(remote_write.data(), remote_write.size()); + + if (success) + { + logger_->info("Buzzer {}", (enabled ? "activated" : "deactivated")); + } + return success; +} + +bool FollowMeRemoteControl::retrieveRemoteParameters(FollowMeRemoteControl::button_mode &mode, bool &buzzer) const +{ + std::array remote_write = { 0x04, 0x00, 0x54 }; + std::array response_buffer = {}; + + if (!serial_port_->write(remote_write.data(), remote_write.size())) + { + logger_->error("Timeout or error while writing to serial!"); + return false; + } + + size_t bytes_read = serial_port_->read(response_buffer.data(), response_buffer.size()); + if (bytes_read == response_buffer.size() && + response_buffer.at(0) == 0x40 && + response_buffer.at(response_buffer.size() - 1) == + crc::crc8(response_buffer.data(), response_buffer.size() - 1)) + { + buzzer = response_buffer.at(1) & 0x0F; + mode = static_cast(response_buffer.at(1) >> 4); + return true; + } + return false; +} + +bool FollowMeRemoteControl::writeCommandCheckACK(const uint8_t frame[], size_t size) const +{ + if (!serial_port_->write(frame, size)) + { + logger_->error("Timeout or error while writing to serial!"); + return false; + } + + if (!readACK()) + { + return false; + } + + return true; +} + +bool FollowMeRemoteControl::readACK() const +{ + std::array ack_buffer; + size_t bytes_read = serial_port_->read(ack_buffer.data(), ack_buffer.size()); + + if (bytes_read < ack_buffer.size()) + { + logger_->error("ACK Error: Not enough bytes read!"); + return false; + } + + for (uint8_t el : ack_buffer) + { + if (el != 0x00) + { + logger_->error("ACK Error: NACK!"); + return false; + } + } + + return true; +} + +} diff --git a/logger/CMakeLists.txt b/logger/CMakeLists.txt new file mode 100644 index 0000000..7834019 --- /dev/null +++ b/logger/CMakeLists.txt @@ -0,0 +1,54 @@ +project(logger) +set(LIB_NAME logger) + +# Download and unpack spdlog at configure time +configure_file(spdlog_download.in ${CMAKE_BINARY_DIR}/spdlog-download/CMakeLists.txt) +execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/spdlog-download ) +if(result) + message(FATAL_ERROR "CMake step for spdlog failed: ${result}") +endif() +execute_process(COMMAND ${CMAKE_COMMAND} --build . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/spdlog-download ) +if(result) + message(FATAL_ERROR "Build step for spdlog failed: ${result}") +endif() + +add_subdirectory(${CMAKE_BINARY_DIR}/spdlog-src + ${CMAKE_BINARY_DIR}/spdlog-build + EXCLUDE_FROM_ALL +) + +set(MODULE_LIBS spdlog_header_only) +set_target_properties(spdlog_header_only PROPERTIES EXPORT_NAME "positioning_systems_api_spdlog_header_only") + +add_library(${LIB_NAME} + src/logger.cpp +) + +target_include_directories(${LIB_NAME} PUBLIC + $ + $ +) + +target_link_libraries(${LIB_NAME} ${MODULE_LIBS}) + +install(DIRECTORY include/ + DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" + COMPONENT headers +) + +install(TARGETS ${LIB_NAME} spdlog_header_only + EXPORT ${LIB_NAME} + DESTINATION "${CMAKE_INSTALL_LIBDIR}" + COMPONENT libraries +) + +install(EXPORT ${LIB_NAME} + DESTINATION "${export_dest_dir}" + NAMESPACE positioning_systems_api:: + FILE ${config_targets_file_logger} + COMPONENT configs +) diff --git a/logger/include/logger/logger.hpp b/logger/include/logger/logger.hpp new file mode 100644 index 0000000..2573b1f --- /dev/null +++ b/logger/include/logger/logger.hpp @@ -0,0 +1,52 @@ +#ifndef LOGGER_HPP +#define LOGGER_HPP + +/** + * @Author Pawel Ptasznik + * + * @Copyright Terabee 2019 + * + */ + +#include +#include + +#include +#include + +#define LOGGER_ENABLE_LOGGING "LOGGER_ENABLE_LOGGING" // this env variable has to be set in order to enable logger at all NOLINT +#define LOGGER_PRINT_STDOUT "LOGGER_PRINT_STDOUT" // set this env variable to 1 to enable prints on stdout NOLINT +#define LOGGER_ENABLE_DEBUG "LOGGER_ENABLE_DEBUG" // set this env variable to 1 to enable debug logs NOLINT + +namespace terabee +{ +namespace utility +{ +namespace logger +{ + +struct LoggerParameterInitializer +{ + LoggerParameterInitializer(); +}; + +class Logger +{ +public: + typedef std::shared_ptr loggerPtr; + Logger() = delete; + Logger(const Logger& l) = default; + explicit Logger(const std::string& name); + loggerPtr operator->() const { + return logger_; + } + private: + static const LoggerParameterInitializer initializer_; + loggerPtr logger_; +}; + +} // namespace logger +} // namespace utility +} // namespace terabee + +#endif // LOGGER_HPP diff --git a/logger/spdlog_download.in b/logger/spdlog_download.in new file mode 100644 index 0000000..0151670 --- /dev/null +++ b/logger/spdlog_download.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10.2) + +project(spdlog-download NONE) + +include(ExternalProject) +ExternalProject_Add(spdlog + GIT_REPOSITORY https://github.com/gabime/spdlog.git + GIT_TAG v1.4.2 + SOURCE_DIR "${CMAKE_BINARY_DIR}/spdlog-src" + BINARY_DIR "${CMAKE_BINARY_DIR}/spdlog-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/logger/src/logger.cpp b/logger/src/logger.cpp new file mode 100644 index 0000000..499cf4e --- /dev/null +++ b/logger/src/logger.cpp @@ -0,0 +1,81 @@ +/** + * @Author Pawel Ptasznik + * + * @Copyright Terabee 2019 + * + */ + +#include "logger/logger.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#define LOGGER_FILENAME "TerabeeLog.log" +#define LOGGER_MAX_FILE_SIZE 1024*1024*64 // using 64MB log files +#define LOGGER_MAX_NUMBER_OF_FILES 20 // let's say that 20 files (64MB each) is enough + +namespace terabee +{ +namespace utility +{ +namespace logger +{ + +namespace +{ +std::shared_ptr g_file_sink; +} + +LoggerParameterInitializer::LoggerParameterInitializer() { + // If LOGGER_ENABLE_LOGGING is not set, no file is created + g_file_sink = nullptr; + if (!std::getenv(LOGGER_ENABLE_LOGGING)) { + spdlog::set_level(spdlog::level::off); + return; + } + // Set the level of logging + if (std::getenv(LOGGER_ENABLE_DEBUG)) { + spdlog::set_level(spdlog::level::debug); + } else { + spdlog::set_level(spdlog::level::info); + } + // If LOGGER_PRINT_STDOUT not set, create the logging file + if (!std::getenv(LOGGER_PRINT_STDOUT)) { + if (!std::getenv("LOGGER_FILENAME")) { + g_file_sink = std::make_shared( + LOGGER_FILENAME, LOGGER_MAX_FILE_SIZE, LOGGER_MAX_NUMBER_OF_FILES); + } else { + g_file_sink = std::make_shared( + std::getenv("LOGGER_FILENAME"), LOGGER_MAX_FILE_SIZE, + LOGGER_MAX_NUMBER_OF_FILES); + } + } + spdlog::set_pattern( + "%^[%Y-%m-%d %H:%M:%S.%e] [%l] [%n] : %v%$"); +} + +Logger::Logger(const std::string& name) { + logger_ = spdlog::get(name); + if (logger_) return; + if (std::getenv(LOGGER_PRINT_STDOUT)) { + logger_ = spdlog::stdout_color_mt(name); + } else if (std::getenv(LOGGER_ENABLE_LOGGING)) { + logger_ = std::make_shared(name, g_file_sink); + spdlog::initialize_logger(logger_); + } else { + logger_ = std::make_shared(name); + spdlog::initialize_logger(logger_); + } +} + +const LoggerParameterInitializer Logger::initializer_ = LoggerParameterInitializer(); + +} // namespace logger +} // namespace utility +} // namespace terabee diff --git a/rtls_driver/CMakeLists.txt b/rtls_driver/CMakeLists.txt new file mode 100644 index 0000000..b195b66 --- /dev/null +++ b/rtls_driver/CMakeLists.txt @@ -0,0 +1,38 @@ +project(rtls_driver) +set(LIB_NAME rtls_driver) + +add_library(${LIB_NAME} SHARED + src/rtls_driver.cpp +) + +target_include_directories(${LIB_NAME} PUBLIC + $ + $ +) + +target_link_libraries(${LIB_NAME} + serial_communication + logger +) + +set_target_properties(${LIB_NAME} PROPERTIES + VERSION ${PACKAGE_VERSION} +) + +install(DIRECTORY include/ + DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" + COMPONENT headers +) + +install(TARGETS ${LIB_NAME} + EXPORT ${LIB_NAME} + DESTINATION "${CMAKE_INSTALL_LIBDIR}" + COMPONENT libraries +) + +install(EXPORT ${LIB_NAME} + DESTINATION "${export_dest_dir}" + NAMESPACE positioning_systems_api:: + FILE ${config_targets_file_rtls_driver} + COMPONENT configs +) diff --git a/rtls_driver/include/rtls_driver/rtls_driver.hpp b/rtls_driver/include/rtls_driver/rtls_driver.hpp new file mode 100644 index 0000000..3b69947 --- /dev/null +++ b/rtls_driver/include/rtls_driver/rtls_driver.hpp @@ -0,0 +1,218 @@ +#ifndef RTLS_DRIVER_HPP +#define RTLS_DRIVER_HPP + +#include +#include +#include +#include +#include "serial_communication/iserial.hpp" +#include "logger/logger.hpp" + +namespace terabee { + +class RtlsDevice +{ +public: + enum class device_type + { + anchor = 0, + tracker = 1 + }; + + enum class tracker_msg_mode + { + msg_short = 0, + msg_long = 1 + }; + + struct anchor_data_t + { + uint8_t number; + uint16_t id; + int32_t pos_x; + int32_t pos_y; + int32_t pos_z; + int32_t distance; + }; + + struct tracker_msg_t + { + uint32_t timestamp = 0; + std::vector anchors_data; + std::array tracker_position_xyz = {0, 0, 0}; + bool is_valid_position = false; + }; + + enum class cmd_header + { + device_anchor_priority = 0x00, + device_tracker_priority = 0x01, + device_label = 0x02, + network_id = 0x03, + update_time = 0x04, + auto_anchor_positioning = 0x05, + anchor_position = 0x06, + anchor_initiator = 0x07, + anchor_height_aap = 0x08, + tracker_message_mode = 0x09, + activate_leds = 0x10, + reset_module = 0x99 + }; + + struct config_t + { + device_type type; + std::string fw_version; + std::string serial_id; + bool is_initiator; + uint16_t priority; + uint16_t label; + uint16_t network_id; + uint16_t update_time; + bool is_auto_anchor_positioning; + int32_t anchor_height; + bool is_tracker_stream_mode; + tracker_msg_mode tracker_message_mode; + bool is_led_enabled; + int32_t x; + int32_t y; + int32_t z; + }; + + using OnTrackerDataCallback = std::function; + + static constexpr size_t TRACKER_STREAM_SHORT_LEN = 4; + static constexpr size_t TRACKER_STREAM_LONG_1_ANCHOR_LEN = + TRACKER_STREAM_SHORT_LEN + 6; + static constexpr size_t TRACKER_STREAM_LONG_2_ANCHOR_LEN = + TRACKER_STREAM_LONG_1_ANCHOR_LEN + 6; + static constexpr size_t TRACKER_STREAM_LONG_3_ANCHOR_LEN = + TRACKER_STREAM_LONG_2_ANCHOR_LEN + 6; + static constexpr size_t TRACKER_STREAM_LONG_4_ANCHOR_LEN = + TRACKER_STREAM_LONG_3_ANCHOR_LEN + 6; + static constexpr size_t TRACKER_STREAM_MAX_ANCHOR_NUMBER = 4; + + static constexpr size_t CMD_ACK_LENGTH = 5; + static constexpr size_t CMD_STD_LEN = 3; + static constexpr size_t CMD_16B_DATA_LENGTH = 4; + static constexpr size_t CMD_ANCHOR_POSITION_LENGTH = 14; + static constexpr size_t CMD_ANCHOR_HEIGHT_AAP_LENGTH = 6; + static const std::array auto_anchor_positioning_enable_; + static const std::array auto_anchor_positioning_disable_; + static const std::array anchor_initiator_enable_; + static const std::array anchor_initiator_disable_; + static const std::array led_enable_; + static const std::array led_disable_; + static const std::array restart_device_; + static const std::array request_config_; + static const std::array tracker_msg_short_; + static const std::array tracker_msg_long_; + static const std::array tracker_stream_enable_; + static const std::array tracker_stream_disable_; + + RtlsDevice() = delete; + explicit RtlsDevice(std::shared_ptr serialIf, + int cmd_send_interval_ms = 200); + RtlsDevice(const RtlsDevice &other) = delete; + RtlsDevice(RtlsDevice &&other) = delete; + ~RtlsDevice(); + + bool setDevice(device_type dev_type, uint16_t priority); + bool setLabel(uint16_t label); + bool setNetworkId(uint16_t network_id); + bool setUpdateTime(uint16_t time_n100ms); + bool setAnchorPosition(int32_t x, int32_t y, int32_t z); + bool setAnchorHeightForAutoPositioning(int32_t height); + bool enableAnchorInitiator(); + bool disableAnchorInitiator(); + bool enableAutoAnchorPositioning(); + bool disableAutoAnchorPositioning(); + bool enableLED(); + bool disableLED(); + bool restartDevice(); + bool requestConfig(); + bool setTrackerMessageShort(); + bool setTrackerMessageLong(); + bool enableTrackerStream(); + void disableTrackerStream(); + config_t getConfig() const { return readback_config_; } + static std::vector splitString(const std::string &input_line, char delimiter); + + bool registerOnDistanceDataCaptureCallback(OnTrackerDataCallback cb); + bool startReadingStream(); + bool stopReadingStream(); + +private: + utility::logger::Logger logger_; + config_t readback_config_; + std::chrono::milliseconds command_send_interval_ms_; + + OnTrackerDataCallback tracker_data_callback_; + std::thread tracker_data_thread_; + std::atomic_bool stop_; + + std::shared_ptr serial_port_; + const float MM_TO_M_FACTOR = 0.001F; + const float DEG_TO_RAD_FACTOR = M_PI/180.0; + + using ConfigOutputMap = std::map; + + enum class device_params + { + type, + fw_version, + serial_id, + is_initiator, + priority, + label, + network_id, + update_time, + is_auto_anchor_positioning, + anchor_height, + is_tracker_stream_mode, + tracker_message_mode, + is_led_enabled, + x, + y, + z, + is_uart_ready + }; + + const std::map PARAM_TO_STRING + { + { device_params::type, "Device Type" }, + { device_params::serial_id, "Serial ID" }, + { device_params::fw_version, "TERABEE UWB 3D RTLS Firmware Version" }, + { device_params::is_initiator, "Initiator Mode" }, + { device_params::priority, "Device Priority" }, + { device_params::label, "Device Label" }, + { device_params::network_id, "Network ID" }, + { device_params::update_time, "Network Update Time[ms]" }, + { device_params::is_auto_anchor_positioning, "Auto Anchor Positioning (AAP)" }, + { device_params::anchor_height, "Anchor Height for AAP [mm]" }, + { device_params::is_tracker_stream_mode, "Tracker Stream Mode" }, + { device_params::tracker_message_mode, "Tracker Message Mode" }, + { device_params::is_led_enabled, "LED Mode" }, + { device_params::x, "Device Position X [mm]" }, + { device_params::y, "Device Position Y [mm]" }, + { device_params::z, "Device Position Z [mm]" } + }; + + bool send16bDataCommand(cmd_header header, uint16_t data); + bool sendRawCommand(const uint8_t cmd[], size_t length); + bool readACK() const; + + bool readConfig(); + void updateConfigFromMap(const ConfigOutputMap &config_map); + bool parseBool(std::string param); + int32_t parseDecInt32(std::string coordinate); + uint16_t parseHexUint16(std::string param); + uint16_t parseDecUint16(std::string update_time); + device_type parseDeviceType(std::string device_type_string); + tracker_msg_mode parseTrackerMsgMode(std::string tracker_msg_mode); + void readTrackerStream(); +}; + +} // namespace terabee + +#endif // RTLS_DRIVER_HPP diff --git a/rtls_driver/src/rtls_driver.cpp b/rtls_driver/src/rtls_driver.cpp new file mode 100644 index 0000000..90a657e --- /dev/null +++ b/rtls_driver/src/rtls_driver.cpp @@ -0,0 +1,698 @@ +#include +#include +#include +#include "rtls_driver/rtls_driver.hpp" +#include "serial_communication/crc.hpp" + +namespace terabee { + +const std::array + RtlsDevice::auto_anchor_positioning_enable_ = {0x05, 0x01, 0x46}; + +const std::array + RtlsDevice::auto_anchor_positioning_disable_{0x05, 0x00, 0x41}; + +const std::array + RtlsDevice::anchor_initiator_enable_{0x07, 0x01, 0x6C}; + +const std::array + RtlsDevice::anchor_initiator_disable_{0x07, 0x00, 0x6B}; + +const std::array + RtlsDevice::led_enable_{0x10, 0x01, 0x50}; + +const std::array + RtlsDevice::led_disable_{0x10, 0x00, 0x57}; + +const std::array + RtlsDevice::restart_device_{0x99, 0x99, 0x9A}; + +const std::array + RtlsDevice::request_config_{0x91, 0xDF, 0xE7}; + +const std::array + RtlsDevice::tracker_msg_short_{0x09, 0x00, 0xBD}; + +const std::array + RtlsDevice::tracker_msg_long_{0x09, 0x01, 0xBA}; + +const std::array + RtlsDevice::tracker_stream_enable_{0x11, 0x01, 0x45}; + +const std::array + RtlsDevice::tracker_stream_disable_{0x11, 0x00, 0x42}; + +RtlsDevice::RtlsDevice(std::shared_ptr serialIf, + int cmd_send_interval_ms): + logger_("RtlsDevice"), + command_send_interval_ms_(cmd_send_interval_ms), + tracker_data_thread_(), + stop_(true), + serial_port_(serialIf) +{ + logger_->debug("RtlsDevice::RtlsDevice(std::shared_ptr)"); +} + +RtlsDevice::~RtlsDevice() +{ + if (tracker_data_thread_.joinable()) { + tracker_data_thread_.join(); + } + logger_->debug("RtlsDevice::~RtlsDevice()"); +} + +bool RtlsDevice::setDevice(RtlsDevice::device_type dev_type, uint16_t priority) +{ + if (!send16bDataCommand(static_cast(dev_type), priority)) + { + logger_->error("Failed to set device type and priority!"); + return false; + } + readback_config_.type = dev_type; + readback_config_.priority = priority; + logger_->info("Device type set to: {}, priority: {}", static_cast(dev_type), priority); + return true; +} + +bool RtlsDevice::setLabel(uint16_t label) +{ + if (!send16bDataCommand(cmd_header::device_label, label)) + { + logger_->error("Failed to set device label!"); + return false; + } + + readback_config_.label = label; + logger_->info("Device label set to: 0x{0:x}", label); + return true; +} + +bool RtlsDevice::setNetworkId(uint16_t network_id) +{ + if (!send16bDataCommand(cmd_header::network_id, network_id)) + { + logger_->error("Failed to set network ID!"); + return false; + } + + readback_config_.network_id = network_id; + logger_->info("Network ID set to: 0x{0:x}", network_id); + return true; +} + +/* + * Sets update time with multiples of 100ms + * E.g. time_n100ms = 3 gives update time of 300ms + */ +bool RtlsDevice::setUpdateTime(uint16_t time_n100ms) +{ + if (time_n100ms < 1 || time_n100ms > 600) + { + logger_->error("Update time parameter must fit in range <1;600>!"); + return false; + } + if (!send16bDataCommand(cmd_header::update_time, time_n100ms)) + { + logger_->error("Failed to set update time!"); + return false; + } + + readback_config_.update_time = time_n100ms; + logger_->info("Update time set to: {}", time_n100ms*100); + return true; +} + +bool RtlsDevice::setAnchorPosition(int32_t x, int32_t y, int32_t z) +{ + if (readback_config_.type != device_type::anchor) + { + logger_->error("Device is not configured as anchor!"); + return false; + } + + std::array write_buffer = { + static_cast(cmd_header::anchor_position), + static_cast(x >> 24), + static_cast(x >> 16), + static_cast(x >> 8), + static_cast(x >> 0), + static_cast(y >> 24), + static_cast(y >> 16), + static_cast(y >> 8), + static_cast(y >> 0), + static_cast(z >> 24), + static_cast(z >> 16), + static_cast(z >> 8), + static_cast(z >> 0), + 0x0, // CRC8 placeholder + }; + write_buffer.at(write_buffer.size() - 1) = + crc::crc8(write_buffer.data(), write_buffer.size() - 1); + + if (!sendRawCommand(write_buffer.data(), write_buffer.size())) + { + logger_->error("Failed to set anchor position!"); + return false; + } + + readback_config_.x = x; + readback_config_.y = y; + readback_config_.z = z; + logger_->info("Anchor position set to: ({}, {}, {}) mm", x, y, z); + return true; +} + +bool RtlsDevice::setAnchorHeightForAutoPositioning(int32_t height) +{ + if (readback_config_.type != device_type::anchor) + { + logger_->error("Device is not configured as anchor!"); + return false; + } + + std::array write_buffer = { + static_cast(cmd_header::anchor_height_aap), + static_cast(height >> 24), + static_cast(height >> 16), + static_cast(height >> 8), + static_cast(height >> 0), + 0x0, // CRC8 placeholder + }; + write_buffer.at(write_buffer.size() - 1) = + crc::crc8(write_buffer.data(), write_buffer.size() - 1); + + if (!sendRawCommand(write_buffer.data(), write_buffer.size())) + { + logger_->error("Failed to set anchor height!"); + return false; + } + + readback_config_.anchor_height = height; + logger_->info("Anchor height set to: {} mm", height); + return true; +} + +bool RtlsDevice::enableAnchorInitiator() +{ + if (readback_config_.type != device_type::anchor) + { + logger_->error("Device is not configured as anchor!"); + return false; + } + + if (!sendRawCommand(anchor_initiator_enable_.data(), + anchor_initiator_enable_.size())) + { + logger_->error("Failed to enable anchor initiator!"); + return false; + } + + readback_config_.is_initiator = true; + logger_->info("Anchor initiator enabled"); + return true; +} + +bool RtlsDevice::disableAnchorInitiator() +{ + if (readback_config_.type != device_type::anchor) + { + logger_->error("Device is not configured as anchor!"); + return false; + } + + if (!sendRawCommand(anchor_initiator_disable_.data(), + anchor_initiator_disable_.size())) + { + logger_->error("Failed to disable anchor initiator!"); + return false; + } + + readback_config_.is_initiator = false; + logger_->info("Anchor initiator disabled"); + return true; +} + +bool RtlsDevice::enableAutoAnchorPositioning() +{ + if (readback_config_.type != device_type::anchor) + { + logger_->error("Device is not configured as anchor!"); + return false; + } + + if (!sendRawCommand(auto_anchor_positioning_enable_.data(), + auto_anchor_positioning_enable_.size())) + { + logger_->error("Failed to enable Auto Anchor Positioning!"); + return false; + } + + readback_config_.is_auto_anchor_positioning = true; + logger_->info("Auto Anchor Positioning enabled"); + return true; +} + +bool RtlsDevice::disableAutoAnchorPositioning() +{ + if (readback_config_.type != device_type::anchor) + { + logger_->error("Device is not configured as anchor!"); + return false; + } + + if (!sendRawCommand(auto_anchor_positioning_disable_.data(), + auto_anchor_positioning_disable_.size())) + { + logger_->error("Failed to disable Auto Anchor Positioning!"); + return false; + } + + readback_config_.is_auto_anchor_positioning = false; + logger_->info("Auto Anchor Positioning disabled"); + return true; +} + +bool RtlsDevice::enableLED() +{ + if (!sendRawCommand(led_enable_.data(), led_enable_.size())) + { + logger_->error("Failed to enable LED!"); + return false; + } + + readback_config_.is_led_enabled = true; + logger_->info("LED enabled"); + return true; +} + +bool RtlsDevice::disableLED() +{ + if (!sendRawCommand(led_disable_.data(), led_disable_.size())) + { + logger_->error("Failed to disable LED!"); + return false; + } + + readback_config_.is_led_enabled = false; + logger_->info("LED disabled"); + return true; +} + +bool RtlsDevice::restartDevice() +{ + if (!sendRawCommand(restart_device_.data(), restart_device_.size())) + { + logger_->error("Failed to restart device!"); + return false; + } + + logger_->info("Device restart triggered"); + return true; +} + +bool RtlsDevice::requestConfig() +{ + if (serial_port_->write(request_config_.data(), request_config_.size()) <= 0) + { + logger_->error("Timeout or error while requesting device config!"); + return false; + } + + if (!readConfig()) + { + logger_->error("Parsing of device config failed!"); + return false; + } + + logger_->info("Config received and parsed successfully"); + std::this_thread::sleep_for(std::chrono::milliseconds(command_send_interval_ms_)); + return true; +} + +bool RtlsDevice::setTrackerMessageShort() +{ + if (readback_config_.type != device_type::tracker) + { + logger_->error("Device is not configured as tracker!"); + return false; + } + + if (!sendRawCommand(tracker_msg_short_.data(), tracker_msg_short_.size())) + { + logger_->error("Failed to set tracker message type Short!"); + return false; + } + + readback_config_.tracker_message_mode = tracker_msg_mode::msg_short; + logger_->info("Tracker message type set to Short"); + return true; +} + +bool RtlsDevice::setTrackerMessageLong() +{ + if (readback_config_.type != device_type::tracker) + { + logger_->error("Device is not configured as tracker!"); + return false; + } + + if (!sendRawCommand(tracker_msg_long_.data(), tracker_msg_long_.size())) + { + logger_->error("Failed to set tracker message type Long!"); + return false; + } + + readback_config_.tracker_message_mode = tracker_msg_mode::msg_long; + logger_->info("Tracker message type set to Long"); + return true; +} + +bool RtlsDevice::enableTrackerStream() +{ + serial_port_->flushInput(); + if (!sendRawCommand(tracker_stream_enable_.data(), tracker_stream_enable_.size())) + { + logger_->error("Failed to enable tracker stream!"); + return false; + } + + readback_config_.is_tracker_stream_mode = true; + logger_->info("Tracker stream enabled"); + return true; +} + +void RtlsDevice::disableTrackerStream() +{ + if (serial_port_->write(tracker_stream_disable_.data(), tracker_stream_disable_.size()) <= 0) + { + logger_->error("Failed to disable tracker stream!"); + return; + } + + readback_config_.is_tracker_stream_mode = false; + logger_->info("Request to disable tracker stream sent"); + + std::this_thread::sleep_for(std::chrono::milliseconds(command_send_interval_ms_)); + serial_port_->flushInput(); + return; +} + +bool RtlsDevice::send16bDataCommand(cmd_header header, uint16_t data) +{ + std::array write_buffer = { + static_cast(header), + static_cast(data >> 8), + static_cast(data & 0xFF), + 0x0, // CRC8 placeholder + }; + write_buffer.at(write_buffer.size() - 1) = + crc::crc8(write_buffer.data(), write_buffer.size() - 1); + + logger_->debug("Sending 0x{0:x}, 0x{1:x}, 0x{2:x}, 0x{3:x}", + static_cast(write_buffer.at(0)), + static_cast(write_buffer.at(1)), + static_cast(write_buffer.at(2)), + static_cast(write_buffer.at(3)) + ); + + return sendRawCommand(write_buffer.data(), write_buffer.size()); +} + +bool RtlsDevice::sendRawCommand(const uint8_t cmd[], size_t length) +{ + if (serial_port_->write(cmd, length) <= 0) + { + logger_->error("Timeout or error while writing to serial!"); + return false; + } + return readACK(); +} + +bool RtlsDevice::readACK() const +{ + bool success = true; + std::array ack_buffer = {0, 0, 0, 0, 0}; + size_t bytes_read = serial_port_->read(ack_buffer.data(), ack_buffer.size()); + if (bytes_read != ack_buffer.size() || + ack_buffer[0] != 0x99 || + ack_buffer[1] != 0x00 || + ack_buffer[2] != 0x5C || + ack_buffer[3] != '\r' || + ack_buffer[4] != '\n') + { + success = false; + logger_->error("ACK Error. Received: 0x{0:x}, 0x{1:x}, 0x{2:x}, 0x{3:x}, 0x{4:x}", + static_cast(ack_buffer[0]), + static_cast(ack_buffer[1]), + static_cast(ack_buffer[2]), + static_cast(ack_buffer[3]), + static_cast(ack_buffer[4]) + ); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(command_send_interval_ms_)); + return success; +} + +bool RtlsDevice::readConfig() +{ + ConfigOutputMap config_output_map = + { + { PARAM_TO_STRING.at(device_params::fw_version), "" }, + { PARAM_TO_STRING.at(device_params::serial_id), "" }, + { PARAM_TO_STRING.at(device_params::type), "" }, + { PARAM_TO_STRING.at(device_params::is_initiator), "" }, + { PARAM_TO_STRING.at(device_params::priority), "" }, + { PARAM_TO_STRING.at(device_params::label), "" }, + { PARAM_TO_STRING.at(device_params::network_id), "" }, + { PARAM_TO_STRING.at(device_params::update_time), "" }, + { PARAM_TO_STRING.at(device_params::is_auto_anchor_positioning), "" }, + { PARAM_TO_STRING.at(device_params::anchor_height), "" }, + { PARAM_TO_STRING.at(device_params::is_tracker_stream_mode), "" }, + { PARAM_TO_STRING.at(device_params::tracker_message_mode), "" }, + { PARAM_TO_STRING.at(device_params::is_led_enabled), "" }, + { PARAM_TO_STRING.at(device_params::x), "" }, + { PARAM_TO_STRING.at(device_params::y), "" }, + { PARAM_TO_STRING.at(device_params::z), "" }, + }; + + std::string key_str; + do + { + std::string line = serial_port_->readline(); + if (line.empty() || line == "CONFIG:END") + break; + + size_t pos = line.find(":"); + key_str = line.substr(0, pos); + + auto element = config_output_map.find(key_str); + if (element == config_output_map.end()) + continue; + + std::string value = line.substr(pos + 1); + logger_->debug("Value: \"{}\" found for key: \"{}\"", value, key_str); + + element->second = value; + } while (!key_str.empty()); + + bool all_config_read = std::all_of(config_output_map.begin(), config_output_map.end(), + [](const auto& conf) + { return !conf.second.empty(); } + ); + + if (!all_config_read) + { + logger_->error("Did not read all config parameters!"); + return false; + } + + updateConfigFromMap(config_output_map); + + return true; +} + +void RtlsDevice::updateConfigFromMap(const ConfigOutputMap &config_map) +{ + readback_config_.fw_version = config_map.at(PARAM_TO_STRING.at(device_params::fw_version)); + readback_config_.serial_id = config_map.at(PARAM_TO_STRING.at(device_params::serial_id)); + readback_config_.type = parseDeviceType(config_map.at(PARAM_TO_STRING.at(device_params::type))); + readback_config_.is_initiator = parseBool(config_map.at(PARAM_TO_STRING.at(device_params::is_initiator))); + readback_config_.priority = parseDecUint16(config_map.at(PARAM_TO_STRING.at(device_params::priority))); + readback_config_.label = parseHexUint16(config_map.at(PARAM_TO_STRING.at(device_params::label))); + readback_config_.network_id = parseHexUint16(config_map.at(PARAM_TO_STRING.at(device_params::network_id))); + readback_config_.update_time = parseDecUint16(config_map.at(PARAM_TO_STRING.at(device_params::update_time))); + readback_config_.is_auto_anchor_positioning = parseBool(config_map.at(PARAM_TO_STRING.at(device_params::is_auto_anchor_positioning))); + readback_config_.anchor_height = parseDecInt32(config_map.at(PARAM_TO_STRING.at(device_params::anchor_height))); + readback_config_.is_tracker_stream_mode = parseBool(config_map.at(PARAM_TO_STRING.at(device_params::is_tracker_stream_mode))); + readback_config_.tracker_message_mode = parseTrackerMsgMode(config_map.at(PARAM_TO_STRING.at(device_params::tracker_message_mode))); + readback_config_.is_led_enabled = parseBool(config_map.at(PARAM_TO_STRING.at(device_params::is_led_enabled))); + readback_config_.x = parseDecInt32(config_map.at(PARAM_TO_STRING.at(device_params::x))); + readback_config_.y = parseDecInt32(config_map.at(PARAM_TO_STRING.at(device_params::y))); + readback_config_.z = parseDecInt32(config_map.at(PARAM_TO_STRING.at(device_params::z))); +} + +bool RtlsDevice::parseBool(std::string param) +{ + return param == "Enabled" ? true : false; +} + +uint16_t RtlsDevice::parseHexUint16(std::string param) +{ + return static_cast(std::strtoul(param.data(), nullptr, 16)); +} + +int32_t RtlsDevice::parseDecInt32(std::string coordinate) +{ + return std::stoi(coordinate); +} + +uint16_t RtlsDevice::parseDecUint16(std::string update_time) +{ + return static_cast(std::stoi(update_time)); +} + +RtlsDevice::device_type RtlsDevice::parseDeviceType(std::string device_type_string) +{ + if (device_type_string == "Anchor") + { + return device_type::anchor; + } + + return device_type::tracker; +} + +RtlsDevice::tracker_msg_mode RtlsDevice::parseTrackerMsgMode(std::string tracker_msg_mode) +{ + if (tracker_msg_mode == "Long") + { + return tracker_msg_mode::msg_long; + } + + return tracker_msg_mode::msg_short; +} + +std::vector RtlsDevice::splitString(const std::string &input_line, char delimiter) +{ + std::vector fields; + std::string field; + std::istringstream line(input_line); + + while (std::getline(line, field, delimiter)) + { + fields.push_back(field); + } + return fields; +} + +bool RtlsDevice::registerOnDistanceDataCaptureCallback(OnTrackerDataCallback cb) +{ + if (tracker_data_thread_.joinable()) { + logger_->warn("Cannot change callback after device initialization"); + return false; + } + tracker_data_callback_ = cb; + return true; +} + +bool RtlsDevice::startReadingStream() +{ + if (readback_config_.type != device_type::tracker) { + logger_->error("Cannot start reading tracker data output:" + " Device not configured as tracker!"); + return false; + } + + if (tracker_data_thread_.joinable()) { + logger_->error("Cannot start streaming. Thread already running!"); + return false; + } + + stop_ = false; + tracker_data_thread_ = std::thread( + [this]() { + logger_->debug("Stream reading thread started."); + serial_port_->flushInput(); + while (!stop_) { + readTrackerStream(); + } + logger_->debug("Stream reading thread finished."); + }); + + return true; +} + +bool RtlsDevice::stopReadingStream() +{ + stop_ = true; + if (tracker_data_thread_.joinable()) { + tracker_data_thread_.join(); + } + serial_port_->flushInput(); + + return !tracker_data_thread_.joinable(); +} + +void RtlsDevice::readTrackerStream() +{ + tracker_msg_t tracker_message; + tracker_message.is_valid_position = true; + std::string tracker_line = serial_port_->readline(); + tracker_line.erase(std::find(tracker_line.begin(), tracker_line.end(), '\0'), + tracker_line.end()); + logger_->debug("Tracker output: {}", tracker_line); + std::vector fields = splitString(tracker_line, ','); + + if (fields.size() != TRACKER_STREAM_SHORT_LEN && + fields.size() != TRACKER_STREAM_LONG_1_ANCHOR_LEN && + fields.size() != TRACKER_STREAM_LONG_2_ANCHOR_LEN && + fields.size() != TRACKER_STREAM_LONG_3_ANCHOR_LEN && + fields.size() != TRACKER_STREAM_LONG_4_ANCHOR_LEN) + { + logger_->error("Invalid frame of tracker output!"); + tracker_message.is_valid_position = false; + return; + } + + size_t cursor = 0; + tracker_message.timestamp = static_cast(std::stoi(fields.at(cursor++))); + + // Parse anchors positions + for (size_t i = 0; i < TRACKER_STREAM_MAX_ANCHOR_NUMBER; i++) + { + if (fields.at(cursor).at(0) == 'D') + { + anchor_data_t anchor_data; + anchor_data.number = static_cast(fields.at(cursor).at(1) - '0'); + ++cursor; + anchor_data.id = parseHexUint16(fields.at(cursor++)); + anchor_data.pos_x = std::stoi(fields.at(cursor++)); + anchor_data.pos_y = std::stoi(fields.at(cursor++)); + anchor_data.pos_z = std::stoi(fields.at(cursor++)); + anchor_data.distance = std::stoi(fields.at(cursor++)); + tracker_message.anchors_data.push_back(anchor_data); + } + } + + // Parse tracker position + for (size_t i = 0; i < 3; i++) + { + std::string coordinate_str = fields.at(cursor + i); + if (coordinate_str == "N/A") + { + tracker_message.tracker_position_xyz.at(i) = 0; + tracker_message.is_valid_position = false; + } + else + { + tracker_message.tracker_position_xyz.at(i) = + std::stoi(coordinate_str); + } + } + if (tracker_data_callback_) + { + tracker_data_callback_(tracker_message); + } +} + +} // namespace terabee diff --git a/serial_communication/CMakeLists.txt b/serial_communication/CMakeLists.txt new file mode 100644 index 0000000..cfc392f --- /dev/null +++ b/serial_communication/CMakeLists.txt @@ -0,0 +1,38 @@ +project(serial_communication) +set(LIB_NAME serial_communication) + +add_library(${LIB_NAME} SHARED + $<$:src/serial_windows.cpp> + $<$:src/serial.cpp> +) + +target_include_directories(${LIB_NAME} PUBLIC + $ + $ +) + +set_target_properties(${LIB_NAME} PROPERTIES + VERSION ${PACKAGE_VERSION} +) + +target_link_libraries(${LIB_NAME} + logger +) + +install(DIRECTORY include/ + DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" + COMPONENT headers +) + +install(TARGETS ${LIB_NAME} + EXPORT ${LIB_NAME} + DESTINATION "${CMAKE_INSTALL_LIBDIR}" + COMPONENT libraries +) + +install(EXPORT ${LIB_NAME} + DESTINATION "${export_dest_dir}" + NAMESPACE positioning_systems_api:: + FILE ${config_targets_file_serial_communication} + COMPONENT configs +) diff --git a/serial_communication/include/serial_communication/crc.hpp b/serial_communication/include/serial_communication/crc.hpp new file mode 100644 index 0000000..81bb78d --- /dev/null +++ b/serial_communication/include/serial_communication/crc.hpp @@ -0,0 +1,47 @@ +#ifndef HELPER_LIB_H +#define HELPER_LIB_H + +#include + +namespace terabee +{ +namespace crc +{ + +static const uint8_t crc_table[] = {0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, + 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, + 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, + 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, + 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, + 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, + 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, 0x27, 0x20, + 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, + 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, + 0xad, 0xaa, 0xa3, 0xa4, 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, + 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, + 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, + 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, 0x4e, 0x49, 0x40, 0x47, + 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, + 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, + 0x84, 0x83, 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, + 0xfa, 0xfd, 0xf4, 0xf3}; + +inline uint8_t crc8(const uint8_t* data, int numBytes) +{ + uint16_t i; + uint16_t crc = 0x0; + + while (numBytes--) + { + i = (crc ^ *data++) & 0xFF; + crc = (crc_table[i] ^ (crc << 8)) & 0xFF; + } + return crc & 0xFF; +} + +} // namespace helper_lib +} // namespace terabee + +#endif // HELPER_LIB_H diff --git a/serial_communication/include/serial_communication/iserial.hpp b/serial_communication/include/serial_communication/iserial.hpp new file mode 100644 index 0000000..a314595 --- /dev/null +++ b/serial_communication/include/serial_communication/iserial.hpp @@ -0,0 +1,70 @@ +#ifndef ISERIAL_HPP +#define ISERIAL_HPP + +#include +#include +#include + +namespace terabee +{ +namespace serial_communication +{ + +class ISerial +{ +public: + enum class bytesize_t + { + fivebits = 5, + sixbits = 6, + sevenbits = 7, + eightbits = 8 + }; + + enum class parity_t + { + parity_none = 0, + parity_odd = 1, + parity_even = 2, + parity_mark = 3, + parity_space = 4 + }; + + enum class stopbits_t + { + stopbits_one = 1, + stopbits_two = 2, + stopbits_one_point_five = 3 + }; + + enum class flowcontrol_t + { + flowcontrol_none = 0, + flowcontrol_software = 1, + flowcontrol_hardware = 2 + }; + + virtual ~ISerial() = default; + virtual bool open() = 0; + virtual bool close() = 0; + virtual bool clear() = 0; + virtual bool isOpen() = 0; + virtual size_t available() = 0; + virtual bool setPortName(const std::string &portname) = 0; + virtual bool setBaudrate(uint32_t baudrate) = 0; + virtual bool setParity(parity_t iparity) = 0; + virtual bool setBytesize(bytesize_t ibytesize) = 0; + virtual bool setStopbits(stopbits_t istopbits) = 0; + virtual bool setFlowcontrol(flowcontrol_t iflowcontrol) = 0; + virtual bool setTimeout(std::chrono::milliseconds timeout) = 0; + virtual std::string readline(size_t max_buffer_size = 65536, char eol = '\n') = 0; + virtual bool flushInput() = 0; + virtual bool flushOutput() = 0; + virtual size_t write(const uint8_t *data, size_t size) = 0; + virtual size_t read(uint8_t *buffer, size_t size) = 0; +}; + +} // namespace serial_communication +} // namespace terabee + +#endif // ISERIAL_HPP diff --git a/serial_communication/include/serial_communication/serial.hpp b/serial_communication/include/serial_communication/serial.hpp new file mode 100644 index 0000000..8ebfc27 --- /dev/null +++ b/serial_communication/include/serial_communication/serial.hpp @@ -0,0 +1,69 @@ +#ifndef SERIAL_HPP +#define SERIAL_HPP + +#include +#include +#include + +#include "serial_communication/iserial.hpp" +#include "logger/logger.hpp" + +namespace terabee +{ +namespace serial_communication +{ + +class Serial: public ISerial +{ +public: + Serial() = delete; + /** + * Constructs Serial from given port, e.g. /dev/ttyUSB0 + * + * with default settings: + * - baud 9600 + * - parity none + * - bytesize 8 + * - 1 stop bit + * - no flow control + * - timeout == 0 (non-blocking) + */ + explicit Serial(const std::string &port); + Serial(const Serial &other) = delete; + Serial(Serial &&other) = delete; + ~Serial() override; + bool open() override; + bool close() override; + bool clear() override; + bool isOpen() override; + size_t available() override; + bool setPortName(const std::string &portname) override; + bool setBaudrate(uint32_t baudrate) override; + bool setParity(parity_t iparity) override; + bool setBytesize(bytesize_t ibytesize) override; + bool setStopbits(stopbits_t istopbits) override; + bool setFlowcontrol(flowcontrol_t iflowcontrol) override; + bool setTimeout(std::chrono::milliseconds timeout) override; + std::string readline(size_t max_buffer_size, char eol) override; + bool flushInput() override; + bool flushOutput() override; + size_t write(const uint8_t *data, size_t size) override; + size_t read(uint8_t *buffer, size_t size) override; + +private: + utility::logger::Logger logger_; + std::string port_; + uint32_t baud_rate_; + parity_t parity_; + bytesize_t byte_size_; + stopbits_t stop_bits_; + flowcontrol_t flow_control_; + std::chrono::milliseconds timeout_; + int serial_fd_; + bool blocking_; +}; + +} // namespace serial_communication +} // namespace terabee + +#endif // SERIAL_HPP diff --git a/serial_communication/include/serial_communication/serial_windows.hpp b/serial_communication/include/serial_communication/serial_windows.hpp new file mode 100755 index 0000000..52483f9 --- /dev/null +++ b/serial_communication/include/serial_communication/serial_windows.hpp @@ -0,0 +1,67 @@ +#ifndef SERIAL_WINDOWS_H +#define SERIAL_WINDOWS_H + +#include + +#include "serial_communication/iserial.hpp" +#include "logger/logger.hpp" + +namespace terabee +{ +namespace serial_communication +{ + +class SerialWindows : public ISerial +{ +public: + SerialWindows() = delete; + /** + * Constructs Serial from given port, e.g. /dev/ttyUSB0 + * + * with default settings: + * - baud 9600 + * - parity none + * - bytesize 8 + * - 1 stop bit + * - no flow control + * - timeout == 0 (non-blocking) + */ + SerialWindows(const std::string &port); + SerialWindows(const SerialWindows &other) = delete; + SerialWindows(SerialWindows &&other) = delete; + ~SerialWindows() override; + bool open() override; + bool close() override; + bool clear() override; + bool isOpen() override; + size_t available() override; + bool setPortName(const std::string &portname) override; + bool setBaudrate(uint32_t baudrate) override; + bool setParity(parity_t iparity) override; + bool setBytesize(bytesize_t ibytesize) override; + bool setStopbits(stopbits_t istopbits) override; + bool setFlowcontrol(flowcontrol_t iflowcontrol) override; + bool setTimeout(std::chrono::milliseconds timeout) override; + std::string readline(size_t max_buffer_size, char eol) override; + bool flushInput() override; + bool flushOutput() override; + size_t write(const uint8_t *data, size_t size) override; + size_t read(uint8_t *buffer, size_t size) override; + +private: + utility::logger::Logger logger_; + bool is_open_; + std::string portname_; + DWORD baudrate_; + BYTE parity_; + BYTE stop_bits_; + BYTE byte_size_; + std::chrono::milliseconds timeout_; + bool blocking_; + HANDLE serial_handle_; +}; + +} // namespace serial_communication +} // namespace terabee + +#endif // SERIAL_WINDOWS_H diff --git a/serial_communication/src/serial.cpp b/serial_communication/src/serial.cpp new file mode 100644 index 0000000..4bc0dd8 --- /dev/null +++ b/serial_communication/src/serial.cpp @@ -0,0 +1,356 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "serial_communication/serial.hpp" + +namespace terabee +{ +namespace serial_communication +{ + +Serial::Serial(const std::string &port): + logger_("Serial(" + port + ")"), + port_(port), + baud_rate_(9600), + parity_(parity_t::parity_none), + byte_size_(bytesize_t::eightbits), + stop_bits_(stopbits_t::stopbits_one), + flow_control_(flowcontrol_t::flowcontrol_none), + timeout_(0), + serial_fd_(0), + blocking_(false) +{ + logger_->debug("Serial::Serial(" + port_ + ")"); +} + +Serial::~Serial() +{ + logger_->debug("Serial::~Serial()"); + close(); +} + +bool Serial::open() +{ + if (serial_fd_ != 0) + { + logger_->warn("Already initialized"); + return false; + } + int fileFlags = O_RDWR | O_NOCTTY | O_NDELAY; + if (!blocking_) + { + fileFlags |= O_NONBLOCK; + } + serial_fd_ = ::open(port_.c_str(), fileFlags); + if (serial_fd_ == -1) + { + logger_->error("Failed to open serial port: ", std::strerror(errno)); + serial_fd_ = 0; + return false; + } + struct termios portSettings{}; + switch (baud_rate_) + { + case 9600: + portSettings.c_cflag = B9600; + break; + case 19200: + portSettings.c_cflag = B19200; + break; + case 38400: + portSettings.c_cflag = B38400; + break; + case 57600: + portSettings.c_cflag = B57600; + break; + case 115200: + portSettings.c_cflag = B115200; + break; + default: + logger_->error("Unsupported baudrate: {}", baud_rate_); + return false; + } + switch (byte_size_) + { + case bytesize_t::eightbits: + portSettings.c_cflag |= CS8; + break; + case bytesize_t::sevenbits: + portSettings.c_cflag |= CS7; + break; + case bytesize_t::sixbits: + portSettings.c_cflag |= CS6; + break; + case bytesize_t::fivebits: + portSettings.c_cflag |= CS5; + break; + } + switch (parity_) + { + case parity_t::parity_none: + break; + case parity_t::parity_odd: + portSettings.c_cflag |= PARENB | PARODD; + break; + case parity_t::parity_even: + portSettings.c_cflag |= PARENB; + break; + case parity_t::parity_mark: + portSettings.c_cflag |= PARENB | PARODD | CMSPAR; + break; + case parity_t::parity_space: + portSettings.c_cflag |= PARENB | CMSPAR; + break; + } + // TODO: take a look if that 1.5 stopbits can be supported + if (stop_bits_ == stopbits_t::stopbits_two) + { + portSettings.c_cflag |= CSTOPB; + } + if (flow_control_ == flowcontrol_t::flowcontrol_software) + { + portSettings.c_iflag |= IXON | IXOFF; + } + else if (flow_control_ == flowcontrol_t::flowcontrol_hardware) + { + portSettings.c_cflag |= CRTSCTS; + } + portSettings.c_cflag |= CREAD | CLOCAL; + portSettings.c_iflag |= IGNPAR; + portSettings.c_oflag = 0; + portSettings.c_lflag = 0; + portSettings.c_cc[VMIN] = blocking_ ? 1 : 0; + portSettings.c_cc[VTIME] = 5; + tcsetattr(serial_fd_, TCSANOW, &portSettings); + tcflush(serial_fd_, TCIFLUSH); + return true; +} + +bool Serial::close() +{ + if (!isOpen()) + { + logger_->warn("Cannot close port, because is not open!"); + return false; + } + ::close(serial_fd_); + serial_fd_ = 0; + return true; +} + +bool Serial::clear() +{ + return (tcflush(serial_fd_, TCIOFLUSH) == -1) ? false : true; +} + +bool Serial::isOpen() +{ + return (serial_fd_ == 0) ? false : true; +} + +size_t Serial::available() +{ + int bytes(0); + ioctl(serial_fd_, TIOCINQ, &bytes); + return static_cast(bytes); +} + +bool Serial::setPortName(const std::string &portname) +{ + if (isOpen()) + { + logger_->warn("Cannot change settings on the open port"); + return false; + } + port_ = portname; + return true; +} + +bool Serial::setBaudrate(uint32_t baudrate) +{ + if (isOpen()) + { + logger_->warn("Cannot change settings on the open port"); + return false; + } + baud_rate_ = baudrate; + return true; +} + +bool Serial::setParity(parity_t iparity) +{ + if (isOpen()) + { + logger_->warn("Cannot change settings on the open port"); + return false; + } + parity_ = iparity; + return true; +} + +bool Serial::setBytesize(bytesize_t ibytesize) +{ + if (isOpen()) + { + logger_->warn("Cannot change settings on the open port"); + return false; + } + byte_size_ = ibytesize; + return true; +} + +bool Serial::setStopbits(stopbits_t istopbits) +{ + if (isOpen()) + { + logger_->warn("Cannot change settings on the open port"); + return false; + } + if (istopbits != stopbits_t::stopbits_one_point_five) + { + stop_bits_ = istopbits; + } + else + { + // TODO: Check if 1.5 can be suppported + logger_->warn("Stop bits 1.5 not supported. Preserved last setting!"); + } + return true; +} + +bool Serial::setFlowcontrol(flowcontrol_t iflowcontrol) +{ + if (isOpen()) + { + logger_->warn("Cannot change settings on the open port"); + return false; + } + flow_control_ = iflowcontrol; + return true; +} + +bool Serial::setTimeout(std::chrono::milliseconds timeout) +{ + if (isOpen()) + { + logger_->warn("Cannot change settings on the open port"); + return false; + } + timeout_ = timeout; + if (timeout_.count() > 0) + { + blocking_ = true; + } + return true; +} + +std::string Serial::readline(size_t max_buffer_size, char eol = '\n') +{ + if (!isOpen()) + { + logger_->warn("Cannot read because not open"); + return ""; + } + std::string result; + uint8_t character = 0; + const uint8_t bytes_number = 1; + for (size_t i = 0; i < max_buffer_size; i++) + { + size_t num_read = read(&character, bytes_number); + if (num_read < bytes_number) + { + logger_->error("Failed to read: {}", std::strerror(errno)); + return ""; + } + if (character == eol) break; + result += static_cast(character); + } + return result; +} + +bool Serial::flushInput() +{ + if (!isOpen()) + { + logger_->warn("Cannot flush because not open"); + return false; + } + return (tcflush(serial_fd_, TCIFLUSH) == -1) ? false : true; +} + +bool Serial::flushOutput() +{ + if (!isOpen()) + { + logger_->warn("Cannot flush because not open"); + return false; + } + return (tcflush(serial_fd_, TCOFLUSH) == -1) ? false : true; +} + +size_t Serial::write(const uint8_t *data, size_t size) +{ + if (!isOpen()) + { + logger_->warn("Cannot write because not open"); + return 0; + } + ssize_t numBytesSent = ::write(serial_fd_, data, size); + if (numBytesSent == -1) + { + logger_->error("Failed to write: {}", std::strerror(errno)); + } + return (numBytesSent >= 0) ? static_cast(numBytesSent) : 0; +} + +size_t Serial::read(uint8_t *buffer, size_t size) +{ + if (!isOpen()) + { + logger_->warn("Cannot read because not open"); + return 0; + } + size_t numBytesRead = 0; + if (blocking_) + { + auto start = std::chrono::high_resolution_clock::now(); + do + { + ssize_t retval = ::read(serial_fd_, buffer + numBytesRead, size - numBytesRead); + if (retval > -1) { + numBytesRead += static_cast(retval); + } + if (retval <= -1 && errno != EAGAIN) + { + logger_->error("Failed to read: {}", std::strerror(errno)); + return numBytesRead; + } + int timeToWait = 1e6*(size-numBytesRead)/(baud_rate_); + std::this_thread::sleep_for(std::chrono::microseconds(timeToWait)); + if ((timeout_.count() != 0) && (std::chrono::high_resolution_clock::now() - start > timeout_)) + { + logger_->warn("Read timeout"); + return numBytesRead; + } + } while (numBytesRead < size); + } + else + { + ssize_t retval = ::read(serial_fd_, buffer, size); + numBytesRead = static_cast(retval); + } + return numBytesRead; +} + +} // namespace serial_communication +} // namespace terabee diff --git a/serial_communication/src/serial_windows.cpp b/serial_communication/src/serial_windows.cpp new file mode 100755 index 0000000..d37b772 --- /dev/null +++ b/serial_communication/src/serial_windows.cpp @@ -0,0 +1,398 @@ +#include "serial_communication/serial_windows.hpp" + +#include + +#include +#include +#include +#include + +namespace terabee +{ +namespace serial_communication +{ + +SerialWindows::SerialWindows(const std::string &port): + logger_("Serial(" + port + ")"), + is_open_(false), + portname_(port), + baudrate_(CBR_9600), + parity_(NOPARITY), + stop_bits_(ONESTOPBIT), + byte_size_(DATABITS_8), + timeout_(0), + blocking_(false), + serial_handle_() +{ + setPortName(portname_); + setBaudrate(9600); + setParity(parity_t::parity_none); + setBytesize(bytesize_t::eightbits); + setStopbits(stopbits_t::stopbits_one); + setFlowcontrol(flowcontrol_t::flowcontrol_none); + logger_->debug("Serial::Serial(" + portname_ + ")"); +} + +SerialWindows::~SerialWindows() +{ + logger_->debug("SerialWindows::~SerialWindows()"); + close(); +} + +bool SerialWindows::open() +{ + if (isOpen()) + { + logger_->error("Cannot open port. Already open!"); + return false; + } + + std::string port_path = "\\\\.\\" + portname_; + logger_->info("Port Name: {}", port_path); + serial_handle_ = CreateFile(port_path.c_str(), GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0); + if (serial_handle_ == INVALID_HANDLE_VALUE) + { + logger_->error("Port cannot be opened: INVALID_HANDLE_VALUE!"); + return false; + } + + DCB serialParams = { 0 }; + serialParams.DCBlength = sizeof(serialParams); + if (GetCommState(serial_handle_, &serialParams) == FALSE) + { + logger_->error("Error in GetCommmState!"); + return false; + } + + serialParams.BaudRate = baudrate_; + serialParams.ByteSize = byte_size_; + serialParams.StopBits = stop_bits_; + serialParams.Parity = parity_; + if (SetCommState(serial_handle_, &serialParams) == FALSE) + { + logger_->error("Error in SetCommState!"); + return false; + } + + COMMTIMEOUTS timeout = { 0 }; + timeout.ReadIntervalTimeout = 50; + timeout.ReadTotalTimeoutConstant = timeout_.count(); + timeout.ReadTotalTimeoutMultiplier = 50; + timeout.WriteTotalTimeoutConstant = 50; + timeout.WriteTotalTimeoutMultiplier = 10; + if (SetCommTimeouts(serial_handle_, &timeout) == FALSE) + { + logger_->error("Error in SetCommTimeouts!"); + return false; + } + is_open_ = true; + return is_open_; +} + +bool SerialWindows::close() +{ + if (!isOpen()) + { + logger_->warn("Cannot close port, because is not open!"); + return false; + } + + if (CloseHandle(serial_handle_) == FALSE) + { + logger_->error("Error in CloseHandle!"); + return false; + } + is_open_ = false; + return true; +} + +bool SerialWindows::clear() +{ + return PurgeComm(serial_handle_, PURGE_RXABORT|PURGE_TXABORT|PURGE_RXCLEAR|PURGE_TXCLEAR); +} + +bool SerialWindows::isOpen() +{ + return is_open_; +} + +size_t SerialWindows::available() +{ + logger_->error("available() Not supported!"); + return 0; +} + +bool SerialWindows::setPortName(const std::string &portname) +{ + if (isOpen()) + { + logger_->error("Cannot change port name on the open port!"); + return false; + } + portname_ = portname; + return true; +} + +bool SerialWindows::setBaudrate(uint32_t baudrate) +{ + if(isOpen()) + { + logger_->error("Cannot change settings on the open port!"); + return false; + } + + switch (baudrate) + { + case 1200: + baudrate_ = CBR_1200; + break; + case 2400: + baudrate_ = CBR_2400; + break; + case 4800: + baudrate_ = CBR_4800; + break; + case 9600: + baudrate_ = CBR_9600; + break; + case 19200: + baudrate_ = CBR_19200; + break; + case 38400: + baudrate_ = CBR_38400; + break; + case 57600: + baudrate_ = CBR_57600; + break; + case 115200: + baudrate_ = CBR_115200; + break; + default: + logger_->error("Unsupported baudrate: {}!", baudrate); + return false; + } + + return true; +} + +bool SerialWindows::setParity(parity_t iparity) +{ + if(isOpen()) + { + logger_->error("Cannot change settings on the open port!"); + return false; + } + + switch (iparity) + { + case parity_t::parity_none: + parity_ = NOPARITY; + break; + case parity_t::parity_odd: + parity_ = ODDPARITY; + break; + case parity_t::parity_even: + parity_ = EVENPARITY; + break; + case parity_t::parity_mark: + parity_ = MARKPARITY; + break; + case parity_t::parity_space: + parity_ = SPACEPARITY; + break; + default: + logger_->error("Unsupported parity: {}!", static_cast(iparity)); + return false; + } + + return true; +} + +bool SerialWindows::setBytesize(bytesize_t ibytesize) +{ + if(isOpen()) + { + logger_->error("Cannot change settings on the open port!"); + return false; + } + + switch (ibytesize) + { + case bytesize_t::fivebits: + byte_size_ = DATABITS_5; + break; + case bytesize_t::sixbits: + byte_size_ = DATABITS_6; + break; + case bytesize_t::sevenbits: + byte_size_ = DATABITS_7; + break; + case bytesize_t::eightbits: + byte_size_ = DATABITS_8; + break; + default: + logger_->error("Unsupported bytesize: {}!", static_cast(ibytesize)); + return false; + } + + return true; +} + +bool SerialWindows::setStopbits(stopbits_t istopbits) +{ + if(isOpen()) + { + logger_->error("Cannot change settings on the open port!"); + return false; + } + + switch (istopbits) + { + case stopbits_t::stopbits_one: + stop_bits_ = ONESTOPBIT; + break; + case stopbits_t::stopbits_two: + stop_bits_ = TWOSTOPBITS; + break; + case stopbits_t::stopbits_one_point_five: + stop_bits_ = ONE5STOPBITS; + break; + default: + logger_->error("Unsupported stopbits: {}!", static_cast(istopbits)); + return false; + } + + return true; +} + +bool SerialWindows::setFlowcontrol(flowcontrol_t iflowcontrol) +{ + logger_->error("setFlowcontrol: Not Implemented!"); + return true; +} + +bool SerialWindows::setTimeout(std::chrono::milliseconds timeout) +{ + if(isOpen()) + { + logger_->error("Cannot change settings on the open port!"); + return false; + } + timeout_ = timeout; + if (timeout_.count() > 0) + { + blocking_ = true; + } + return true; +} + +std::string SerialWindows::readline(size_t max_buffer_size, char eol = '\n') +{ + if (!isOpen()) + { + logger_->error("Cannot read because not open!"); + return ""; + } + std::string result; + uint8_t character = 0; + const uint8_t bytes_number = 1; + + for (size_t i = 0; i < max_buffer_size; i++) + { + size_t num_read = read(&character, bytes_number); + if (num_read < bytes_number) + { + logger_->error("Failed to read: {}!", std::strerror(errno)); + return ""; + } + if (character == eol) break; + result += static_cast(character); + } + return result; +} + +bool SerialWindows::flushInput() +{ + if (!isOpen()) + { + logger_->error("Cannot flush because not open!"); + return false; + } + return (0 != PurgeComm(serial_handle_, PURGE_RXABORT|PURGE_RXCLEAR)); +} + +bool SerialWindows::flushOutput() +{ + if (!isOpen()) + { + logger_->error("Cannot flush because not open!"); + return false; + } + return (0 != PurgeComm(serial_handle_, PURGE_TXABORT|PURGE_TXCLEAR)); +} + +size_t SerialWindows::write(const uint8_t *data, size_t size) +{ + if (!isOpen()) + { + logger_->error("Cannot write because not open!"); + return 0; + } + DWORD numBytesSent; + if (WriteFile(serial_handle_, data, size, &numBytesSent, NULL) == FALSE) + { + logger_->error("Error in WriteFile!"); + return 0; + } + if (numBytesSent <= 0) + { + logger_->error("Failed to write: {}!", std::strerror(errno)); + } + return (numBytesSent >= 0) ? numBytesSent : 0; +} + +size_t SerialWindows::read(uint8_t *buffer, size_t size) +{ + if (!isOpen()) + { + logger_->error("Cannot read because not open!"); + return 0; + } + size_t numBytesRead = 0; + if (blocking_) + { + auto start = std::chrono::high_resolution_clock::now(); + do + { + DWORD nRead; + bool retval = ReadFile(serial_handle_, buffer + numBytesRead, size - numBytesRead, + &nRead, NULL); + if (retval) + { + numBytesRead += static_cast(nRead); + } + else + { + logger_->error("Failed to read. Error code: {}!", GetLastError()); + return numBytesRead; + } + int timeToWait = 1e6*(size-numBytesRead)/(baudrate_); + std::this_thread::sleep_for(std::chrono::microseconds(timeToWait)); + if ((timeout_.count() != 0) && (std::chrono::high_resolution_clock::now() - start > timeout_)) + { + logger_->error("Read timeout!"); + return numBytesRead; + } + } while (numBytesRead < size); + } + else + { + DWORD nRead; + numBytesRead = ReadFile(serial_handle_, buffer, size, + &nRead, NULL); + } + return numBytesRead; +} + +} // namespace serial_communication +} // namespace terabee diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 0000000..6fe31df --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,58 @@ +# Download and unpack googletest at configure time +configure_file(CMakeLists.txt.in ${CMAKE_BINARY_DIR}/googletest-download/CMakeLists.txt) +execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download ) +if(result) + message(FATAL_ERROR "CMake step for googletest failed: ${result}") +endif() +execute_process(COMMAND ${CMAKE_COMMAND} --build . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download ) +if(result) + message(FATAL_ERROR "Build step for googletest failed: ${result}") +endif() + +# Prevent overriding the parent project's compiler/linker +# settings on Windows - not sure if actually needed ... +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +# Add googletest directly to our build. This defines +# the gtest and gtest_main targets. + +add_subdirectory(${CMAKE_BINARY_DIR}/googletest-src + ${CMAKE_BINARY_DIR}/googletest-build + EXCLUDE_FROM_ALL +) + +# I have to add this include dir manually because of the bug in gmock build system +include_directories(SYSTEM ${CMAKE_BINARY_DIR}/googletest-src/googletest/include) + +enable_testing() +add_executable(test_serial + test_serial.cpp +) +target_link_libraries(test_serial + serial_communication + gmock_main +) +add_test(SerialTest ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/test_serial) + +add_executable(test_follow_me_driver + test_follow_me_driver.cpp +) +target_link_libraries(test_follow_me_driver + serial_communication + follow_me_driver + gmock_main +) +add_test(FollowMeDriverTest ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/test_follow_me_driver) + +add_executable(test_rtls_driver + test_rtls_driver.cpp +) +target_link_libraries(test_rtls_driver + serial_communication + rtls_driver + gmock_main +) +add_test(RTLSDriverTest ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/test_rtls_driver) diff --git a/tests/CMakeLists.txt.in b/tests/CMakeLists.txt.in new file mode 100644 index 0000000..9c4e88a --- /dev/null +++ b/tests/CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10.2) + +project(googletest-download NONE) + +include(ExternalProject) +ExternalProject_Add(googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG release-1.10.0 + SOURCE_DIR "${CMAKE_BINARY_DIR}/googletest-src" + BINARY_DIR "${CMAKE_BINARY_DIR}/googletest-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/tests/MockISerial.hpp b/tests/MockISerial.hpp new file mode 100644 index 0000000..62cbb83 --- /dev/null +++ b/tests/MockISerial.hpp @@ -0,0 +1,51 @@ +#ifndef MOCK_I_SERIAL_H +#define MOCK_I_SERIAL_H + + +#include +#include + +namespace terabee { +namespace serial_communication { + +class MockISerial : public ISerial { + public: + MOCK_METHOD0(open, + bool()); + MOCK_METHOD0(close, + bool()); + MOCK_METHOD0(clear, + bool()); + MOCK_METHOD0(isOpen, + bool()); + MOCK_METHOD0(available, + size_t()); + MOCK_METHOD1(setPortName, + bool(const std::string &portname)); + MOCK_METHOD1(setBaudrate, + bool(uint32_t baudrate)); + MOCK_METHOD1(setParity, + bool(parity_t iparity)); + MOCK_METHOD1(setBytesize, + bool(bytesize_t ibytesize)); + MOCK_METHOD1(setStopbits, + bool(stopbits_t istopbits)); + MOCK_METHOD1(setFlowcontrol, + bool(flowcontrol_t iflowcontrol)); + MOCK_METHOD1(setTimeout, + bool(std::chrono::milliseconds timeout)); + MOCK_METHOD2(readline, + std::string(size_t, char eol)); + MOCK_METHOD0(flushInput, + bool()); + MOCK_METHOD0(flushOutput, + bool()); + MOCK_METHOD2(write, + size_t(const uint8_t *data, size_t size)); + MOCK_METHOD2(read, + size_t(uint8_t *buffer, size_t size)); +}; + +} // namespace serial_communication +} // namespace terabee +#endif // MOCK_I_SERIAL_H diff --git a/tests/test_follow_me_driver.cpp b/tests/test_follow_me_driver.cpp new file mode 100644 index 0000000..6896db7 --- /dev/null +++ b/tests/test_follow_me_driver.cpp @@ -0,0 +1,202 @@ +#include +#include +#include +#include +#include "follow_me_driver/follow_me_driver.hpp" + +#include "MockISerial.hpp" + +using testing::_; +using testing::Return; +using testing::Invoke; + +namespace terabee { + +class FollowMeDriverFixture : public testing::Test +{ +protected: + FollowMeDriverFixture(): + serialInterface_(std::make_shared()) + { + ON_CALL(*serialInterface_, open()).WillByDefault(Return(true)); + ON_CALL(*serialInterface_, isOpen()).WillByDefault(Return(true)); + ON_CALL(*serialInterface_, available()).WillByDefault(Return(69)); + ON_CALL(*serialInterface_, readline(_, _)).WillByDefault(Return(fakeDataText)); + ON_CALL(*serialInterface_, write(_, _)) + .WillByDefault(Invoke( + [this](const uint8_t* data, size_t size) { + data_sent_to_write.assign(data, data + size); + return size; + })); + ON_CALL(*serialInterface_, read(_, _)) + .WillByDefault(Invoke( + [this](uint8_t* data, size_t size) { + std::memcpy(data, dummyAck, 4); + return size; + })); + + followMe_.reset(new FollowMeMasterBeacon(serialInterface_)); + remoteControl_.reset(new FollowMeRemoteControl(serialInterface_)); + } + virtual ~FollowMeDriverFixture(); + + std::unique_ptr followMe_; + std::unique_ptr remoteControl_; + + std::shared_ptr serialInterface_; + std::vector data_sent_to_write; + uint8_t fakeDataBin_[6] = {'F', 'M', 0x02, 0x04, 0xF1, 0xBD}; + uint8_t dummyAck[4] = { 0x00, 0x03, 0x01, 0x38 }; + std::string fakeDataText = "FM\t2503\t79\n"; + + uint8_t ack_printout_text[4] = { 0x00, 0x01, 0x00, 0x15 }; + + // TEST Frames + const uint8_t CMD_PRINTOUT_TEXT[4] = { 0x00, 0x11, 0x01, 0x45 }; + const uint8_t CMD_PRINTOUT_BINARY[4] = { 0x00, 0x11, 0x02, 0x4C }; + const uint8_t CMD_DEACTIVATE_DEBUG[7] = { 0x00, 0x54, 0x0D, 0x00, 0x00, 0x00, 0x8B }; + const uint8_t CMD_ACTIVATE_DEBUG[7] = { 0x00, 0x54, 0x0D, 0x01, 0x00, 0x00, 0xE0 }; + const uint8_t CMD_AUTOCALIBRATE[5] = { 0x00, 0x22, 0x00, 0x00, 0x95 }; + const uint8_t CMD_EMA_WINDOW[5] = { 0x00, 0x52, 0x01, 0x0A, 0xD1 }; + +public: + +}; + +FollowMeDriverFixture::~FollowMeDriverFixture() { + followMe_.reset(); +} + +TEST_F(FollowMeDriverFixture, CalcCmdLen) +{ + EXPECT_EQ(4, followMe_->calculateCommandLength(CMD_PRINTOUT_TEXT)); + EXPECT_EQ(4, followMe_->calculateCommandLength(CMD_PRINTOUT_BINARY)); + EXPECT_EQ(7, followMe_->calculateCommandLength(CMD_DEACTIVATE_DEBUG)); + EXPECT_EQ(7, followMe_->calculateCommandLength(CMD_ACTIVATE_DEBUG)); + EXPECT_EQ(5, followMe_->calculateCommandLength(CMD_AUTOCALIBRATE)); + EXPECT_EQ(5, followMe_->calculateCommandLength(CMD_EMA_WINDOW)); +} + +TEST_F(FollowMeDriverFixture, ProcessFrameText) +{ + EXPECT_CALL(*serialInterface_, read(_,_)) + .WillRepeatedly(Invoke( + [this](uint8_t* data, size_t size) { + std::memcpy(data, ack_printout_text, 4); + return size; + })); + followMe_->printoutModeText(); + + PolarPoint2D ref_point = { 2.503f, 1.3788f }; + PolarPoint2D measured_point; + + followMe_->process(measured_point); + + EXPECT_NEAR(ref_point.distance, measured_point.distance, 1e-4); + EXPECT_NEAR(ref_point.heading, measured_point.heading, 1e-4); +} + +TEST_F(FollowMeDriverFixture, ProcessFrameBin) +{ + EXPECT_CALL(*serialInterface_, read(_,_)) + .WillRepeatedly(Invoke( + [this](uint8_t* data, size_t size) { + std::memcpy(data, ack_printout_text, 4); + return size; + })); + followMe_->printoutModeBin(); + + PolarPoint2D ref_point = { 0.516f, -0.26179f }; + PolarPoint2D measured_point; + + EXPECT_CALL(*serialInterface_, available()).WillRepeatedly(Return(7)); + EXPECT_CALL(*serialInterface_, read(_, _)) + .WillRepeatedly(Invoke( + [this](uint8_t* data, size_t size) { + std::memcpy(data, fakeDataBin_, 7); + return size; + })); + followMe_->process(measured_point); + + EXPECT_NEAR(ref_point.distance, measured_point.distance, 1e-4); + EXPECT_NEAR(ref_point.heading, measured_point.heading, 1e-4); +} + +TEST_F(FollowMeDriverFixture, commandSwapBeacons) +{ + std::vector expected_swap_true = { 0x00, 0x31, 0x01, 0xEB }; + std::vector expected_swap_false = { 0x00, 0x31, 0x00, 0xEC }; + + followMe_->swapBeacons(true); + EXPECT_EQ(data_sent_to_write, expected_swap_true); + followMe_->swapBeacons(false); + EXPECT_EQ(data_sent_to_write, expected_swap_false); +} + +TEST_F(FollowMeDriverFixture, commandBeaconsSpan) +{ + std::vector expected_frame = { 0x00, 0x22, 0x0B, 0xAB, 0x5A }; + + followMe_->setBeaconsSpan(2987); + EXPECT_EQ(data_sent_to_write, expected_frame); +} + +TEST_F(FollowMeDriverFixture, commandEMAWindow) +{ + std::vector expected_frame = { 0x00, 0x52, 0x01, 0x17, 0x82 }; + + followMe_->setEMAWindow(23); + EXPECT_EQ(data_sent_to_write, expected_frame); +} + +TEST_F(FollowMeDriverFixture, commandRS485_Parameters) +{ + std::vector expected_frame_baud_parity = + { 0x00, 0x55, 0x02, 0x01, 0xC2, 0x00, 0x01, 0xEB }; + + followMe_->setRS485_Parameters(115200, FollowMeMasterBeacon::rs485_parity::rs485_parity_odd); + EXPECT_EQ(data_sent_to_write, expected_frame_baud_parity); +} + +TEST_F(FollowMeDriverFixture, commandRS485_SlaveId) +{ + std::vector expected_frame_slave_id = + { 0x00, 0x52, 0x03, 0x17, 0xA8 }; + + followMe_->setRS485_SlaveId(23); + EXPECT_EQ(data_sent_to_write, expected_frame_slave_id); +} + +TEST_F(FollowMeDriverFixture, remoteButtonMode) +{ + std::vector expected_toggle = { 0x08, 0x01, 0xAF }; + std::vector expected_hold = { 0x08, 0x00, 0xA8 }; + + remoteControl_->setButtonMode(FollowMeRemoteControl::button_mode::toggle); + EXPECT_EQ(data_sent_to_write, expected_toggle); + + remoteControl_->setButtonMode(FollowMeRemoteControl::button_mode::hold); + EXPECT_EQ(data_sent_to_write, expected_hold); +} + +TEST_F(FollowMeDriverFixture, remoteBuzzer) +{ + std::vector expected_enabled = { 0x09, 0x01, 0xBA }; + std::vector expected_disabled = { 0x09, 0x00, 0xBD }; + + remoteControl_->setBuzzer(true); + EXPECT_EQ(data_sent_to_write, expected_enabled); + + remoteControl_->setBuzzer(false); + EXPECT_EQ(data_sent_to_write, expected_disabled); +} + +} // namespace terabee + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + + return ret; +} diff --git a/tests/test_rtls_driver.cpp b/tests/test_rtls_driver.cpp new file mode 100644 index 0000000..5f018c3 --- /dev/null +++ b/tests/test_rtls_driver.cpp @@ -0,0 +1,764 @@ +#include +#include +#include +#include +#include "rtls_driver/rtls_driver.hpp" +#include "serial_communication/crc.hpp" + +#include "MockISerial.hpp" + +using testing::_; +using testing::Return; +using testing::Invoke; + +namespace terabee { + +class RtlsDriverFixture : public testing::Test +{ +protected: + RtlsDriverFixture(): + serialInterface_(std::make_shared()), + read_tracker_stream_(), + m_(), + read_times_(0) + { + ON_CALL(*serialInterface_, open()).WillByDefault(Return(true)); + ON_CALL(*serialInterface_, isOpen()).WillByDefault(Return(true)); + ON_CALL(*serialInterface_, available()).WillByDefault(Return(69)); + ON_CALL(*serialInterface_, write(_, _)) + .WillByDefault(Invoke( + [this](const uint8_t* data, size_t size) { + data_sent_to_write.assign(data, data + size); + return size; + })); + ON_CALL(*serialInterface_, read(_, _)) + .WillByDefault(Invoke( + [this](uint8_t* data, size_t size) { + std::memcpy(data, ackDataBin_, 5); + return size; + })); + ON_CALL(*serialInterface_, readline(_, _)) + .WillByDefault(Invoke( + [this](size_t, char) { + read_times_++; + read_tracker_stream_.notify_all(); + return tracker_output_data_; + })); + + rtls_.reset(new RtlsDevice(serialInterface_, 0)); + + rtls_->registerOnDistanceDataCaptureCallback( + [this](const terabee::RtlsDevice::tracker_msg_t& tracker_output) + { + std::cout << "T:" << tracker_output.timestamp << "\t"; + std::cout << "Tracker position: (" << + tracker_output.tracker_position_xyz.at(0) << ", " << + tracker_output.tracker_position_xyz.at(1) << ", " << + tracker_output.tracker_position_xyz.at(2) << + ")" << std::endl; + tracker_msg_ = tracker_output; + }); + } + + bool waitForTrackerData() + { + std::unique_lock l(m_); + return read_tracker_stream_.wait_for(l, std::chrono::milliseconds(500), + [this]() + { + return read_times_.load() > 2; + }); + } + + virtual ~RtlsDriverFixture(); + + std::unique_ptr rtls_; + std::shared_ptr serialInterface_; + + std::vector data_sent_to_write; + uint8_t ackDataBin_[5] = { 0x99, 0x00, 0x5C, '\r', '\n' }; + uint8_t nackDataBin_[5] = { 0x99, 0xFF, 0xAF, '\r', '\n' }; + + std::condition_variable read_tracker_stream_; + std::string tracker_output_data_; + terabee::RtlsDevice::tracker_msg_t tracker_msg_; + std::mutex m_; + std::atomic_int read_times_; +public: + +}; + +RtlsDriverFixture::~RtlsDriverFixture() { + rtls_.reset(); +} + +TEST_F(RtlsDriverFixture, validParameters) +{ + EXPECT_TRUE(rtls_->setLabel(0xFEED)); + EXPECT_EQ(rtls_->getConfig().label, 0xFEED); + EXPECT_TRUE(rtls_->setNetworkId(0xFACE)); + EXPECT_EQ(rtls_->getConfig().network_id, 0xFACE); + EXPECT_TRUE(rtls_->setUpdateTime(1)); + EXPECT_EQ(rtls_->getConfig().update_time, 1); + EXPECT_TRUE(rtls_->setUpdateTime(600)); + EXPECT_EQ(rtls_->getConfig().update_time, 600); + EXPECT_TRUE(rtls_->enableLED()); + EXPECT_EQ(rtls_->getConfig().is_led_enabled, true); + EXPECT_TRUE(rtls_->disableLED()); + EXPECT_EQ(rtls_->getConfig().is_led_enabled, false); + + EXPECT_TRUE(rtls_->setDevice(RtlsDevice::device_type::anchor, 4)); + EXPECT_EQ(rtls_->getConfig().type, RtlsDevice::device_type::anchor); + EXPECT_EQ(rtls_->getConfig().priority, 4); + EXPECT_TRUE(rtls_->setAnchorPosition(1000, 200, 2000)); + EXPECT_EQ(rtls_->getConfig().x, 1000); + EXPECT_EQ(rtls_->getConfig().y, 200); + EXPECT_EQ(rtls_->getConfig().z, 2000); + EXPECT_TRUE(rtls_->setAnchorHeightForAutoPositioning(1000)); + EXPECT_EQ(rtls_->getConfig().anchor_height, 1000); + EXPECT_TRUE(rtls_->enableAnchorInitiator()); + EXPECT_EQ(rtls_->getConfig().is_initiator, true); + EXPECT_TRUE(rtls_->disableAnchorInitiator()); + EXPECT_EQ(rtls_->getConfig().is_initiator, false); + EXPECT_TRUE(rtls_->enableAutoAnchorPositioning()); + EXPECT_EQ(rtls_->getConfig().is_auto_anchor_positioning, true); + EXPECT_TRUE(rtls_->disableAutoAnchorPositioning()); + EXPECT_EQ(rtls_->getConfig().is_auto_anchor_positioning, false); + + EXPECT_TRUE(rtls_->setDevice(RtlsDevice::device_type::tracker, 7)); + EXPECT_EQ(rtls_->getConfig().priority, 7); + EXPECT_EQ(rtls_->getConfig().type, RtlsDevice::device_type::tracker); + EXPECT_TRUE(rtls_->setTrackerMessageShort()); + EXPECT_EQ(rtls_->getConfig().tracker_message_mode, RtlsDevice::tracker_msg_mode::msg_short); + EXPECT_TRUE(rtls_->setTrackerMessageLong()); + EXPECT_EQ(rtls_->getConfig().tracker_message_mode, RtlsDevice::tracker_msg_mode::msg_long); + EXPECT_TRUE(rtls_->enableTrackerStream()); + EXPECT_EQ(rtls_->getConfig().is_tracker_stream_mode, true); + rtls_->disableTrackerStream(); + EXPECT_EQ(rtls_->getConfig().is_tracker_stream_mode, false); + + EXPECT_TRUE(rtls_->restartDevice()); + + std::vector expected_data_sent = {0x91, 0xDF, 0xE7}; + rtls_->requestConfig(); + EXPECT_EQ(data_sent_to_write, expected_data_sent); +} + +TEST_F(RtlsDriverFixture, writtenZeroBytes) +{ + EXPECT_CALL(*serialInterface_, write(_, _)) + .WillRepeatedly(Return(0)); + + EXPECT_FALSE(rtls_->setLabel(0xFEED)); + EXPECT_FALSE(rtls_->setNetworkId(0xFACE)); + EXPECT_FALSE(rtls_->setUpdateTime(2)); + EXPECT_FALSE(rtls_->enableLED()); + EXPECT_FALSE(rtls_->disableLED()); + + EXPECT_FALSE(rtls_->setDevice(RtlsDevice::device_type::anchor, 1)); + EXPECT_FALSE(rtls_->setAnchorPosition(1000, 200, 2000)); + EXPECT_FALSE(rtls_->setAnchorHeightForAutoPositioning(1000)); + EXPECT_FALSE(rtls_->enableAnchorInitiator()); + EXPECT_FALSE(rtls_->disableAnchorInitiator()); + EXPECT_FALSE(rtls_->enableAutoAnchorPositioning()); + EXPECT_FALSE(rtls_->disableAutoAnchorPositioning()); + + EXPECT_FALSE(rtls_->setDevice(RtlsDevice::device_type::tracker, 1)); + EXPECT_FALSE(rtls_->setTrackerMessageShort()); + EXPECT_FALSE(rtls_->setTrackerMessageLong()); + EXPECT_FALSE(rtls_->enableTrackerStream()); + + EXPECT_FALSE(rtls_->restartDevice()); +} + +TEST_F(RtlsDriverFixture, receivedNack) +{ + EXPECT_CALL(*serialInterface_, read(_, _)) + .WillRepeatedly(Invoke( + [this](uint8_t* data, size_t size) { + std::memcpy(data, nackDataBin_, 5); + return size; + })); + + EXPECT_FALSE(rtls_->setLabel(0xFEED)); + EXPECT_FALSE(rtls_->setNetworkId(0xFACE)); + EXPECT_FALSE(rtls_->setUpdateTime(4)); + EXPECT_FALSE(rtls_->enableLED()); + EXPECT_FALSE(rtls_->disableLED()); + + EXPECT_FALSE(rtls_->setDevice(RtlsDevice::device_type::anchor, 1)); + EXPECT_FALSE(rtls_->setAnchorPosition(1000, 200, 2000)); + EXPECT_FALSE(rtls_->setAnchorHeightForAutoPositioning(1000)); + EXPECT_FALSE(rtls_->enableAnchorInitiator()); + EXPECT_FALSE(rtls_->disableAnchorInitiator()); + EXPECT_FALSE(rtls_->enableAutoAnchorPositioning()); + EXPECT_FALSE(rtls_->disableAutoAnchorPositioning()); + + EXPECT_FALSE(rtls_->setDevice(RtlsDevice::device_type::tracker, 1)); + EXPECT_FALSE(rtls_->setTrackerMessageShort()); + EXPECT_FALSE(rtls_->setTrackerMessageLong()); + EXPECT_FALSE(rtls_->enableTrackerStream()); + + EXPECT_FALSE(rtls_->restartDevice()); +} + +TEST_F(RtlsDriverFixture, validUpdateTime) +{ + EXPECT_TRUE(rtls_->setUpdateTime(1)); + EXPECT_EQ(rtls_->getConfig().update_time, 1); + EXPECT_TRUE(rtls_->setUpdateTime(600)); + EXPECT_EQ(rtls_->getConfig().update_time, 600); +} + +TEST_F(RtlsDriverFixture, invalidUpdateTime) +{ + EXPECT_FALSE(rtls_->setUpdateTime(0)); + EXPECT_NE(rtls_->getConfig().update_time, 0); + EXPECT_FALSE(rtls_->setUpdateTime(601)); + EXPECT_NE(rtls_->getConfig().update_time, 601); +} + +TEST_F(RtlsDriverFixture, trackerFailSettingsForAnchor) +{ + EXPECT_TRUE(rtls_->setDevice(RtlsDevice::device_type::tracker, 1)); + EXPECT_FALSE(rtls_->setAnchorPosition(1000, 200, 2000)); + EXPECT_FALSE(rtls_->setAnchorHeightForAutoPositioning(1000)); + EXPECT_FALSE(rtls_->enableAnchorInitiator()); + EXPECT_FALSE(rtls_->disableAnchorInitiator()); + EXPECT_FALSE(rtls_->enableAutoAnchorPositioning()); + EXPECT_FALSE(rtls_->disableAutoAnchorPositioning()); +} + +TEST_F(RtlsDriverFixture, anchorFailSettingsForTracker) +{ + EXPECT_TRUE(rtls_->setDevice(RtlsDevice::device_type::anchor, 1)); + EXPECT_FALSE(rtls_->setTrackerMessageShort()); + EXPECT_FALSE(rtls_->setTrackerMessageLong()); + EXPECT_TRUE(rtls_->enableTrackerStream()); // possible both in anchor and tracker + + std::vector expected_data_sent = {0x11, 0x00, 0x42}; + rtls_->disableTrackerStream(); + EXPECT_EQ(data_sent_to_write, expected_data_sent); +} + +TEST_F(RtlsDriverFixture, validateMsgBuilding) +{ + std::vector set_device_sample = { + static_cast(RtlsDevice::device_type::tracker), 0x12, 0x34, 0x9A + }; + + std::vector set_label_sample = { + static_cast(RtlsDevice::cmd_header::device_label), 0xC0, 0xFE, 0xCF + }; + + std::vector set_network_id_sample = { + static_cast(RtlsDevice::cmd_header::network_id), 0xDE, 0xAD, 0x9B + }; + + std::vector set_update_time_sample = { + static_cast(RtlsDevice::cmd_header::update_time), 0x00, 0x03, 0xA2 + }; + + rtls_->setDevice(RtlsDevice::device_type::tracker, 0x1234); + EXPECT_EQ(data_sent_to_write, set_device_sample); + rtls_->setLabel(0xC0FE); + EXPECT_EQ(data_sent_to_write, set_label_sample); + rtls_->setNetworkId(0xDEAD); + EXPECT_EQ(data_sent_to_write, set_network_id_sample); + rtls_->setUpdateTime(3); + EXPECT_EQ(data_sent_to_write, set_update_time_sample); +} + +TEST_F(RtlsDriverFixture, setAnchorHeight) +{ + std::vector expected_frame = { + 0x08, + 0x10, 0xF2, 0x77, 0xD5, // z value + 0x28 + }; + + int32_t z = 284325845; + + rtls_->setDevice(RtlsDevice::device_type::anchor, 1); + rtls_->setAnchorHeightForAutoPositioning(z); + EXPECT_EQ(data_sent_to_write, expected_frame); + EXPECT_EQ(rtls_->getConfig().anchor_height, z); +} + +TEST_F(RtlsDriverFixture, setAnchorPosition) +{ + std::vector expected_frame = { + 0x06, + 0xD0, 0xF1, 0xDB, 0x05, // x value + 0x32, 0xD3, 0xCF, 0x4B, // y value + 0x00, 0x06, 0xF8, 0x55, // z value + 0x8A + }; + + int32_t x = -789456123; + int32_t y = 852741963; + int32_t z = 456789; + + rtls_->setDevice(RtlsDevice::device_type::anchor, 1); + rtls_->setAnchorPosition(x, y, z); + EXPECT_EQ(data_sent_to_write, expected_frame); + EXPECT_EQ(rtls_->getConfig().x, x); + EXPECT_EQ(rtls_->getConfig().y, y); + EXPECT_EQ(rtls_->getConfig().z, z); +} + +TEST_F(RtlsDriverFixture, readConfigOkSet1) +{ + std::vector input = { + "TERABEE UWB 3D RTLS Firmware Version:1.0", + "Serial ID:12345678", + "Function Passed.", + "Device Type:Anchor", + "Initiator Mode:Disabled", + "Device Priority:15987", + "Device Label:0x1ABE", + "Network ID:0xC0FE", + "Network Update Time[ms]:100", + "Auto Anchor Positioning (AAP):Disabled", + "Anchor Height for AAP [mm]:1234", + "Tracker Stream Mode:Disabled", + "Tracker Message Mode:Long", + "LED Mode:Disabled", + "Device Position X [mm]:5584200", + "Device Position Y [mm]:1400230", + "Device Position Z [mm]:204501296", + "CONFIG:END" + }; + + EXPECT_CALL(*serialInterface_, readline(_, _)) + .WillRepeatedly(Invoke( + [&input](size_t, char) { + std::string line = input.front(); + input.erase(input.begin()); + return line; + })); + + EXPECT_TRUE(rtls_->requestConfig()); + EXPECT_EQ(rtls_->getConfig().fw_version, "1.0"); + EXPECT_EQ(rtls_->getConfig().serial_id, "12345678"); + EXPECT_EQ(rtls_->getConfig().type, RtlsDevice::device_type::anchor); + EXPECT_EQ(rtls_->getConfig().is_initiator, false); + EXPECT_EQ(rtls_->getConfig().priority, 15987); + EXPECT_EQ(rtls_->getConfig().label, 0x1ABE); + EXPECT_EQ(rtls_->getConfig().network_id, 0xC0FE); + EXPECT_EQ(rtls_->getConfig().update_time, 100); + EXPECT_EQ(rtls_->getConfig().is_auto_anchor_positioning, false); + EXPECT_EQ(rtls_->getConfig().anchor_height, 1234); + EXPECT_EQ(rtls_->getConfig().is_tracker_stream_mode, false); + EXPECT_EQ(rtls_->getConfig().tracker_message_mode, RtlsDevice::tracker_msg_mode::msg_long); + EXPECT_EQ(rtls_->getConfig().is_led_enabled, false); + EXPECT_EQ(rtls_->getConfig().x, 5584200); + EXPECT_EQ(rtls_->getConfig().y, 1400230); + EXPECT_EQ(rtls_->getConfig().z, 204501296); +} + +TEST_F(RtlsDriverFixture, readConfigOkSet2) +{ + std::vector input = { + "TERABEE UWB 3D RTLS Firmware Version:1.4.9", + "Serial ID:12345678", + "Function Passed.", + "Device Type:Tracker", + "Initiator Mode:Enabled", + "Device Priority:65535", + "Device Label:0x1ABE", + "Network ID:0xC0FE", + "Network Update Time[ms]:900", + "Auto Anchor Positioning (AAP):Enabled", + "Anchor Height for AAP [mm]:1234", + "Tracker Stream Mode:Enabled", + "Tracker Message Mode:Short", + "LED Mode:Enabled", + "Device Position X [mm]:5584200", + "Device Position Y [mm]:1400230", + "Device Position Z [mm]:-204501296", + "CONFIG:END" + }; + + EXPECT_CALL(*serialInterface_, readline(_, _)) + .WillRepeatedly(Invoke( + [&input](size_t, char) { + std::string line = input.front(); + input.erase(input.begin()); + return line; + })); + + EXPECT_TRUE(rtls_->requestConfig()); + EXPECT_EQ(rtls_->getConfig().fw_version, "1.4.9"); + EXPECT_EQ(rtls_->getConfig().serial_id, "12345678"); + EXPECT_EQ(rtls_->getConfig().type, RtlsDevice::device_type::tracker); + EXPECT_EQ(rtls_->getConfig().is_initiator, true); + EXPECT_EQ(rtls_->getConfig().priority, 65535); + EXPECT_EQ(rtls_->getConfig().label, 0x1ABE); + EXPECT_EQ(rtls_->getConfig().network_id, 0xC0FE); + EXPECT_EQ(rtls_->getConfig().update_time, 900); + EXPECT_EQ(rtls_->getConfig().is_auto_anchor_positioning, true); + EXPECT_EQ(rtls_->getConfig().anchor_height, 1234); + EXPECT_EQ(rtls_->getConfig().is_tracker_stream_mode, true); + EXPECT_EQ(rtls_->getConfig().tracker_message_mode, RtlsDevice::tracker_msg_mode::msg_short); + EXPECT_EQ(rtls_->getConfig().is_led_enabled, true); + EXPECT_EQ(rtls_->getConfig().x, 5584200); + EXPECT_EQ(rtls_->getConfig().y, 1400230); + EXPECT_EQ(rtls_->getConfig().z, -204501296); +} + +TEST_F(RtlsDriverFixture, readConfigMissingLine) +{ + std::vector input = { + "TERABEE UWB 3D RTLS Firmware Version:1.7.9", + "Serial ID:12345678", + "Function Passed.", + "Device Type:Tracker", + "Initiator Mode:Enabled", + "Device Priority:0xFACE", + "Device Label:0x1ABE", + "Network Update Time[ms]:900", + "Auto Anchor Positioning (AAP):Enabled", + "Tracker Stream Mode:Enabled", + "Tracker Message Mode:Short", + "LED Mode:Enabled", + "Device Position X [mm]:5584200", + "Device Position Y [mm]:1400230", + "Device Position Z [mm]:-204501296", + "CONFIG:END" + }; + + EXPECT_CALL(*serialInterface_, readline(_, _)) + .WillRepeatedly(Invoke( + [&input](size_t, char) { + std::string line = input.front(); + input.erase(input.begin()); + return line; + })); + + EXPECT_FALSE(rtls_->requestConfig()); +} + +TEST_F(RtlsDriverFixture, readConfigMissingLastLine) +{ + std::vector input = { + "TERABEE UWB 3D RTLS Firmware Version:1.9.9", + "Serial ID:12345678", + "Function Passed.", + "Device Type:Tracker", + "Initiator Mode:Enabled", + "Device Priority:0xFACE", + "Device Position X [mm]:5584200", + "Device Position Y [mm]:1400230", + "Device Position Z [mm]:-204501296", + "" + // The implementation of `ISerial::readline` returns an empty string + // if it didn't read any data - e.g. we are expecting the readConfig to finish + // either on line containing UART or when there is no mora data in the buffer + }; + + EXPECT_CALL(*serialInterface_, readline(_, _)) + .WillRepeatedly(Invoke( + [&input](size_t, char) { + std::string line = input.front(); + input.erase(input.begin()); + return line; + })); + + EXPECT_FALSE(rtls_->requestConfig()); +} + +TEST_F(RtlsDriverFixture, readConfigIncorrectKeys) +{ + std::vector input = { + "TERABEE UWB 3D RTLS Firmware Version:2.0.0", + "Serial ID:12345678", + "Function Passed.", + "Device Type:Tracker", + "Initiator Mode:Enabled", + "Device Priority:0xFACE", + "Device Label:0x1ABE", + "Network ID:0xC0FE", + "Network Update Time[ms]:900", + "Auto Anchor Positioning (AAP):Enabled", + "Anchor Height for AAP [mm]:1234", + "INCORRECT Stream Mode:Enabled", + "Tracker Message Mode:Short", + "LED Mode:Enabled", + "Device Position X [mm]:5584200", + "Device Position Y [mm]:1400230", + "Device Position Z [mm]:-204501296", + "CONFIG:END" + }; + + EXPECT_CALL(*serialInterface_, readline(_, _)) + .WillRepeatedly(Invoke( + [&input](size_t, char) { + std::string line = input.front(); + input.erase(input.begin()); + return line; + })); + + EXPECT_FALSE(rtls_->requestConfig()); +} + +TEST_F(RtlsDriverFixture, DISABLED_readConfigIncorrectValues) +{ + // TODO: Define input with non-expected value for each parameter + std::vector input = { + "TERABEE UWB 3D RTLS Firmware Version:2.0.0", + "Serial ID:12345678", + "Function Passed.", + "Device Type:Tracker", + "Initiator Mode:Enabled", + "Device Priority:0xFACE", + "Device Label:0x1ABE", + "Network ID:0xC0FE", + "Network Update Time[ms]:900", + "Auto Anchor Positioning (AAP):Enabled", + "Anchor Height for AAP [mm]:1234", + "Tracker Stream Mode:Enabled", + "Tracker Message Mode:Short", + "LED Mode:Enabled", + "Device Position X [mm]:5584200", + "Device Position Y [mm]:1400230", + "Device Position Z [mm]:-204501296", + "CONFIG:END" + }; + + EXPECT_CALL(*serialInterface_, readline(_, _)) + .WillRepeatedly(Invoke( + [&input](size_t, char) { + std::string line = input.front(); + input.erase(input.begin()); + return line; + })); + + EXPECT_FALSE(rtls_->requestConfig()); + EXPECT_EQ(rtls_->getConfig().fw_version, "1.0"); + EXPECT_EQ(rtls_->getConfig().type, RtlsDevice::device_type::anchor); + EXPECT_EQ(rtls_->getConfig().is_initiator, false); + EXPECT_EQ(rtls_->getConfig().priority, 1); + EXPECT_EQ(rtls_->getConfig().label, 0x1ABE); + EXPECT_EQ(rtls_->getConfig().network_id, 0xC0FE); + EXPECT_EQ(rtls_->getConfig().update_time, 100); + EXPECT_EQ(rtls_->getConfig().is_auto_anchor_positioning, false); + EXPECT_EQ(rtls_->getConfig().anchor_height, 1234); + EXPECT_EQ(rtls_->getConfig().tracker_message_mode, RtlsDevice::tracker_msg_mode::msg_long); + EXPECT_EQ(rtls_->getConfig().is_led_enabled, false); + EXPECT_EQ(rtls_->getConfig().x, 5584200); + EXPECT_EQ(rtls_->getConfig().y, 1400230); + EXPECT_EQ(rtls_->getConfig().z, 204501296); +} + +TEST_F(RtlsDriverFixture, splitStringValid) +{ + std::string test_string_long_1 = "118334716,D0,0x14B3,0,1800,1300,1433,D1,0xC326,0,0,1600,1897," + "D2,0xCD0E,1300,0,1600,2660,-855,1149,341"; + std::string test_string_long_2 = "118836472,D0,0xC326,0,0,1600,1911,D1,0xCD0E,1300,0,1600,3063," + "D2,0x14B3,0,1800,1300,1428,N/A,N/A,N/A"; + std::string test_string_short_1 = "1529569522,-472,1149,293"; + std::string test_string_short_2 = "1529971347,N/A,N/A,N/A"; + + std::vector fields_long_1 = RtlsDevice::splitString(test_string_long_1, ','); + std::vector fields_long_2 = RtlsDevice::splitString(test_string_long_2, ','); + std::vector fields_short_1 = RtlsDevice::splitString(test_string_short_1, ','); + std::vector fields_short_2 = RtlsDevice::splitString(test_string_short_2, ','); + + EXPECT_EQ(fields_long_1.size(), 22); + EXPECT_EQ(fields_long_2.size(), 22); + EXPECT_EQ(fields_short_1.size(), 4); + EXPECT_EQ(fields_short_2.size(), 4); +} + +TEST_F(RtlsDriverFixture, trackerStreamLongLocationValid) +{ + tracker_output_data_ = "118334716,D0,0x14B3,0,1800,1300,1433,D1,0xC326,0,0,1600,1897," + "D2,0xCD0E,1300,0,1600,2660,-855,1149,341"; + + rtls_->startReadingStream(); + waitForTrackerData(); + rtls_->stopReadingStream(); + + EXPECT_EQ(tracker_msg_.timestamp, 118334716); + + EXPECT_EQ(tracker_msg_.anchors_data.size(), 3); + + EXPECT_EQ(tracker_msg_.anchors_data.at(0).number, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).id, 0x14B3); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_x, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_y, 1800); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_z, 1300); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).distance, 1433); + + EXPECT_EQ(tracker_msg_.anchors_data.at(1).number, 1); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).id, 0xC326); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_x, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_y, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_z, 1600); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).distance, 1897); + + EXPECT_EQ(tracker_msg_.anchors_data.at(2).number, 2); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).id, 0xCD0E); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).pos_x, 1300); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).pos_y, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).pos_z, 1600); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).distance, 2660); + + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(0), -855); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(1), 1149); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(2), 341); + + EXPECT_EQ(tracker_msg_.is_valid_position, true); +} + +TEST_F(RtlsDriverFixture, trackerStreamLongLocationNotComputed) +{ + tracker_output_data_ = + "118836472,D0,0xC326,0,0,1600,1911,D1,0xCD0E,1300,0,1600,3063," + "D2,0x14B3,0,1800,1300,1428,N/A,N/A,N/A"; + + rtls_->startReadingStream(); + waitForTrackerData(); + rtls_->stopReadingStream(); + + EXPECT_EQ(tracker_msg_.timestamp, 118836472); + + EXPECT_EQ(tracker_msg_.anchors_data.size(), 3); + + EXPECT_EQ(tracker_msg_.anchors_data.at(0).number, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).id, 0xC326); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_x, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_y, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_z, 1600); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).distance, 1911); + + EXPECT_EQ(tracker_msg_.anchors_data.at(1).number, 1); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).id, 0xCD0E); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_x, 1300); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_y, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_z, 1600); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).distance, 3063); + + EXPECT_EQ(tracker_msg_.anchors_data.at(2).number, 2); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).id, 0x14B3); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).pos_x, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).pos_y, 1800); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).pos_z, 1300); + EXPECT_EQ(tracker_msg_.anchors_data.at(2).distance, 1428); + + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(0), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(1), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(2), 0); + + EXPECT_EQ(tracker_msg_.is_valid_position, false); +} + +TEST_F(RtlsDriverFixture, trackerStreamLongLocationOneAnchor) +{ + tracker_output_data_ = + "118836472,D0,0xC326,0,0,1600,1911,N/A,N/A,N/A"; + + rtls_->startReadingStream(); + waitForTrackerData(); + rtls_->stopReadingStream(); + + EXPECT_EQ(tracker_msg_.timestamp, 118836472); + + EXPECT_EQ(tracker_msg_.anchors_data.size(), 1); + + EXPECT_EQ(tracker_msg_.anchors_data.at(0).number, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).id, 0xC326); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_x, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_y, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_z, 1600); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).distance, 1911); + + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(0), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(1), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(2), 0); + + EXPECT_EQ(tracker_msg_.is_valid_position, false); +} + +TEST_F(RtlsDriverFixture, trackerStreamLongLocationTwoAnchors) +{ + tracker_output_data_ = + "118836472,D0,0xC326,0,0,1600,1911,D1,0xCD0E,1300,0,1600,3063,N/A,N/A,N/A"; + + rtls_->startReadingStream(); + waitForTrackerData(); + rtls_->stopReadingStream(); + + EXPECT_EQ(tracker_msg_.timestamp, 118836472); + + EXPECT_EQ(tracker_msg_.anchors_data.size(), 2); + + EXPECT_EQ(tracker_msg_.anchors_data.at(0).number, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).id, 0xC326); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_x, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_y, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).pos_z, 1600); + EXPECT_EQ(tracker_msg_.anchors_data.at(0).distance, 1911); + + EXPECT_EQ(tracker_msg_.anchors_data.at(1).number, 1); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).id, 0xCD0E); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_x, 1300); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_y, 0); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).pos_z, 1600); + EXPECT_EQ(tracker_msg_.anchors_data.at(1).distance, 3063); + + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(0), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(1), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(2), 0); + + EXPECT_EQ(tracker_msg_.is_valid_position, false); +} + +TEST_F(RtlsDriverFixture, trackerStreamShortLocationValid) +{ + tracker_output_data_ = "1529569522,-472,1149,293"; + + rtls_->startReadingStream(); + waitForTrackerData(); + rtls_->stopReadingStream(); + + EXPECT_EQ(tracker_msg_.timestamp, 1529569522); + + EXPECT_EQ(tracker_msg_.anchors_data.size(), 0); + + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(0), -472); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(1), 1149); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(2), 293); + + EXPECT_EQ(tracker_msg_.is_valid_position, true); +} + +TEST_F(RtlsDriverFixture, trackerStreamShortLocationNotComputed) +{ + tracker_output_data_ = "1529971347,N/A,N/A,N/A"; + + rtls_->startReadingStream(); + waitForTrackerData(); + rtls_->stopReadingStream(); + + EXPECT_EQ(tracker_msg_.timestamp, 1529971347); + + EXPECT_EQ(tracker_msg_.anchors_data.size(), 0); + + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(0), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(1), 0); + EXPECT_EQ(tracker_msg_.tracker_position_xyz.at(2), 0); + + EXPECT_EQ(tracker_msg_.is_valid_position, false); +} + +} // namespace terabee + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + + return ret; +} diff --git a/tests/test_serial.cpp b/tests/test_serial.cpp new file mode 100644 index 0000000..b526c88 --- /dev/null +++ b/tests/test_serial.cpp @@ -0,0 +1,224 @@ +#include +#include +#include + +#include +#include +#include + +#ifdef __linux__ + +#include "serial_communication/serial.hpp" +#include "serial_communication/crc.hpp" +using SerialInterface = terabee::serial_communication::Serial; + +#elif defined(__MINGW32__) || defined(__MINGW64__) || defined(_WIN32) || defined(_WIN64) + +#include "serial_communication/serial_windows.hpp" +#include "serial_communication/crc.hpp" +using SerialInterface = terabee::serial_communication::SerialWindows; + +#endif + +namespace terabee { + +using terabee::serial_communication::ISerial; + +/* + * Linux null modem emulator is required to test some functionalities of + * this SerialCommunication. If the appropriate devices are not found in the + * system, some tests will be aborted and just return success. + * + * https://github.com/freemed/tty0tty + */ +namespace { +bool fileExists(const std::string& name) { + struct stat buffer; + return (stat(name.c_str(), &buffer) == 0); +} +} // unnamed namespace + +class SerialFixture: public testing::Test { +protected: + SerialFixture(): + device0Filename_("/dev/tnt0"), + device1Filename_("/dev/tnt1") + { + sut0_.reset(new SerialInterface(device0Filename_)); + sut1_.reset(new SerialInterface(device1Filename_)); + } + + void SetUp() override + { + ASSERT_TRUE(fileExists(device0Filename_)); + ASSERT_TRUE(fileExists(device1Filename_)); + } + + std::string device0Filename_; + std::string device1Filename_; + std::unique_ptr sut0_; + std::unique_ptr sut1_; + + const uint8_t CMD_PRINTOUT_TEXT[4] = { 0x00, 0x11, 0x01, 0x45 }; + const uint8_t CMD_PRINTOUT_BINARY[4] = { 0x00, 0x11, 0x02, 0x4C }; + const uint8_t CMD_DEACTIVATE_DEBUG[7] = { 0x00, 0x54, 0x0D, 0x00, 0x00, 0x00, 0x8B }; + const uint8_t CMD_ACTIVATE_DEBUG[7] = { 0x00, 0x54, 0x0D, 0x01, 0x00, 0x00, 0xE0 }; + const uint8_t CMD_AUTOCALIBRATE[5] = { 0x00, 0x22, 0x00, 0x00, 0x95 }; + const uint8_t CMD_EMA_WINDOW[5] = { 0x00, 0x52, 0x01, 0x0A, 0xD1 }; + const uint8_t FRAME_BINARY[6] = { 'F', 'M', 0x02, 0x04, 0x01, 0x63 }; +}; + +TEST_F(SerialFixture, crc8CorrectFrames) +{ + EXPECT_EQ(crc::crc8(CMD_PRINTOUT_TEXT, 3), 0x45); + EXPECT_EQ(crc::crc8(CMD_PRINTOUT_BINARY, 3), 0x4C); + EXPECT_EQ(crc::crc8(CMD_DEACTIVATE_DEBUG, 6), 0x8B); + EXPECT_EQ(crc::crc8(CMD_ACTIVATE_DEBUG, 6), 0xE0); + EXPECT_EQ(crc::crc8(CMD_AUTOCALIBRATE, 4), 0x95); + EXPECT_EQ(crc::crc8(CMD_EMA_WINDOW, 4), 0xD1); + EXPECT_EQ(crc::crc8(FRAME_BINARY, 5), 0x63); +} + +TEST_F(SerialFixture, crc8BadFrames) +{ + EXPECT_NE(crc::crc8(CMD_PRINTOUT_TEXT, 3), 0x15); + EXPECT_NE(crc::crc8(CMD_PRINTOUT_BINARY, 3), 0x9C); + EXPECT_NE(crc::crc8(CMD_DEACTIVATE_DEBUG, 6), 0xAB); + EXPECT_NE(crc::crc8(CMD_ACTIVATE_DEBUG, 6), 0xEA); + EXPECT_NE(crc::crc8(CMD_AUTOCALIBRATE, 4), 0xDE); + EXPECT_NE(crc::crc8(CMD_EMA_WINDOW, 4), 0xDF); + EXPECT_NE(crc::crc8(FRAME_BINARY, 5), 0x67); +} + +TEST_F(SerialFixture, returnFalseIfOpenedOrClosedTwice) +{ + ASSERT_TRUE(sut0_->open()); + ASSERT_FALSE(sut0_->open()); + ASSERT_TRUE(sut0_->isOpen()); + ASSERT_TRUE(sut0_->close()); + ASSERT_FALSE(sut0_->close()); + ASSERT_FALSE(sut0_->isOpen()); + + ASSERT_TRUE(sut1_->open()); + ASSERT_FALSE(sut1_->open()); + ASSERT_TRUE(sut1_->isOpen()); + ASSERT_TRUE(sut1_->close()); + ASSERT_FALSE(sut1_->close()); + ASSERT_FALSE(sut1_->isOpen()); +} + +TEST_F(SerialFixture, returnFalseWhenFailToOpenDevice) +{ + sut0_.reset(new SerialInterface("garbage device")); + ASSERT_FALSE(sut0_->open()); +} + +TEST_F(SerialFixture, returnNumberOfBytesAvailableForRead) +{ + uint8_t data[] = {0x01, 0x02, 0x03, 0x04, 0x05}; + size_t num_bytes = sizeof(data); + ASSERT_TRUE(sut0_->setBaudrate(115200)); + ASSERT_TRUE(sut1_->setBaudrate(115200)); + ASSERT_TRUE(sut0_->open()); + ASSERT_TRUE(sut1_->open()); + ASSERT_EQ(num_bytes, sut0_->write(data, num_bytes)); + // hardcoded sleep to make sure transmitted data is available + // TODO: I don't like this solution, because it might fail with heavy CPU load + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + ASSERT_EQ(num_bytes, sut1_->available()); +} + +TEST_F(SerialFixture, returnTrueWhenSetBaudrateParityBytesizeStopbitsFlowcontrolTimeoutBeforeOpen) +{ + ASSERT_TRUE(sut0_->setBaudrate(115200)); + ASSERT_TRUE(sut0_->setParity(ISerial::parity_t::parity_none)); + ASSERT_TRUE(sut0_->setBytesize(ISerial::bytesize_t::eightbits)); + ASSERT_TRUE(sut0_->setStopbits(ISerial::stopbits_t::stopbits_two)); + ASSERT_TRUE(sut0_->setFlowcontrol(ISerial::flowcontrol_t::flowcontrol_hardware)); + ASSERT_TRUE(sut0_->setTimeout(std::chrono::milliseconds(100))); + ASSERT_TRUE(sut0_->open()); +} + +TEST_F(SerialFixture, returnFalseWhenSetBaudrateParityBytesizeStopbitsFlowcontrolTimeoutAfterOpen) +{ + ASSERT_TRUE(sut0_->open()); + ASSERT_FALSE(sut0_->setBaudrate(115200)); + ASSERT_FALSE(sut0_->setParity(ISerial::parity_t::parity_none)); + ASSERT_FALSE(sut0_->setBytesize(ISerial::bytesize_t::eightbits)); + ASSERT_FALSE(sut0_->setStopbits(ISerial::stopbits_t::stopbits_two)); + ASSERT_FALSE(sut0_->setFlowcontrol(ISerial::flowcontrol_t::flowcontrol_hardware)); + ASSERT_FALSE(sut0_->setTimeout(std::chrono::milliseconds(100))); +} + +TEST_F(SerialFixture, readJustOneLineFromBuffer) +{ + uint8_t data[] = {'t', 'e', 's', 't', '\n', 'x', 'x', 'x', '\n'}; + size_t num_bytes = sizeof(data); + std::string expected_line((char*)data, 4); + ASSERT_TRUE(sut0_->open()); + ASSERT_TRUE(sut1_->open()); + ASSERT_EQ(num_bytes, sut0_->write(data, num_bytes)); + ASSERT_EQ(expected_line, sut1_->readline()); +} + +TEST_F(SerialFixture, read0BytesAfterFlush) +{ + uint8_t data[] = {0x01, 0x02, 0x03, 0x04, 0x05}; + size_t num_bytes = sizeof(data); + ASSERT_TRUE(sut1_->setTimeout(std::chrono::milliseconds(1))); + ASSERT_TRUE(sut0_->open()); + ASSERT_TRUE(sut1_->open()); + ASSERT_EQ(num_bytes, sut0_->write(data, num_bytes)); + ASSERT_TRUE(sut1_->flushInput()); + ASSERT_EQ(0, sut1_->read(data, num_bytes)); +} + +//TODO: not sure how can I test flushOutput + +TEST_F(SerialFixture, readWriteCorrectly) +{ + ASSERT_TRUE(sut0_->setBaudrate(115200)); + ASSERT_TRUE(sut0_->setTimeout(std::chrono::milliseconds(100))); + ASSERT_TRUE(sut0_->setParity(ISerial::parity_t::parity_even)); + ASSERT_TRUE(sut0_->setStopbits(ISerial::stopbits_t::stopbits_two)); + ASSERT_TRUE(sut0_->setFlowcontrol(ISerial::flowcontrol_t::flowcontrol_software)); + ASSERT_TRUE(sut1_->setBaudrate(115200)); + ASSERT_TRUE(sut1_->setTimeout(std::chrono::milliseconds(100))); + ASSERT_TRUE(sut1_->setParity(ISerial::parity_t::parity_even)); + ASSERT_TRUE(sut1_->setStopbits(ISerial::stopbits_t::stopbits_two)); + ASSERT_TRUE(sut1_->setFlowcontrol(ISerial::flowcontrol_t::flowcontrol_software)); + uint8_t data[] = {0x01, 0x02, 0x03, 0x04, 0x05}; + size_t num_bytes = sizeof(data); + uint8_t received_data[5] = {0, 0, 0, 0, 0}; + ASSERT_TRUE(sut0_->open()); + ASSERT_TRUE(sut1_->open()); + ASSERT_EQ(num_bytes, sut0_->write(data, num_bytes)); + ASSERT_EQ(num_bytes, sut1_->read(received_data, num_bytes)); + for (size_t i = 0; i < num_bytes; i++) + { + ASSERT_EQ(data[i], received_data[i]); + } +} + +TEST_F(SerialFixture, return0ReadWriteWhenNotOpen) +{ + ASSERT_TRUE(sut0_->setBaudrate(115200)); + ASSERT_TRUE(sut0_->setTimeout(std::chrono::milliseconds(100))); + ASSERT_TRUE(sut1_->setBaudrate(115200)); + ASSERT_TRUE(sut1_->setTimeout(std::chrono::milliseconds(100))); + uint8_t data[] = {0x01, 0x02, 0x03, 0x04, 0x05}; + size_t num_bytes = sizeof(data); + uint8_t received_data[5] = {0, 0, 0, 0, 0}; + ASSERT_EQ(0, sut0_->write(data, num_bytes)); + ASSERT_EQ(0, sut1_->read(received_data, num_bytes)); +} + +} // namespace terabee + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + + return ret; +}