robot/project/Spoolend/modbus.c

#include "modbus.h"
#include <string.h>

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/
uint16_t holdingRegisters[REGISTER_COUNT] = {
    0x0032, 0x0032, 0x0032, 0x0032,
    0x0000, 0x0000, 0x0000, 0x0000,
    0x0000, 0x0000, 0x0000, 0x0000
};

uint8_t registerWritable[REGISTER_COUNT] = {
    1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1
};

uint8_t RS485_1_RX = 0;
uint8_t RS485_1_Host_Existed = 0;
char dmk_read_state = 0;

/*----------------------------------------------*
 * 内部变量                                     *
 *----------------------------------------------*/
static TComCtrl *s_ptMasterCom = NULL;  /* Modbus 主机通信对象 */
static uint8_t response[256];
static uint8_t master_tx_buf[256];

/*----------------------------------------------*
 * 内部函数声明                                 *
 *----------------------------------------------*/
static void Modbus_HandleFunction03(uint16_t startAddress, uint16_t registerCount);
static void Modbus_HandleFunction06(uint16_t registerAddress, uint16_t value);
static void Modbus_HandleFunction10(uint16_t startAddress, uint16_t registerCount, uint8_t *data);
static void Modbus_SendExceptionResponse(uint8_t exceptionCode);
static void Modbus_SendResponse(TComCtrl *ptCom, uint8_t *data, uint8_t length);

/*----------------------------------------------*
 * CRC 计算                                     *
 *----------------------------------------------*/
uint16_t CalculateCRC(uint8_t *data, uint8_t length)
{
    uint16_t crc = 0xFFFF;

    for (uint8_t i = 0; i < length; i++)
    {
        crc ^= (uint16_t)data[i];

        for (uint8_t j = 0; j < 8; j++)
        {
            if ((crc & 0x0001) != 0)
            {
                crc >>= 1;
                crc ^= 0xA001;
            }
            else
            {
                crc >>= 1;
            }
        }
    }

    return crc;
}

/*----------------------------------------------*
 * TComCtrl check 回调                          *
 * 返回 >0: 帧长, =0: 数据不足, <0: 错误       *
 *----------------------------------------------*/
int Modbus_Slave_Check(char *_pBuffer, uint32_t _iSize)
{
    if (_iSize < 4) return 0;

    uint8_t func = (uint8_t)_pBuffer[1];
    uint16_t expected_len = 0;

    switch (func)
    {
    case MODBUS_FUNCTION_CODE_03:
    case MODBUS_FUNCTION_CODE_06:
        expected_len = 8;
        break;
    case MODBUS_FUNCTION_CODE_10:
        if (_iSize >= 7)
        {
            uint8_t byteCount = (uint8_t)_pBuffer[6];
            expected_len = 7 + byteCount + 2;
        }
        else
        {
            return 0;
        }
        break;
    default:
        return -1; // 由于这一行导致当前的不支持功能码的行为也是静默不响应
    }

    if (_iSize < expected_len) return 0;

    /* CRC 校验 */
    if (!MODBUS_CRC_OFF)
    {
        uint16_t crcReceived = ((uint8_t)_pBuffer[expected_len - 1] << 8)
                              | (uint8_t)_pBuffer[expected_len - 2];
        uint16_t crcCalculated = CalculateCRC((uint8_t *)_pBuffer, expected_len - 2);
        if (crcReceived != crcCalculated) return -1;
    }

    return expected_len;
}

/*----------------------------------------------*
 * TComCtrl decode 回调                         *
 * 由 rd_ComRead 内部调用，处理完整 Modbus 帧   *
 *----------------------------------------------*/
