Establish the basic anlysis of modbus command

CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.8)
+project(modbus)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+set(CMAKE_BUILD_TYPE "release")
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(src src/modbus_command.cpp)
+
+add_library(${PROJECT_NAME} SHARED ${src})
+target_include_directories(${PROJECT_NAME} PUBLIC include)

include/modbus_command.hpp

@@ -0,0 +1,33 @@
+#ifndef MODBUS_COMMAND_HPP
+#define MODBUS_COMMAND_HPP
+
+#include <cstdint>
+#include <vector>
+
+namespace CommunicationStuff {
+
+class ModbusCommand
+{
+  public:
+    uint8_t get_slave_addr();
+    void set_slave_addr(const uint8_t& address);
+    std::vector<uint8_t> get_send_data();
+    void set_send_data(const std::vector<uint8_t>& data);
+
+    bool parse_response(const std::vector<uint8_t>& response_data,
+                        std::vector<uint16_t>& register_data);
+    ModbusCommand(uint8_t default_address);
+    ~ModbusCommand();
+
+  private:
+    uint8_t slave_address_;
+    std::vector<uint8_t> send_data_;
+    std::vector<uint8_t> response_data_;
+    std::vector<uint8_t> register_data_;
+
+};
+
+
+}  // namespace CommunicationStuff
+
+#endif

src/log.hpp

@@ -0,0 +1,26 @@
+#ifndef LOG_HPP
+#define LOG_HPP
+
+/**
+ * @brief define log function on linux or android
+ *
+ */
+
+#ifdef __linux__
+#include <cstdio>
+
+#define LOGI(fmt, ...) printf("[Info] " fmt "\n", ##__VA_ARGS__)
+#define LOGD(fmt, ...) printf("[Debug] " fmt "\n", ##__VA_ARGS__)
+#define LOGE(fmt, ...) printf("[Error] " fmt "\n", ##__VA_ARGS__)
+// #    define LOGE(...) printf("[ERROR]",format __VA_ARGS__)
+
+#else
+
+#    include <android/log.h>
+#    define LOGD(...)                                                          \
+        __android_log_print(ANDROID_LOG_INFO, "NoiseReduction", __VA_ARGS__)
+#    define LOGE(...)                                                          \
+        __android_log_print(ANDROID_LOG_ERROR, "NoiseReduction", __VA_ARGS__)
+#endif
+
+#endif  // MY_APPLICATION_MY_LOG_H

src/modbus_command.cpp

