agrobot_base/Firmware/Modulos/ModbusService.h

#include <Arduino.h>
#include <HardwareSerial.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>

#ifndef ModbusService
#define ModbusService

HardwareSerial SerialDriver(1);
#define RXD1 16
#define TXD1 17
#define DE_RE_PIN 4 // Pino para DE e RE
bool Debug = false;

class ModbusGetResponse {
    public:
        byte _address;
        byte _command_code;
        byte _num_registers;
        double _value;
        bool _success = false;

        ModbusGetResponse() : _address(0), _command_code(0), _num_registers(0), _value(0.0) {}
};

// Definir prioridades
#define PRIORITY_HIGH 1
#define PRIORITY_LOW 0

// Estrutura para armazenar dados da mensagem Modbus
struct ModbusMessage {
    uint8_t slaveAddress;
    uint8_t functionCode;
    uint16_t registerAddress;
    int value;
    uint8_t* response;
    uint8_t* length;
    SemaphoreHandle_t responseSemaphore;
    uint8_t priority;
    bool waitCallback;
};

// Fila para armazenar mensagens Modbus
QueueHandle_t modbusQueue;

// Função para calcular CRC16
uint16_t calculateCRC(const uint8_t* data, uint8_t length) {
    uint16_t crc = 0xFFFF;

    for (uint8_t i = 0; i < length; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

void processModbusMessage(ModbusMessage& message) {
    uint8_t command[8];

    command[0] = message.slaveAddress;
    command[1] = message.functionCode;
    command[2] = (message.registerAddress >> 8) & 0xFF; // Big endian
    command[3] = message.registerAddress & 0xFF;
        
    // Converter o valor inteiro para o formato adequado
    command[4] = (message.value >> 8) & 0xFF; // Big endian
    command[5] = message.value & 0xFF;

    uint16_t crc = calculateCRC(command, 6);
    command[6] = crc & 0xFF;
    command[7] = (crc >> 8) & 0xFF;

    // Alternar para modo de transmissão
    digitalWrite(DE_RE_PIN, HIGH);
    vTaskDelay(pdMS_TO_TICKS(1)); // Pequeno atraso para garantir que o modo de transmissão esteja ativo

    SerialDriver.write(command, 8);
    if (Debug) {
        // Imprimir o comando em formato hexadecimal
        Serial.print("Comando enviado: ");
        for (int i = 0; i < 8; i++) {
            if (command[i] < 0x10) {
                Serial.print("0"); // Adiciona um zero à esquerda para bytes menores que 0x10
            }
            Serial.print(command[i], HEX);
            Serial.print(" ");
        }
        Serial.println();
    }

    // Alternar para modo de recepção
    SerialDriver.flush(); // Garantir que todos os dados foram enviados
    digitalWrite(DE_RE_PIN, LOW);
    vTaskDelay(pdMS_TO_TICKS(1)); // Pequeno atraso para garantir que o modo de recepção esteja ativo

    *message.length = 0;

    if (message.waitCallback) {
        // Aguardar a resposta
        vTaskDelay(pdMS_TO_TICKS(100)); // Ajuste o tempo de delay conforme necessário

        // Ler a resposta
        if (SerialDriver.available()) {
            *message.length = SerialDriver.readBytes(message.response, SerialDriver.available());

            if (Debug) {
                // Imprimir a resposta em formato hexadecimal
                Serial.print("Resposta recebida: ");
                for (int i = 0; i < *message.length; i++) {
                    if (message.response[i] < 0x10) {
                        Serial.print("0"); // Adiciona um zero à esquerda para bytes menores que 0x10
                    }
                    Serial.print(message.response[i], HEX);
                    Serial.print(" ");
                }
                Serial.println();
            }
        }
    }
    else {
        vTaskDelay(pdMS_TO_TICKS(50));
    }

    // Sinalizar que a resposta está disponível
    xSemaphoreGive(message.responseSemaphore);
}

void processModbusQueue(void* pvParameters) {
    ModbusMessage message;

    while (true) {
        // Aguardar até que uma mensagem esteja disponível na fila
        if (xQueueReceive(modbusQueue, &message, portMAX_DELAY) == pdPASS) {
            // Processar mensagem de alta prioridade imediatamente
            if (message.priority == PRIORITY_HIGH) {
                // Processar mensagem de alta prioridade
                processModbusMessage(message);
            } else {
                // Verificar se há mensagens de alta prioridade na fila
                ModbusMessage highPriorityMessage;
                if (xQueuePeek(modbusQueue, &highPriorityMessage, 0) == pdPASS && highPriorityMessage.priority == PRIORITY_HIGH) {
                    // Recolocar a mensagem de baixa prioridade na fila
                    xQueueSendToBack(modbusQueue, &message, 0);
                } else {
                    // Processar mensagem de baixa prioridade
                    processModbusMessage(message);
                }
            }
        }
    }
}


// Função para inicializar o Modbus
void InicializarModbus(bool _debug = false) {
    SerialDriver.begin(9600, SERIAL_8N2, RXD1, TXD1);
    Serial.println("Modbus Inicializado");

    pinMode(DE_RE_PIN, OUTPUT);
    digitalWrite(DE_RE_PIN, LOW); // Inicialmente em modo de recepção

    Debug = _debug;

    // Criar a fila de mensagens Modbus
    modbusQueue = xQueueCreate(10, sizeof(ModbusMessage));
    if (modbusQueue == NULL) {
        Serial.println("Erro ao criar a fila de mensagens Modbus");
    }

    // Criar a tarefa de processamento da fila
    xTaskCreate(processModbusQueue, "ProcessModbusQueue", 4096, NULL, 1, NULL);
}

// Função para construir o comando Modbus
void sendModbusCommand(uint8_t slaveAddress, uint8_t functionCode, uint16_t registerAddress, int value, uint8_t* response, uint8_t& length, bool waitCallback) {
    ModbusMessage message;
    message.slaveAddress = slaveAddress;
    message.functionCode = functionCode;
    message.registerAddress = registerAddress;
    message.value = value;
    message.response = response;
    message.length = &length;
    message.priority = functionCode == 0x06 ? PRIORITY_HIGH : PRIORITY_LOW;
    message.waitCallback = waitCallback;
    message.responseSemaphore = xSemaphoreCreateBinary();

    // Adicionar a mensagem à fila
    if (xQueueSend(modbusQueue, &message, portMAX_DELAY) != pdPASS) {
        Serial.println("Erro ao adicionar mensagem à fila");
    }

    // Esperar até que a resposta esteja disponível
    xSemaphoreTake(message.responseSemaphore, portMAX_DELAY);
    vSemaphoreDelete(message.responseSemaphore);
}

#endif