474 lines
14 KiB
C++
474 lines
14 KiB
C++
#include "SerialService.h"
|
|
|
|
// CanService.h
|
|
#ifndef CANSERVICE_H
|
|
#define CANSERVICE_H
|
|
|
|
/*
|
|
#include <CAN.h>
|
|
#include <Arduino.h>
|
|
|
|
class CanService {
|
|
public:
|
|
typedef void (*OnReceiveCallback)(int packetSize, int senderId, byte funcCode, byte* data, int dataLength);
|
|
|
|
CanService(uint8_t nodeId, uint32_t baudRate = 500E3) {
|
|
_baudRate = baudRate;
|
|
_nodeId = nodeId;
|
|
}
|
|
|
|
void begin() {
|
|
if (!CAN.begin(_baudRate)) {
|
|
PrintTela("[CAN] Falha ao iniciar");
|
|
return;
|
|
}
|
|
|
|
PrintTela("[CAN] Inicializado com sucesso");
|
|
CAN.onReceive(onReceiveWrapper);
|
|
instance = this;
|
|
}
|
|
|
|
void loop() {
|
|
// Apenas processa mensagens recebidas via interrupcao
|
|
}
|
|
|
|
void setReceiveCallback(OnReceiveCallback cb) {
|
|
_callback = cb;
|
|
}
|
|
|
|
bool enviarMensagem(byte funcCode, const std::vector<uint8_t>& payload) {
|
|
if (payload.size() > 7) return false;
|
|
|
|
CAN.beginPacket(_nodeId);
|
|
CAN.write(funcCode);
|
|
for (uint8_t b : payload) {
|
|
CAN.write(b);
|
|
}
|
|
return CAN.endPacket();
|
|
}
|
|
|
|
std::vector<uint8_t> MontarFrameChk(T_Code D_Code, bool Conectado, int Versao) {
|
|
std::vector<uint8_t> resposta;
|
|
resposta.push_back(static_cast<uint8_t>(D_Code)); // Tipo do módulo
|
|
resposta.push_back(Conectado ? 1 : 0); // Conectado
|
|
resposta.push_back(Versao); // Versão
|
|
|
|
return resposta;
|
|
}
|
|
|
|
private:
|
|
uint32_t _baudRate;
|
|
uint8_t _nodeId;
|
|
OnReceiveCallback _callback = nullptr;
|
|
|
|
static CanService* instance;
|
|
|
|
static void onReceiveWrapper(int packetSize) {
|
|
if (instance) instance->onReceive(packetSize);
|
|
}
|
|
|
|
void onReceive(int packetSize) {
|
|
int senderId = CAN.packetId();
|
|
byte buffer[8];
|
|
int i = 0;
|
|
|
|
while (CAN.available() && i < 8) {
|
|
buffer[i++] = CAN.read();
|
|
}
|
|
|
|
byte funcCode = buffer[0];
|
|
if (_callback != nullptr) {
|
|
_callback(packetSize, senderId, funcCode, buffer + 1, i - 1);
|
|
}
|
|
}
|
|
};
|
|
|
|
CanService* CanService::instance = nullptr;
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <driver/twai.h>
|
|
|
|
class CanService {
|
|
|
|
typedef void (*OnReceiveCallback)(int packetSize, int senderId, CanMessagePosicaoDados posicao, byte* data, int dataLength);
|
|
|
|
typedef struct {
|
|
OnReceiveCallback callback;
|
|
int packetSize;
|
|
int senderId;
|
|
CanMessagePosicaoDados posicao;
|
|
byte data[7];
|
|
int dataLength;
|
|
long idMsg;
|
|
} CallbackPayload;
|
|
|
|
static TaskHandle_t handleCallbackTask;
|
|
|
|
public:
|
|
uint8_t ID_Num_sMOD = 0;
|
|
uint8_t ID_Num_sTOD = 250;
|
|
uint8_t ID_Num_sLRA = 251;
|
|
|
|
CanService(uint8_t nodeId, uint32_t baudRate = 500000) {
|
|
_nodeId = nodeId;
|
|
_baudRate = baudRate;
|
|
}
|
|
|
|
void begin() {
|
|
PrintTela("[CAN] Iniciando TWAI...");
|
|
|
|
twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_5, GPIO_NUM_4, TWAI_MODE_NORMAL); // modo sem ACK
|
|
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
|
|
twai_filter_config_t f_config = {
|
|
.acceptance_code = (_nodeId << 21), // Shiftado para alinhar com o padrão TWAI
|
|
.acceptance_mask = ~(0x7FF << 21), // Mascara para filtrar apenas esse ID
|
|
.single_filter = true
|
|
};
|
|
|
|
esp_err_t err;
|
|
|
|
err = twai_driver_install(&g_config, &t_config, &f_config);
|
|
if (err != ESP_OK) {
|
|
PrintTela("[CAN] Erro ao instalar driver: ", false);
|
|
PrintTela(String(err));
|
|
return;
|
|
}
|
|
|
|
err = twai_start();
|
|
if (err != ESP_OK) {
|
|
PrintTela("[CAN] Erro ao iniciar TWAI: ", false);
|
|
PrintTela(String(err));
|
|
return;
|
|
}
|
|
|
|
if (CanTaskRxHandle == NULL) {
|
|
canQueueRx = xQueueCreate(50, sizeof(twai_message_t));
|
|
if (canQueueRx == NULL) {
|
|
PrintTela("[CAN] Falha ao criar fila RX!");
|
|
}
|
|
xTaskCreatePinnedToCore(CanService::CanTaskRxWrapper, "CanTaskRx", 4096, this, 17, &CanTaskRxHandle, APP_CPU_NUM);
|
|
}
|
|
|
|
if (CanTaskTxHandle == NULL) {
|
|
canQueueTx = xQueueCreate(50, sizeof(twai_message_t));
|
|
if (canQueueTx == NULL) {
|
|
PrintTela("[CAN] Falha ao criar fila TX!");
|
|
}
|
|
xTaskCreatePinnedToCore(CanService::CanTaskTxWrapper, "CanTaskTx", 4096, this, 16, &CanTaskTxHandle, APP_CPU_NUM);
|
|
}
|
|
|
|
if (CanTaskProcessHandle == NULL) {
|
|
xTaskCreatePinnedToCore(CanService::CanTaskProcessWrapper, "CanTaskProcess", 6144, this, 18, &CanTaskProcessHandle, APP_CPU_NUM);
|
|
}
|
|
|
|
PrintTela("[CAN] TWAI iniciado com sucesso");
|
|
}
|
|
|
|
|
|
QueueHandle_t canQueueRx;
|
|
TaskHandle_t CanTaskRxHandle = NULL;
|
|
static void CanTaskRxWrapper(void *pvParameters) {
|
|
CanService *service = static_cast<CanService*>(pvParameters);
|
|
service->CanTaskRx(pvParameters);
|
|
}
|
|
|
|
void CanTaskRx(void* pvParameters) {
|
|
twai_message_t message;
|
|
while (true) {
|
|
if (twai_receive(&message, pdMS_TO_TICKS(10)) == ESP_OK) {
|
|
|
|
if (message.data_length_code == 0 || message.extd) continue;
|
|
|
|
bool MensagemTx = message.identifier == static_cast<uint32_t>(_nodeId + 1);
|
|
|
|
if (DebugMode) {
|
|
PrintTela("[CAN ", false);
|
|
PrintTela(MensagemTx ? "TX" : "RX", false);
|
|
PrintTela("] ID: 0x", false);
|
|
PrintTela(String(message.identifier, HEX), false);
|
|
PrintTela(" Len: ", false);
|
|
PrintTela(String(message.data_length_code), false);
|
|
PrintTela(" Data: ", false);
|
|
for (int i = 0; i < message.data_length_code; i++) {
|
|
PrintTela(String(message.data[i], HEX), false);
|
|
PrintTela(" ", false);
|
|
}
|
|
PrintTela("");
|
|
}
|
|
|
|
if (MensagemTx) {
|
|
PrintTela("[CAN RX] Ouviu a própria resposta!");
|
|
continue;
|
|
}
|
|
|
|
if (!mensagemJaNaFila(canQueueRx, message)) {
|
|
|
|
if (uxQueueSpacesAvailable(canQueueRx) < 5) {
|
|
PrintTela("[CAN RX] Alerta: Fila RX quase cheia (" + String(uxQueueSpacesAvailable(canQueueRx)) + " mensagens restantes)");
|
|
}
|
|
|
|
if (xQueueSend(canQueueRx, &message, 0) != pdTRUE) {
|
|
PrintTela("[CAN RX] Fila RX cheia! Mensagem descartada.");
|
|
}
|
|
else {
|
|
//PrintTela("[CAN RX] Mensagem adicionada na fila RX");
|
|
}
|
|
} else {
|
|
PrintTela("[CAN RX] Mensagem duplicada ignorada.");
|
|
}
|
|
}
|
|
vTaskDelay(1);
|
|
}
|
|
|
|
}
|
|
|
|
QueueHandle_t canQueueTx;
|
|
TaskHandle_t CanTaskTxHandle = NULL;
|
|
static void CanTaskTxWrapper(void *pvParameters) {
|
|
CanService *service = static_cast<CanService*>(pvParameters);
|
|
service->CanTaskTx(pvParameters);
|
|
}
|
|
|
|
void CanTaskTx(void* pvParameters) {
|
|
CanService *service = static_cast<CanService*>(pvParameters);
|
|
twai_message_t msg;
|
|
while (true) {
|
|
if (xQueueReceive(service->canQueueTx, &msg, portMAX_DELAY) == pdTRUE) {
|
|
//PrintTela("[CAN TX] Mensagem recebida da fila TX");
|
|
if (service->enviarDadosCan(msg)) {
|
|
//PrintTela("[CAN TX] Mensagem enviada via CAN com sucesso");
|
|
}
|
|
else {
|
|
PrintTela("[CAN TX] Erro ao enviar mensagem via CAN");
|
|
}
|
|
}
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
TaskHandle_t CanTaskProcessHandle = NULL;
|
|
static void CanTaskProcessWrapper(void *pvParameters) {
|
|
CanService *service = static_cast<CanService*>(pvParameters);
|
|
service->CanTaskProcess(pvParameters);
|
|
}
|
|
|
|
void CanTaskProcess(void* pvParameters) {
|
|
CanService *service = static_cast<CanService*>(pvParameters);
|
|
twai_message_t msg;
|
|
|
|
while (true) {
|
|
while (uxQueueMessagesWaiting(service->canQueueRx) > 0) {
|
|
if (xQueueReceive(service->canQueueRx, &msg, portMAX_DELAY) == pdTRUE) {
|
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)msg.data[0];
|
|
|
|
if (service->_callback) {
|
|
String frameEmProcessamento = "";
|
|
for (int i = 0; i < msg.data_length_code; i++) {
|
|
frameEmProcessamento += String(msg.data[i], HEX) + " ";
|
|
}
|
|
|
|
idmsg++;
|
|
|
|
CallbackPayload* payload = new CallbackPayload();
|
|
payload->idMsg = idmsg;
|
|
payload->callback = service->_callback;
|
|
payload->packetSize = msg.data_length_code;
|
|
payload->senderId = msg.identifier;
|
|
payload->posicao = posicao;
|
|
memcpy(payload->data, &msg.data[1], msg.data_length_code - 1);
|
|
payload->dataLength = msg.data_length_code - 1;
|
|
|
|
// Captura o handle da task atual (quem vai esperar a notificação)
|
|
TaskHandle_t callingTaskHandle = xTaskGetCurrentTaskHandle();
|
|
|
|
// Cria um pacote com o payload + handle do notificador
|
|
auto* bundle = new std::pair<CallbackPayload*, TaskHandle_t>(payload, callingTaskHandle);
|
|
|
|
PrintTela("[CAN Process] Processando frame: " + frameEmProcessamento);
|
|
|
|
String taskName = "TaskCallbackTimeout_" + String(idmsg);
|
|
|
|
TaskHandle_t handleTask = nullptr;
|
|
|
|
// Cria a task do callback
|
|
BaseType_t status = xTaskCreatePinnedToCore(
|
|
TaskCallbackWrapper,
|
|
taskName.c_str(),
|
|
8192,
|
|
bundle,
|
|
5,
|
|
&handleTask,
|
|
APP_CPU_NUM
|
|
);
|
|
|
|
if (status != pdPASS) {
|
|
PrintTela("[CAN Process] ❌ Falha ao criar task de callback!");
|
|
delete payload;
|
|
delete bundle;
|
|
continue;
|
|
}
|
|
|
|
if (false && msg.data[1] == ID_Num_sLRA) {
|
|
// Comandos para o LoRa nao precisam aguardar, podem ser processados sem tempo
|
|
}
|
|
else {
|
|
// Aguarda até 5000ms pela notificação de término
|
|
BaseType_t sinal = ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(5000));
|
|
|
|
if (sinal == 0) {
|
|
PrintTela("[CAN Process] ⚠️ Callback demorou demais: " + String(payload->idMsg) + " — timeout ao executar comando: " + frameEmProcessamento);
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(10)); // dá tempo da task morrer sozinha
|
|
eTaskState estado = eTaskGetState(handleTask);
|
|
|
|
if (estado != eDeleted && estado != eInvalid) {
|
|
vTaskDelete(handleTask);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
bool mensagemJaNaFila(QueueHandle_t fila, const twai_message_t& novaMsg) {
|
|
twai_message_t msgTmp;
|
|
UBaseType_t items = uxQueueMessagesWaiting(fila);
|
|
|
|
for (UBaseType_t i = 0; i < items; i++) {
|
|
if (xQueuePeek(fila, &msgTmp, 0) == pdTRUE) {
|
|
if (msgTmp.identifier == novaMsg.identifier && msgTmp.data[0] == novaMsg.data[0] && msgTmp.data[1] == novaMsg.data[1]) {
|
|
return true; // já tem uma igual
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
void setReceiveCallback(OnReceiveCallback cb) {
|
|
_callback = cb;
|
|
}
|
|
|
|
bool adicionarMensagemFila(const std::vector<uint8_t>& payload) {
|
|
if (payload.size() > 8) return false;
|
|
|
|
twai_message_t msg;
|
|
msg.identifier = _nodeId; // + 1;
|
|
msg.extd = 0;
|
|
msg.rtr = 0;
|
|
msg.ss = 0;
|
|
msg.data_length_code = payload.size();
|
|
|
|
for (uint8_t i = 0; i < payload.size(); ++i) {
|
|
msg.data[i] = payload[i]; // 👈 Preenche os dados corretamente
|
|
}
|
|
|
|
if (!mensagemJaNaFila(canQueueTx, msg)) {
|
|
if (xQueueSend(canQueueTx, &msg, 0) != pdTRUE) {
|
|
PrintTela("[CAN] Fila TX cheia! Mensagem descartada.");
|
|
return false;
|
|
}
|
|
else {
|
|
//PrintTela("[CAN] Mensagem adicionada na fila TX");
|
|
return true;
|
|
}
|
|
}
|
|
else {
|
|
PrintTela("[CAN] Mensagem duplicada ignorada.");
|
|
}
|
|
}
|
|
|
|
bool enviarDadosCan(twai_message_t msg) {
|
|
return twai_transmit(&msg, pdMS_TO_TICKS(10)) == ESP_OK;
|
|
}
|
|
|
|
|
|
F_Code FuncaoPorPosicao(CanMessagePosicaoDados posicao) {
|
|
F_Code funcao = F_Code::Nda;
|
|
|
|
int pos = static_cast<int>(posicao);
|
|
|
|
if (pos >= 0 && pos <= 50) {
|
|
funcao = F_Code::ReqTx;
|
|
}
|
|
else if (pos > 50 && pos <= 100) {
|
|
funcao = F_Code::CfgTx;
|
|
}
|
|
else if (pos > 100 && pos <= 150) {
|
|
funcao = F_Code::CmdTx;
|
|
}
|
|
|
|
//PrintTela("Funcao parseada para posicao: " + String(pos) + ", funcao: " + String(funcao));
|
|
|
|
return funcao;
|
|
}
|
|
|
|
std::vector<uint8_t> MontarFrameReqStatusMod(T_Code D_Code, bool Conectado, int Versao) {
|
|
std::vector<uint8_t> data;
|
|
data.push_back(static_cast<uint8_t>(CanMessagePosicaoDados::Status));
|
|
data.push_back(ID_Num_sMOD);
|
|
data.push_back(static_cast<uint8_t>(D_Code));
|
|
data.push_back(Conectado ? 1 : 0);
|
|
data.push_back(Versao);
|
|
return data;
|
|
}
|
|
|
|
std::vector<uint8_t> MontarFrameReqDadosFim(int latencia) {
|
|
std::vector<uint8_t> data;
|
|
data.push_back(static_cast<uint8_t>(CanMessagePosicaoDados::DadosAll));
|
|
data.push_back(ID_Num_sTOD);
|
|
data.push_back(latencia >> 8); data.push_back(latencia & 0xFF);
|
|
return data;
|
|
}
|
|
|
|
|
|
|
|
|
|
static void TaskCallbackWrapper(void* param) {
|
|
auto bundle = static_cast<std::pair<CallbackPayload*, TaskHandle_t>*>(param);
|
|
CallbackPayload* payload = bundle->first;
|
|
TaskHandle_t notifyTo = bundle->second;
|
|
|
|
payload->callback(
|
|
payload->packetSize,
|
|
payload->senderId,
|
|
payload->posicao,
|
|
payload->data,
|
|
payload->dataLength
|
|
);
|
|
|
|
PrintTela("[CAN Callback] Processamento finalizado: " + String(payload->idMsg));
|
|
|
|
// Notifica a task principal que terminou
|
|
xTaskNotifyGive(notifyTo);
|
|
|
|
// Limpa memória
|
|
delete payload;
|
|
delete bundle;
|
|
|
|
// Encerra a task
|
|
vTaskDelete(NULL);
|
|
}
|
|
|
|
private:
|
|
uint8_t _nodeId;
|
|
uint32_t _baudRate;
|
|
OnReceiveCallback _callback = nullptr;
|
|
bool DebugMode = false;
|
|
volatile bool processamentoEmAndamento = false;
|
|
volatile long idmsg = 0;
|
|
|
|
|
|
};
|
|
|
|
TaskHandle_t CanService::handleCallbackTask = NULL;
|
|
|
|
#endif |