void Modbus_Slave_Decode(const char *_pBuffer, uint32_t _iSize)
{
    uint16_t startAddress = 0;
    uint16_t registerCount = 0;

    RS485_1_Host_Existed = 1;

    if ((uint8_t)_pBuffer[0] != MODBUS_SLAVE_ADDRESS) return;

    RS485_1_RX = 1;

    switch ((uint8_t)_pBuffer[1])
    {
    case MODBUS_FUNCTION_CODE_03:
        startAddress = ((uint8_t)_pBuffer[2] << 8) | (uint8_t)_pBuffer[3];
        registerCount = ((uint8_t)_pBuffer[4] << 8) | (uint8_t)_pBuffer[5];
        Modbus_HandleFunction03(startAddress, registerCount);
        break;

    case MODBUS_FUNCTION_CODE_06:
        startAddress = ((uint8_t)_pBuffer[2] << 8) | (uint8_t)_pBuffer[3];
        {
            uint16_t value = ((uint8_t)_pBuffer[4] << 8) | (uint8_t)_pBuffer[5];
            Modbus_HandleFunction06(startAddress, value);
        }
        break;

    case MODBUS_FUNCTION_CODE_10:
        startAddress = ((uint8_t)_pBuffer[2] << 8) | (uint8_t)_pBuffer[3];
        registerCount = ((uint8_t)_pBuffer[4] << 8) | (uint8_t)_pBuffer[5];
        {
            uint8_t byteCount = (uint8_t)_pBuffer[6];
            if (byteCount == registerCount * 2 && _iSize >= 7 + byteCount + 2)
            {
                Modbus_HandleFunction10(startAddress, registerCount, (uint8_t *)&_pBuffer[7]);
            }
            else
            {
                Modbus_SendExceptionResponse(0x03);// 数据长度错误
            }
        }
        break;

    default:
        //Modbus_SendExceptionResponse(0x01); // 不支持的功能码
        break;
    }
}

/*----------------------------------------------*
 * Modbus FC03 - 读保持寄存器                   *
 *----------------------------------------------*/
static void Modbus_HandleFunction03(uint16_t startAddress, uint16_t registerCount)
{
    if (startAddress < REGISTER_START_ADDRESS
        || startAddress + registerCount > REGISTER_START_ADDRESS + REGISTER_COUNT
        || registerCount > 0x7D)
    {
        Modbus_SendExceptionResponse(0x02);// 地址错误
        return;
    }

    uint8_t byteCount = registerCount * 2;
    uint8_t index = 0;

    response[index++] = MODBUS_SLAVE_ADDRESS;
    response[index++] = MODBUS_FUNCTION_CODE_03;
    response[index++] = byteCount;

    for (uint16_t i = 0; i < registerCount; i++)
    {
        uint16_t regValue = holdingRegisters[(startAddress - REGISTER_START_ADDRESS) + i];
        response[index++] = (regValue >> 8) & 0xFF;
        response[index++] = regValue & 0xFF;
    }

    uint16_t crc = CalculateCRC(response, index);
    response[index++] = crc & 0xFF;
    response[index++] = (crc >> 8) & 0xFF;

    /* Modbus 从机响应通过 g_ptRS485_1 发送（由 Spoolend 传入）*/
    /* 这里使用 s_ptMasterCom 临时存储从机 com，后面在 Init 中设置 */
    /* 实际发送通过 decode 上下文中的 com 对象 */
    /* 由于 decode 回调没有传入 com 指针，需要一个全局引用 */
    extern TComCtrl *g_ptRS485_1;
    Modbus_SendResponse(g_ptRS485_1, response, index);
}

/*----------------------------------------------*
 * Modbus FC06 - 写单个寄存器                   *
 *----------------------------------------------*/
static void Modbus_HandleFunction06(uint16_t registerAddress, uint16_t value)
{
    if (registerAddress < REGISTER_START_ADDRESS
        || registerAddress >= REGISTER_START_ADDRESS + REGISTER_COUNT)
    {
        Modbus_SendExceptionResponse(0x02);// 地址错误
        return;
    }

    holdingRegisters[registerAddress - REGISTER_START_ADDRESS] = value;

    uint8_t index = 0;
    response[index++] = MODBUS_SLAVE_ADDRESS;
    response[index++] = MODBUS_FUNCTION_CODE_06;
    response[index++] = (registerAddress >> 8) & 0xFF;
    response[index++] = registerAddress & 0xFF;
    response[index++] = (value >> 8) & 0xFF;
    response[index++] = value & 0xFF;

    uint16_t crc = CalculateCRC(response, index);
    response[index++] = crc & 0xFF;
    response[index++] = (crc >> 8) & 0xFF;

    extern TComCtrl *g_ptRS485_1;
    Modbus_SendResponse(g_ptRS485_1, response, index);
}