@@ -0,0 +1,188 @@
+#include <cstdint>
+#include <vector>
+
+#include "log.hpp"
+#include "modbus_command.hpp"
+
+namespace CommunicationStuff {
+static bool parse_read_holding_registers_response(
+    const std::vector<uint8_t>& response_data,
+    std::vector<uint16_t>& register_data);
+static bool parse_read_input_registers_response(
+    const std::vector<uint8_t>& response_data,
+    std::vector<uint16_t>& register_data);
+static bool parse_write_single_register_response(
+    const std::vector<uint8_t>& response_data,
+    std::vector<uint16_t>& register_data);
+static bool parse_write_multiple_registers_response(
+    const std::vector<uint8_t>& response_data);
+static void add_crc(std::vector<uint8_t>& data);
+static bool check_crc(const std::vector<uint8_t>& data);
+
+ModbusCommand::ModbusCommand(uint8_t default_address)
+    : slave_address_(default_address), send_data_({}), response_data_({}),
+      register_data_({})
+{
+}
+
+ModbusCommand::~ModbusCommand() = default;
+
+uint8_t ModbusCommand::get_slave_addr()
+{
+    return slave_address_;
+}
+
+void ModbusCommand::set_slave_addr(const uint8_t& address)
+{
+    slave_address_ = address;
+}
+
+std::vector<uint8_t> ModbusCommand::get_send_data()
+{
+    return send_data_;
+}
+
+void ModbusCommand::set_send_data(const std::vector<uint8_t>& data)
+{
+    send_data_ = data;
+    add_crc(send_data_);
+}
+
+// parse response data from sensor
+bool ModbusCommand::parse_response(const std::vector<uint8_t>& response_data,
+                                   std::vector<uint16_t>& register_data)
+{
+    // response size should include 1 byte address, 1 byte function code,
+    // 1 byte register count, 2 bytes CRC
+    if (response_data.size() < 5) {
+        LOGE("Error: Invalid Modbus response");
+        return false;
+    }
+
+    if (!check_crc(response_data)) {
+        LOGE("Error: Invalid CRC code");
+    }
+
+    uint8_t function_code = response_data[1];
+    switch (function_code) {
+        case 0x03:
+            return parse_read_holding_registers_response(response_data,
+                                                         register_data);
+        case 0x04:
+            return parse_read_input_registers_response(response_data,
+                                                       register_data);
+
+        case 0x06: return parse_write_single_register_response(response_data, register_data);
+        case 0x10:
+            return parse_write_multiple_registers_response(response_data);
+        default: LOGE("Error: Invalid Modbus function code."); return true;
+    }
+}
+
+static bool parse_read_holding_registers_response(
+    const std::vector<uint8_t>& response_data,
+    std::vector<uint16_t>& register_data)
+{
+    // calculate register number
+    size_t registerCount = response_data[2] / 2;
+    if (registerCount * 2 + 5 != response_data.size()) {
+        LOGE("Error: Invalid Modbus response size");
+        return false;
+    }
+
+    // parse register value
+    for (size_t i = 0; i < registerCount; ++i) {
+        uint16_t registerValue =
+            (response_data[3 + i * 2] << 8) | response_data[4 + i * 2];
+        register_data.emplace_back(registerValue);
+        // register_data.emplace_back(response_data[3 + i * 2]);
+        // register_data.emplace_back(response_data[4 + i * 2]);
+        LOGE("Register %ld: %d\n", i, registerValue);
+    }
+
+    return true;
+}
+
+static bool parse_read_input_registers_response(
+    const std::vector<uint8_t>& response_data,
+    std::vector<uint16_t>& register_data)
+{
+    return parse_read_holding_registers_response(response_data, register_data);
+}
+
+static bool parse_write_single_register_response(
+    const std::vector<uint8_t>& response_data,
+    std::vector<uint16_t>& register_data)
+{
+    if (response_data.size() == 8 && response_data[1] == 0x06) {
+        LOGD("Response OK.\n");
+        register_data.emplace_back(static_cast<uint16_t>(response_data[4]));
+        register_data.emplace_back(static_cast<uint16_t>(response_data[5]));
+        return true;
+    } else {
+        LOGE("Error: Invalid write single register response");
+        return false;
+    }
+}
+
+static bool parse_write_multiple_registers_response(
+    const std::vector<uint8_t>& response_data)
+{
+    if (response_data.size() == 8 && response_data[1] == 0x10) {
+        LOGD("Response OK.\n");
+        return true;
+    } else {
+        LOGE("Error: Invalid write multiple registers response");
+        return false;
+    }
+}
+
+static void add_crc(std::vector<uint8_t>& data)
+{
+    // Calculate CRC
+    uint16_t crc_register = 0xFFFF;
+    for (const uint8_t& byte : data) {
+        crc_register ^= byte;
+
+        for (int i = 0; i < 8; ++i) {
+            bool lsb = crc_register & 0x0001;
+            crc_register >>= 1;
+
+            if (lsb) {
+                crc_register ^= 0xA001;
+            }
+        }
+    }
+
+    uint8_t crc_l = crc_register & 0xFF;
+    uint8_t crc_h = (crc_register >> 8) & 0xFF;
+
+    // Add CRC to data
+    data.push_back(crc_l);
+    data.push_back(crc_h);
+}
+
+static bool check_crc(const std::vector<uint8_t>& data)
+{
+    uint16_t crc_register = 0xFFFF;
+    for (auto iter = data.begin(); iter != data.end() - 2; ++iter) {
+        crc_register ^= *iter;
+
+        for (int i = 0; i < 8; ++i) {
+            bool lsb = crc_register & 0x0001;
+            crc_register >>= 1;
+
+            if (lsb) {
+                crc_register ^= 0xA001;
+            }
+        }
+    }
+
+    uint8_t crc_l = crc_register & 0xFF;
+    uint8_t crc_h = (crc_register >> 8) & 0xFF;
+    if (crc_l == *(data.end() - 2) && crc_h == *(data.end() - 1)) {
+        return true;
+    }
+    return false;
+}
+}  // namespace CommunicationStuff