677 lines
20 KiB
C++
677 lines
20 KiB
C++
#include "SerialService.h"
|
|
|
|
#ifndef CANSERVICE_H
|
|
#define CANSERVICE_H
|
|
|
|
#include <Arduino.h>
|
|
#include <driver/twai.h>
|
|
#include <esp_task_wdt.h>
|
|
#include <esp_timer.h>
|
|
#include <freertos/FreeRTOS.h>
|
|
#include <freertos/queue.h>
|
|
#include <freertos/semphr.h>
|
|
#include <freertos/task.h>
|
|
#include <vector>
|
|
|
|
#ifndef APP_CPU_NUM
|
|
#define APP_CPU_NUM 1
|
|
#endif
|
|
|
|
class CanService {
|
|
public:
|
|
typedef void (*OnReceiveCallback)(int packetSize,
|
|
int senderId,
|
|
CanMessagePosicaoDados posicao,
|
|
byte* data,
|
|
int dataLength);
|
|
|
|
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 = 250000)
|
|
: _nodeId(nodeId), _baudRate(baudRate) {}
|
|
|
|
void begin() {
|
|
PrintTela("[CAN] Iniciando TWAI robusto...");
|
|
|
|
if (_started) {
|
|
PrintTela("[CAN] begin() ignorado: servico ja iniciado.");
|
|
return;
|
|
}
|
|
|
|
// Criados antes do driver/tasks para que todo acesso compartilhado ja nasca
|
|
// protegido.
|
|
_slotsMutex = xSemaphoreCreateMutex();
|
|
_driverMutex = xSemaphoreCreateMutex();
|
|
callbacksQ = xQueueCreate(CALLBACK_QUEUE_LEN, sizeof(twai_message_t));
|
|
keysQ = xQueueCreate(KEY_QUEUE_LEN, sizeof(uint16_t));
|
|
|
|
if (!_slotsMutex || !_driverMutex || !callbacksQ || !keysQ) {
|
|
PrintTela("[CAN] Falha ao criar mutex/filas.");
|
|
return;
|
|
}
|
|
|
|
if (!installAndStartDriver_()) {
|
|
PrintTela("[CAN] Falha ao instalar/iniciar TWAI.");
|
|
return;
|
|
}
|
|
|
|
BaseType_t okRx = xTaskCreatePinnedToCore(
|
|
CanTaskRxWrapper, "CanTaskRx", 4096, this, 6,
|
|
&CanTaskRxHandle, APP_CPU_NUM);
|
|
BaseType_t okTx = xTaskCreatePinnedToCore(
|
|
CanTaskTxWrapper, "CanTaskTx", 4096, this, 6,
|
|
&CanTaskTxHandle, APP_CPU_NUM);
|
|
BaseType_t okCb = xTaskCreatePinnedToCore(
|
|
CallbackWorkerWrapper, "CallbackWorker", 6144, this, 5,
|
|
&CallbackWorkerHandle, APP_CPU_NUM);
|
|
BaseType_t okHl = xTaskCreatePinnedToCore(
|
|
HealthTaskWrapper, "CanHealth", 4096, this, 2,
|
|
&HealthTaskHandle, APP_CPU_NUM);
|
|
|
|
if (okRx != pdPASS || okTx != pdPASS || okCb != pdPASS || okHl != pdPASS) {
|
|
PrintTela("[CAN] Falha ao criar uma ou mais tasks.");
|
|
return;
|
|
}
|
|
|
|
// Mantem o watchdog de tasks como ultimo cinto de seguranca contra firmware
|
|
// realmente travado. Ele nao e usado para decidir se o barramento esta vivo.
|
|
esp_task_wdt_init(TASK_WDT_SECONDS, true);
|
|
esp_task_wdt_add(CanTaskRxHandle);
|
|
esp_task_wdt_add(CanTaskTxHandle);
|
|
esp_task_wdt_add(CallbackWorkerHandle);
|
|
esp_task_wdt_add(HealthTaskHandle);
|
|
|
|
_started = true;
|
|
PrintTela("[CAN] TWAI robusto iniciado com sucesso.");
|
|
}
|
|
|
|
QueueHandle_t callbacksQ = nullptr;
|
|
QueueHandle_t keysQ = nullptr;
|
|
|
|
TaskHandle_t CanTaskRxHandle = NULL;
|
|
TaskHandle_t CallbackWorkerHandle = NULL;
|
|
TaskHandle_t CanTaskTxHandle = NULL;
|
|
TaskHandle_t HealthTaskHandle = NULL;
|
|
|
|
// Metricas mantidas publicas para compatibilidade/diagnostico.
|
|
volatile uint64_t last_rx_ts = 0;
|
|
volatile uint64_t last_tx_ts = 0;
|
|
volatile uint32_t rx_drops = 0;
|
|
volatile uint32_t tx_retries = 0;
|
|
|
|
// Novas metricas. Sao muito uteis no proximo teste de campo.
|
|
volatile uint32_t tx_queue_full = 0;
|
|
volatile uint32_t tx_requeues = 0;
|
|
volatile uint32_t tx_rescued = 0;
|
|
volatile uint32_t tx_publish_replaced = 0;
|
|
volatile uint32_t tx_table_full = 0;
|
|
volatile uint32_t tx_failures = 0;
|
|
volatile uint32_t driver_reinits = 0;
|
|
volatile uint32_t driver_reinit_failures = 0;
|
|
volatile uint32_t bus_off_events = 0;
|
|
volatile uint32_t rx_queue_full_alerts = 0;
|
|
volatile uint32_t tx_failed_alerts = 0;
|
|
|
|
static const int MAX_KEYS = 64;
|
|
|
|
struct Slot {
|
|
uint16_t key = 0;
|
|
bool used = false;
|
|
bool pending = false; // existe token na keysQ OU TX desta chave esta em voo
|
|
twai_message_t msg{};
|
|
uint32_t gen = 0; // ultima geracao publicada
|
|
uint32_t sentGen = 0; // ultima geracao confirmada em twai_transmit()
|
|
};
|
|
|
|
Slot latestByKey[MAX_KEYS];
|
|
|
|
static inline uint16_t make_key(const twai_message_t& m) {
|
|
return (m.data_length_code >= 2)
|
|
? ((uint16_t(m.data[0]) << 8) | uint16_t(m.data[1]))
|
|
: 0xFFFF;
|
|
}
|
|
|
|
void PublicarTx(const twai_message_t& msg) {
|
|
if (msg.data_length_code < 2 || msg.data_length_code > 8) return;
|
|
if (msg.extd || msg.rtr) return;
|
|
|
|
const uint16_t key = make_key(msg);
|
|
if (key == 0xFFFF || !_slotsMutex) return;
|
|
|
|
if (xSemaphoreTake(_slotsMutex, pdMS_TO_TICKS(20)) != pdTRUE) {
|
|
// Nao bloqueia callback indefinidamente. A disputa normal aqui deve durar
|
|
// microssegundos; timeout indica algo realmente anormal.
|
|
tx_failures++;
|
|
return;
|
|
}
|
|
|
|
const int idx = findOrAllocateSlotLocked_(key);
|
|
if (idx < 0) {
|
|
tx_table_full++;
|
|
xSemaphoreGive(_slotsMutex);
|
|
return;
|
|
}
|
|
|
|
Slot& s = latestByKey[idx];
|
|
if (s.used && s.gen != s.sentGen) {
|
|
// Nao e perda: estamos deliberadamente substituindo uma versao ainda nao
|
|
// enviada pela versao mais nova da mesma (posicao,id_num).
|
|
tx_publish_replaced++;
|
|
}
|
|
|
|
s.used = true;
|
|
s.key = key;
|
|
s.msg = msg;
|
|
s.gen++;
|
|
if (s.gen == 0) {
|
|
// Wrap apos ~4 bilhoes de publicacoes: preserva a propriedade dirty.
|
|
s.gen = 1;
|
|
s.sentGen = 0;
|
|
}
|
|
|
|
if (!s.pending) {
|
|
if (enqueueKeyLocked_(s)) {
|
|
s.pending = true;
|
|
}
|
|
// Se a fila estiver cheia, pending fica false. O scanner de resgate da
|
|
// CanTaskTx vai reenfileirar essa geracao assim que houver espaco.
|
|
}
|
|
|
|
xSemaphoreGive(_slotsMutex);
|
|
}
|
|
|
|
bool adicionarMensagemFila(const std::vector<uint8_t>& payload) {
|
|
if (payload.size() < 2 || payload.size() > 8) {
|
|
PrintTela("[CAN] Payload invalido: esperado 2..8 bytes.");
|
|
return false;
|
|
}
|
|
|
|
twai_message_t msg{};
|
|
msg.identifier = _nodeId;
|
|
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];
|
|
|
|
PublicarTx(msg);
|
|
return true;
|
|
}
|
|
|
|
// Mantida publica por compatibilidade com a versao anterior. Chamadas novas
|
|
// devem preferir adicionarMensagemFila()/PublicarTx() para ganhar coalescencia.
|
|
bool enviarDadosCan(const twai_message_t& msg, TickType_t timeoutTicks) {
|
|
return transmitDriverSafe_(msg, timeoutTicks);
|
|
}
|
|
|
|
void setReceiveCallback(OnReceiveCallback cb) { _callback = cb; }
|
|
void setDebugMode(bool enabled) { DebugMode = enabled; }
|
|
|
|
F_Code FuncaoPorPosicao(CanMessagePosicaoDados posicao) {
|
|
const int pos = static_cast<int>(posicao);
|
|
if (pos >= 0 && pos <= 50) return F_Code::ReqTx;
|
|
if (pos > 50 && pos <= 100) return F_Code::CfgTx;
|
|
if (pos > 100 && pos <= 150) return F_Code::CmdTx;
|
|
return F_Code::Nda;
|
|
}
|
|
|
|
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(static_cast<uint8_t>(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;
|
|
}
|
|
|
|
private:
|
|
static constexpr uint8_t CALLBACK_QUEUE_LEN = 64;
|
|
static constexpr uint8_t KEY_QUEUE_LEN = 64;
|
|
static constexpr uint8_t TASK_WDT_SECONDS = 20;
|
|
static constexpr uint8_t MAX_TX_ATTEMPTS = 3;
|
|
static constexpr uint8_t TX_FAILS_BEFORE_REINIT = 8;
|
|
static constexpr uint32_t HEALTH_PERIOD_MS = 1000;
|
|
static constexpr uint32_t DIAG_PERIOD_MS = 10000;
|
|
|
|
uint32_t _baudRate = 250000;
|
|
uint8_t _nodeId = 0;
|
|
uint8_t _tx_queue_len = 32;
|
|
uint8_t _rx_queue_len = 32;
|
|
OnReceiveCallback _callback = nullptr;
|
|
bool DebugMode = false;
|
|
|
|
SemaphoreHandle_t _slotsMutex = nullptr;
|
|
SemaphoreHandle_t _driverMutex = nullptr;
|
|
|
|
enum class CanState : uint8_t { Down, Running, Recovering };
|
|
volatile CanState _state = CanState::Down;
|
|
volatile bool _driverInstalled = false;
|
|
volatile bool _reinitInProgress = false;
|
|
volatile bool _started = false;
|
|
volatile uint8_t _consecutiveTxFails = 0;
|
|
|
|
static void CanTaskRxWrapper(void* pv) {
|
|
static_cast<CanService*>(pv)->CanTaskRx();
|
|
}
|
|
static void CanTaskTxWrapper(void* pv) {
|
|
static_cast<CanService*>(pv)->CanTaskTx();
|
|
}
|
|
static void CallbackWorkerWrapper(void* pv) {
|
|
static_cast<CanService*>(pv)->CallbackWorker();
|
|
}
|
|
static void HealthTaskWrapper(void* pv) {
|
|
static_cast<CanService*>(pv)->HealthTask();
|
|
}
|
|
|
|
bool timingConfig_(twai_timing_config_t& out) const {
|
|
if (_baudRate == 250000) {
|
|
out = TWAI_TIMING_CONFIG_250KBITS();
|
|
return true;
|
|
}
|
|
if (_baudRate == 500000) {
|
|
out = TWAI_TIMING_CONFIG_500KBITS();
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
uint32_t alertMask_() const {
|
|
uint32_t mask = TWAI_ALERT_BUS_OFF |
|
|
TWAI_ALERT_RX_QUEUE_FULL |
|
|
TWAI_ALERT_TX_FAILED;
|
|
#if defined(TWAI_ALERT_PERIPH_ERR)
|
|
mask |= TWAI_ALERT_PERIPH_ERR;
|
|
#elif defined(TWAI_ALERT_PERIPH_RESET)
|
|
mask |= TWAI_ALERT_PERIPH_RESET;
|
|
#endif
|
|
#if defined(TWAI_ALERT_BUS_RECOVERED)
|
|
mask |= TWAI_ALERT_BUS_RECOVERED;
|
|
#elif defined(TWAI_ALERT_RECOVERY_COMPLETE)
|
|
mask |= TWAI_ALERT_RECOVERY_COMPLETE;
|
|
#endif
|
|
return mask;
|
|
}
|
|
|
|
bool installAndStartDriver_() {
|
|
twai_general_config_t g =
|
|
TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_5, GPIO_NUM_4, TWAI_MODE_NORMAL);
|
|
g.tx_queue_len = _tx_queue_len;
|
|
g.rx_queue_len = _rx_queue_len;
|
|
|
|
twai_timing_config_t t;
|
|
if (!timingConfig_(t)) {
|
|
PrintTela("[CAN] Baudrate nao suportado: " + String(_baudRate));
|
|
return false;
|
|
}
|
|
|
|
// Este filtro recebe somente frames standard destinados ao nodeId. Logo
|
|
// last_rx_ts mede atividade PARA ESTE MODULO, nao atividade global do CAN.
|
|
twai_filter_config_t f = {
|
|
.acceptance_code = (uint32_t(_nodeId) << 21),
|
|
.acceptance_mask = ~(0x7FFu << 21),
|
|
.single_filter = true
|
|
};
|
|
|
|
_state = CanState::Down;
|
|
_driverInstalled = false;
|
|
|
|
esp_err_t err = twai_driver_install(&g, &t, &f);
|
|
if (err != ESP_OK) {
|
|
PrintTela("[CAN] twai_driver_install falhou: " + String((int)err));
|
|
return false;
|
|
}
|
|
|
|
err = twai_start();
|
|
if (err != ESP_OK) {
|
|
PrintTela("[CAN] twai_start falhou: " + String((int)err));
|
|
twai_driver_uninstall();
|
|
return false;
|
|
}
|
|
|
|
twai_reconfigure_alerts(alertMask_(), nullptr);
|
|
_driverInstalled = true;
|
|
_state = CanState::Running;
|
|
return true;
|
|
}
|
|
|
|
bool safeReinitializeDriver_(const char* reason) {
|
|
if (!_driverMutex || _reinitInProgress) return false;
|
|
if (xSemaphoreTake(_driverMutex, pdMS_TO_TICKS(250)) != pdTRUE) return false;
|
|
|
|
if (_reinitInProgress) {
|
|
xSemaphoreGive(_driverMutex);
|
|
return false;
|
|
}
|
|
|
|
_reinitInProgress = true;
|
|
_state = CanState::Recovering;
|
|
_driverInstalled = false;
|
|
|
|
PrintTela("[CAN] Reinit TWAI. Motivo: " + String(reason));
|
|
|
|
// RX/TX usam este mesmo mutex antes de entrar no driver. Portanto, quando
|
|
// chegamos aqui nenhuma outra task esta dentro de twai_receive/transmit.
|
|
twai_stop();
|
|
twai_driver_uninstall();
|
|
vTaskDelay(pdMS_TO_TICKS(5));
|
|
|
|
const bool ok = installAndStartDriver_();
|
|
if (ok) {
|
|
driver_reinits++;
|
|
_consecutiveTxFails = 0;
|
|
last_rx_ts = 0;
|
|
last_tx_ts = 0;
|
|
} else {
|
|
driver_reinit_failures++;
|
|
_state = CanState::Down;
|
|
_driverInstalled = false;
|
|
}
|
|
|
|
_reinitInProgress = false;
|
|
xSemaphoreGive(_driverMutex);
|
|
return ok;
|
|
}
|
|
|
|
int findSlotLocked_(uint16_t key) const {
|
|
int idx = key % MAX_KEYS;
|
|
for (int i = 0; i < MAX_KEYS; ++i, idx = (idx + 1) % MAX_KEYS) {
|
|
const Slot& s = latestByKey[idx];
|
|
if (!s.used) return -1;
|
|
if (s.key == key) return idx;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int findOrAllocateSlotLocked_(uint16_t key) {
|
|
int idx = key % MAX_KEYS;
|
|
for (int i = 0; i < MAX_KEYS; ++i, idx = (idx + 1) % MAX_KEYS) {
|
|
Slot& s = latestByKey[idx];
|
|
if (!s.used || s.key == key) return idx;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
bool enqueueKeyLocked_(Slot& s) {
|
|
if (!keysQ) return false;
|
|
const uint16_t key = s.key;
|
|
if (xQueueSend(keysQ, &key, 0) == pdTRUE) return true;
|
|
tx_queue_full++;
|
|
return false;
|
|
}
|
|
|
|
void rescueUnqueuedSlots_() {
|
|
if (!_slotsMutex || !keysQ) return;
|
|
if (xSemaphoreTake(_slotsMutex, 0) != pdTRUE) return;
|
|
|
|
// Um slot dirty e !pending e exatamente uma publicacao que nao conseguiu
|
|
// entrar na fila (ou um requeue que encontrou a fila cheia).
|
|
for (int i = 0; i < MAX_KEYS; ++i) {
|
|
Slot& s = latestByKey[i];
|
|
if (!s.used || s.pending || s.gen == s.sentGen) continue;
|
|
if (enqueueKeyLocked_(s)) {
|
|
s.pending = true;
|
|
tx_rescued++;
|
|
} else {
|
|
break; // fila cheia; tentamos novamente no proximo ciclo
|
|
}
|
|
}
|
|
|
|
xSemaphoreGive(_slotsMutex);
|
|
}
|
|
|
|
bool transmitDriverSafe_(const twai_message_t& msg, TickType_t timeoutTicks) {
|
|
if (!_driverMutex) return false;
|
|
if (xSemaphoreTake(_driverMutex, pdMS_TO_TICKS(20)) != pdTRUE) return false;
|
|
|
|
bool ok = false;
|
|
if (_driverInstalled && _state == CanState::Running) {
|
|
const esp_err_t err = twai_transmit(&msg, timeoutTicks);
|
|
ok = (err == ESP_OK);
|
|
}
|
|
|
|
xSemaphoreGive(_driverMutex);
|
|
return ok;
|
|
}
|
|
|
|
bool receiveDriverSafe_(twai_message_t& msg, TickType_t timeoutTicks) {
|
|
if (!_driverMutex) return false;
|
|
if (xSemaphoreTake(_driverMutex, pdMS_TO_TICKS(5)) != pdTRUE) return false;
|
|
|
|
bool ok = false;
|
|
if (_driverInstalled && _state == CanState::Running) {
|
|
ok = (twai_receive(&msg, timeoutTicks) == ESP_OK);
|
|
}
|
|
|
|
xSemaphoreGive(_driverMutex);
|
|
return ok;
|
|
}
|
|
|
|
void CanTaskRx() {
|
|
twai_message_t msg{};
|
|
|
|
for (;;) {
|
|
esp_task_wdt_reset();
|
|
|
|
// Timeout curto porque o driverMutex tambem protege TX e lifecycle.
|
|
if (receiveDriverSafe_(msg, pdMS_TO_TICKS(2))) {
|
|
if (msg.data_length_code > 0 && !msg.extd && !msg.rtr) {
|
|
last_rx_ts = esp_timer_get_time();
|
|
|
|
// Nao removemos request antigo para inserir novo: isso podia apagar
|
|
// justamente um DadosAll ainda nao processado. Se lotar, contamos.
|
|
if (callbacksQ &&
|
|
xQueueSend(callbacksQ, &msg, pdMS_TO_TICKS(2)) != pdTRUE) {
|
|
rx_drops++;
|
|
}
|
|
}
|
|
}
|
|
|
|
taskYIELD();
|
|
}
|
|
}
|
|
|
|
void CallbackWorker() {
|
|
twai_message_t msg{};
|
|
|
|
for (;;) {
|
|
esp_task_wdt_reset();
|
|
|
|
if (callbacksQ &&
|
|
xQueueReceive(callbacksQ, &msg, pdMS_TO_TICKS(250)) == pdTRUE) {
|
|
if (_callback && msg.data_length_code > 0) {
|
|
const CanMessagePosicaoDados posicao =
|
|
(CanMessagePosicaoDados)msg.data[0];
|
|
_callback(msg.data_length_code,
|
|
msg.identifier,
|
|
posicao,
|
|
&msg.data[1],
|
|
msg.data_length_code - 1);
|
|
}
|
|
}
|
|
|
|
taskYIELD();
|
|
}
|
|
}
|
|
|
|
void CanTaskTx() {
|
|
uint16_t key = 0;
|
|
|
|
for (;;) {
|
|
esp_task_wdt_reset();
|
|
|
|
if (!keysQ || xQueueReceive(keysQ, &key, pdMS_TO_TICKS(10)) != pdTRUE) {
|
|
rescueUnqueuedSlots_();
|
|
continue;
|
|
}
|
|
|
|
twai_message_t msg{};
|
|
uint32_t genToSend = 0;
|
|
int foundIdx = -1;
|
|
|
|
if (xSemaphoreTake(_slotsMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
|
|
foundIdx = findSlotLocked_(key);
|
|
if (foundIdx >= 0) {
|
|
Slot& s = latestByKey[foundIdx];
|
|
msg = s.msg;
|
|
genToSend = s.gen;
|
|
// pending permanece true enquanto este frame esta em voo. Assim uma
|
|
// publicacao concorrente apenas incrementa gen e nao duplica token.
|
|
}
|
|
xSemaphoreGive(_slotsMutex);
|
|
}
|
|
|
|
if (foundIdx < 0) {
|
|
rescueUnqueuedSlots_();
|
|
continue;
|
|
}
|
|
|
|
msg.extd = 0;
|
|
msg.rtr = 0;
|
|
|
|
bool ok = false;
|
|
for (uint8_t attempt = 0; attempt < MAX_TX_ATTEMPTS; ++attempt) {
|
|
esp_task_wdt_reset();
|
|
if (transmitDriverSafe_(msg, pdMS_TO_TICKS(5))) {
|
|
ok = true;
|
|
last_tx_ts = esp_timer_get_time();
|
|
_consecutiveTxFails = 0;
|
|
break;
|
|
}
|
|
tx_retries++;
|
|
vTaskDelay(pdMS_TO_TICKS(2 + attempt * 3));
|
|
}
|
|
|
|
if (!ok) {
|
|
tx_failures++;
|
|
if (_consecutiveTxFails < 255) _consecutiveTxFails++;
|
|
}
|
|
|
|
// Fecha a janela critica usando a geracao capturada ANTES do TX.
|
|
if (xSemaphoreTake(_slotsMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
|
|
const int idx = findSlotLocked_(key);
|
|
if (idx >= 0) {
|
|
Slot& s = latestByKey[idx];
|
|
|
|
if (ok && genToSend > s.sentGen) s.sentGen = genToSend;
|
|
|
|
const bool newerGeneration = (s.gen != genToSend);
|
|
const bool stillDirty = (s.gen != s.sentGen);
|
|
|
|
if (newerGeneration || stillDirty || !ok) {
|
|
// A chave que acabamos de consumir precisa voltar para keysQ.
|
|
if (enqueueKeyLocked_(s)) {
|
|
s.pending = true;
|
|
tx_requeues++;
|
|
} else {
|
|
s.pending = false; // scanner de resgate assumira daqui
|
|
}
|
|
} else {
|
|
s.pending = false;
|
|
}
|
|
}
|
|
xSemaphoreGive(_slotsMutex);
|
|
}
|
|
|
|
rescueUnqueuedSlots_();
|
|
taskYIELD();
|
|
}
|
|
}
|
|
|
|
void HealthTask() {
|
|
uint64_t lastDiagUs = 0;
|
|
|
|
for (;;) {
|
|
esp_task_wdt_reset();
|
|
|
|
uint32_t alerts = 0;
|
|
bool gotAlerts = false;
|
|
|
|
// twai_read_alerts tambem pertence ao lifecycle protegido.
|
|
if (_driverMutex &&
|
|
xSemaphoreTake(_driverMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
|
|
if (_driverInstalled) {
|
|
gotAlerts = (twai_read_alerts(&alerts, 0) == ESP_OK);
|
|
}
|
|
xSemaphoreGive(_driverMutex);
|
|
}
|
|
|
|
bool mustReinit = false;
|
|
const char* reason = nullptr;
|
|
|
|
if (gotAlerts && alerts) {
|
|
if (alerts & TWAI_ALERT_BUS_OFF) {
|
|
bus_off_events++;
|
|
mustReinit = true;
|
|
reason = "BUS_OFF";
|
|
}
|
|
if (alerts & TWAI_ALERT_RX_QUEUE_FULL) {
|
|
rx_queue_full_alerts++;
|
|
PrintTela("[CAN ALERT] RX interno cheio.");
|
|
}
|
|
if (alerts & TWAI_ALERT_TX_FAILED) {
|
|
tx_failed_alerts++;
|
|
}
|
|
#if defined(TWAI_ALERT_PERIPH_ERR)
|
|
if (alerts & TWAI_ALERT_PERIPH_ERR) {
|
|
mustReinit = true;
|
|
reason = "PERIPH_ERR";
|
|
}
|
|
#elif defined(TWAI_ALERT_PERIPH_RESET)
|
|
if (alerts & TWAI_ALERT_PERIPH_RESET) {
|
|
mustReinit = true;
|
|
reason = "PERIPH_RESET";
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// Nao usamos silencio de RX como motivo de recovery. O filtro so enxerga
|
|
// frames deste nodeId; ficar 5/20 s sem request pode ser perfeitamente
|
|
// legitimo. Falhas TX consecutivas, por outro lado, sao evidencia local.
|
|
if (_consecutiveTxFails >= TX_FAILS_BEFORE_REINIT) {
|
|
mustReinit = true;
|
|
reason = "TX_FAIL_PERSISTENTE";
|
|
}
|
|
|
|
if (mustReinit && !_reinitInProgress) {
|
|
safeReinitializeDriver_(reason ? reason : "ERRO_TWAI");
|
|
}
|
|
|
|
// Telemetria diagnostica periodica, sem flood de Serial.
|
|
const uint64_t now = esp_timer_get_time();
|
|
if (DebugMode &&
|
|
(lastDiagUs == 0 || (now - lastDiagUs) >= uint64_t(DIAG_PERIOD_MS) * 1000ULL)) {
|
|
lastDiagUs = now;
|
|
const UBaseType_t rxWait = callbacksQ ? uxQueueMessagesWaiting(callbacksQ) : 0;
|
|
const UBaseType_t txWait = keysQ ? uxQueueMessagesWaiting(keysQ) : 0;
|
|
PrintTela("[CAN DIAG] rxQ=" + String((uint32_t)rxWait) +
|
|
" txQ=" + String((uint32_t)txWait) +
|
|
" rxDrop=" + String(rx_drops) +
|
|
" txFail=" + String(tx_failures) +
|
|
" qFull=" + String(tx_queue_full) +
|
|
" req=" + String(tx_requeues) +
|
|
" rescue=" + String(tx_rescued) +
|
|
" repl=" + String(tx_publish_replaced) +
|
|
" reinit=" + String(driver_reinits));
|
|
}
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(HEALTH_PERIOD_MS));
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif // CANSERVICE_H
|