MODBUS RTU Device Info Getter for ESP32 series

modbus_rtu_get_device_info.cpp

/*
 * modbus_rtu_get_device_info.cpp
 * MODBUS RTU Device Info Getter for ESP32 series
 *
 * SPDX-FileCopyrightText: 2024 Tomoatsu Shimada/Walbrix Corporation
 *
 * SPDX-License-Identifier: MIT
 */

#include <cstdint>
#include <cstring>
#include <iostream>
#include <map>
#include <format>
#include <variant>

#include "sdkconfig.h"
#include "esp_log.h"
#include "esp_crc.h"
#include "driver/uart.h"
#include "driver/gpio.h"

static const char* TAG = "modbus_get_device_info";

static std::pair<uint8_t/*lo*/,uint8_t/*hi*/> 
    crc16_modbus(const uint8_t *data, size_t length)
{
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < length; i++) {
        crc ^= data[i];
        for (int j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return {(uint8_t)(crc & 0xff), (uint8_t)((crc & 0xff00) >> 8)};
}

std::variant<std::map<uint8_t,std::string>,std::string>
    get_basic_device_info(uart_port_t uart_num, uint8_t slave_id = 1)
{
    uint8_t message[] = {slave_id, 0x2b, 0x0e, 0x01/*basic info*/,0x00, 0x00, 0x00 };
    auto [lo, hi] = crc16_modbus(message, sizeof(message) - 2);
    message[sizeof(message) - 2] = lo;
    message[sizeof(message) - 1] = hi;

    uart_write_bytes(uart_num, (const char*)message, sizeof(message));

    ESP_LOGI(TAG, "Sent %d octets", sizeof(message));

    uint8_t data[255];
    ssize_t message_size = uart_read_bytes(uart_num, data, sizeof(data), 100);

    if (message_size < 0) return "Read error(Timeout)";

    //else
    /* bad practice */
    if (message_size >= 7/*modbus hdr+crc+0x00*2*/ && data[0] == 0x00 && data[message_size - 1] == 0x00) {
        // remove leading 0x00 and trailing 0x00
        memmove(data, data + 1, message_size - 2);
        message_size -= 2;
        ESP_LOGI(TAG, "Removed leading and trailing 0x00");
    }

    ESP_LOGI(TAG, "Received %d octets", message_size);

    if (message_size < 3/*hdr*/ + 2/*crc*/) {
        return std::format("Response message too short({} octets)", message_size);
    }

    auto [crc_lo, crc_hi] = crc16_modbus(data, message_size - 2);

    if (data[message_size - 2] != crc_lo || data[message_size - 1] != crc_hi) {
        std::string message_hex;
        char buf[4];
        for (int i = 0; i < message_size; i++) {
            sprintf(buf, "%02x ", data[i]);
            message_hex += buf;
        }
        return std::format("CRC mismatch. Message=[ {} ]({} bytes), expected={:02x} {:02x}", 
            message_hex, message_size, crc_lo, crc_hi);
    }

    if (data[0] != slave_id) {
        return std::format("Rresponse slave id mismatch. expected=0x{:02x},actual=0x{:02x}.",
            (int)slave_id, (int)data[0]);
    }
    if (data[1] != 0x2b || data[2] != 0x0e || data[3] != 0x01) {
        return "Function code/field mismatch. expected=0x2b 0x0e 0x01.";
    }
    // else

    std::map<uint8_t,std::string> info;
    uint8_t num_objects = data[7];
    const uint8_t* pt = data + 8;
    for (int i = 0; i < num_objects; i++) {
        if (pt - data + 2 >= message_size) {
            return "Device info response is too short";
        }
        uint8_t object_id = pt[0];
        uint8_t object_length = pt[1];
        const uint8_t* object_value = pt + 2;
        if (pt - data + 2 + object_length >= message_size) {
            return "Device info response is too short";
        }
        info[object_id] = std::string((const char*)object_value, object_length);
        pt += 2 + object_length;
    }
    return info;
}

void real_main(
    uart_port_t uart_num,
    gpio_num_t txd, gpio_num_t rxd, gpio_num_t de_nre,
    uint8_t slave_id = 1
)
{
    uart_config_t uart_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .rx_flow_ctrl_thresh = 122,
        .source_clk = UART_SCLK_DEFAULT,
    };

    ESP_ERROR_CHECK(uart_driver_install(uart_num, 127 * 2, 0, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(uart_num, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(uart_num, txd, rxd, de_nre, -1));
    ESP_ERROR_CHECK(uart_set_mode(uart_num, uart_mode_t::UART_MODE_RS485_HALF_DUPLEX));
    ESP_ERROR_CHECK(uart_set_rx_timeout(uart_num, 3));
    ESP_LOGI(TAG, "UART %d initialized.  TXD pin=%d, RXD pin=%d, DE/NRE pin = %d", uart_num, txd, rxd, de_nre);

    vTaskDelay(10);

retry:;
    auto result = get_basic_device_info(uart_num);
    if (std::holds_alternative<std::string>(result)) {
        ESP_LOGE(TAG, "Modbus Error: %s", std::get<std::string>(result).c_str());
        std::cout << "Comminication error.  Retry in 5 seconds." << std::endl;
        vTaskDelay(5000 / portTICK_PERIOD_MS);
        goto retry;
    }
    //else
    auto info = std::get<std::map<uint8_t,std::string>>(result);
    ESP_LOGI(TAG, "Device Info received.");
    for (const auto& kv : info) {
        std::cout << "key=" << ((int)kv.first) << ", value=" << kv.second << std::endl;
    }
}

extern "C" { void app_main(void) { 
    real_main(
        /* UART NUM =   */ UART_NUM_1,
        /* TXD PIN =    */ GPIO_NUM_4,
        /* RXD PIN =    */ GPIO_NUM_5,
        /* DE/NRE PIN = */ GPIO_NUM_6,
        /* SLAVE ID =   */ 1
    );
 } }