/*----------------------------------------------*
 * Modbus FC10 - 写多个寄存器                   *
 *----------------------------------------------*/
static void Modbus_HandleFunction10(uint16_t startAddress, uint16_t registerCount, uint8_t *data)
{
    if (startAddress < REGISTER_START_ADDRESS
        || startAddress + registerCount > REGISTER_START_ADDRESS + REGISTER_COUNT
        || registerCount == 0 || registerCount > 0x7B)
    {
        Modbus_SendExceptionResponse(0x02);// 地址错误
        return;
    }

    for (uint16_t i = 0; i < registerCount; i++)
    {
        uint16_t regIndex = (startAddress - REGISTER_START_ADDRESS) + i;
        uint16_t value = (data[i * 2] << 8) | data[i * 2 + 1];

        if (registerWritable[regIndex])
        {
            holdingRegisters[regIndex] = value;
        }
    }

    uint8_t index = 0;
    response[index++] = MODBUS_SLAVE_ADDRESS;
    response[index++] = MODBUS_FUNCTION_CODE_10;
    response[index++] = (startAddress >> 8) & 0xFF;
    response[index++] = startAddress & 0xFF;
    response[index++] = (registerCount >> 8) & 0xFF;
    response[index++] = registerCount & 0xFF;

    uint16_t crc = CalculateCRC(response, index);
    response[index++] = crc & 0xFF;
    response[index++] = (crc >> 8) & 0xFF;

    extern TComCtrl *g_ptRS485_1;
    Modbus_SendResponse(g_ptRS485_1, response, index);
}

/*----------------------------------------------*
 * 发送异常响应                                 *
 *----------------------------------------------*/
static void Modbus_SendExceptionResponse(uint8_t exceptionCode)
{
    extern TComCtrl *g_ptRS485_1;

    response[0] = MODBUS_SLAVE_ADDRESS;
    response[1] = 0x80; //这里与原版差异为直接回复0x80而不是与一个功能码
    response[2] = exceptionCode;

    uint16_t crc = CalculateCRC(response, 3);
    response[3] = crc & 0xFF;
    response[4] = (crc >> 8) & 0xFF;

    Modbus_SendResponse(g_ptRS485_1, response, 5);
}

/*----------------------------------------------*
 * 发送响应（通过 TComCtrl 异步发送）           *
 *----------------------------------------------*/
static void Modbus_SendResponse(TComCtrl *ptCom, uint8_t *data, uint8_t length)
{
    if (ptCom)
    {
        rd_ComWrite(ptCom, (char *)data, length);
    }
}

/*----------------------------------------------*
 * Modbus 主机初始化                            *
 *----------------------------------------------*/
void Modbus_Master_Init(TComCtrl *ptRS485)
{
    s_ptMasterCom = ptRS485;
}

/*----------------------------------------------*
 * Modbus 主机 - 写单个寄存器                   *
 *----------------------------------------------*/
void MB_WriteHoldingReg(uint8_t _addr, uint16_t _reg, uint16_t _data)
{
    uint16_t TxCount = 0;
    uint16_t crc = 0;

    master_tx_buf[TxCount++] = _addr;
    master_tx_buf[TxCount++] = 0x06;
    master_tx_buf[TxCount++] = _reg >> 8;
    master_tx_buf[TxCount++] = _reg;
    master_tx_buf[TxCount++] = _data >> 8;
    master_tx_buf[TxCount++] = _data;

    crc = CalculateCRC(master_tx_buf, TxCount);
    master_tx_buf[TxCount++] = crc & 0xFF;
    master_tx_buf[TxCount++] = (crc >> 8) & 0xFF;

    if (s_ptMasterCom)
    {
        rd_ComWrite(s_ptMasterCom, (char *)master_tx_buf, TxCount);
    }
    dmk_read_state = 0;
}

/*----------------------------------------------*
 * DMK 响应超时检测                              *
 *----------------------------------------------*/
void calculate_dmk(void)
{
    dmk_read_state = 1;
}