canard_class_rain.cpp

Go to the documentation of this file.

 
#include "drivers/module_slave_hal.hpp"
#include "canard_class_rain.hpp"
#include "canard_config.hpp"
#include "bxcan.h"
 
// Callback per ISR Routine HAL STM32
canardClass::CanardRxQueue *__CAN1_RX0_IRQHandler_PTR;
extern "C" void CAN1_RX0_IRQHandler(void) {
    #if (ENABLE_CAN)
    HAL_CAN_IRQHandler(&hcan1);
    #endif
}
 
// ***************** ISR READ RX CAN_BUS, BUFFER RX SETUP ISR, CALLBACK *****************
// Gestita come coda FIFO (In sostituzione interrupt bxCAN non funzionante correttamente)
extern "C" void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    uint8_t testElement = __CAN1_RX0_IRQHandler_PTR->wr_ptr + 1;
    if(testElement >= CAN_RX_QUEUE_CAPACITY) testElement = 0;
    // Leggo il messaggio già pronto per libreria CANARD (Frame) e Inserisco in Buffer RX
    if (bxCANPop(IFACE_CAN_IDX,
        &__CAN1_RX0_IRQHandler_PTR->msg[testElement].frame.extended_can_id,
        &__CAN1_RX0_IRQHandler_PTR->msg[testElement].frame.payload_size,
        __CAN1_RX0_IRQHandler_PTR->msg[testElement].buf)) {
        if(testElement != __CAN1_RX0_IRQHandler_PTR->rd_ptr) {
            // Non posso registrare il dato (MAX_QUEUE) se (testElement == _canard_rx_queue.rd_ptr)
            // raggiunto MAX Buffer. E' più importante non perdere il primo FIFO payload
            // Quindi non aggiungo il dato ma leggo per svuotare il Buffer FIFO
            // altrimenti rientro sempre in Interrupt RX e mando in stallo la CPU senza RX...
            // READ DATA BUFFER MSG ->
            // Get payload from Buffer (possibilie inizializzazione statica fissa)
            // Il Buffer non cambia indirizzo quindi basterebbe un'init statico di frame[x].payload
            __CAN1_RX0_IRQHandler_PTR->msg[testElement].frame.payload =
            __CAN1_RX0_IRQHandler_PTR->msg[testElement].buf;
            // Push data in queue (Next_WR, Data in testElement + 1 Element from RX)
            __CAN1_RX0_IRQHandler_PTR->wr_ptr = testElement;
        }
    }
}
 
// ********************************************************************************
//                 Contructor Init Class Canard, O1Heap Mem Ptr ISR
// ********************************************************************************
 
canardClass::canardClass() {
 
    // CallBack RX Messaggi
    _attach_rx_callback_PTR = NULL;
    _attach_rx_callback = false;
    _attach_param_PTR = NULL;
 
    // Sottoscrizioni
    _rxSubscriptionIdx = 0;
 
    // Reset master ID all'avvio
    _master_id = UINT16_MAX;
 
    // Init Timing
    _lastMicros = micros();
    _currMicros = _lastMicros;
    _syncMicros = _lastMicros;
 
    // Init O1Heap arena base access ram Canard
    _heap = o1heapInit(_heap_arena, sizeof(_heap_arena));
    LOCAL_ASSERT(NULL != _heap);
 
    // Setup CanardIstance, SET Memory function Allocate, Free and set Canard Mem reference
    _canard = canardInit(_memAllocate, _memFree);
    _canard.user_reference = this;
    _canard.node_id = CANARD_NODE_ID_UNSET;
 
    // Interrupt vector CB to Class PTR
    memset(&_canard_rx_queue, 0, sizeof(_canard_rx_queue));
    __CAN1_RX0_IRQHandler_PTR = &_canard_rx_queue;
 
    // Init memory (0) per le sotto strutture dati che necessitano un INIT_MEM_RESET
    memset(&master, 0, sizeof(master));
    memset(&next_transfer_id, 0, sizeof(next_transfer_id));
    memset(&flag, 0, sizeof(flag));
 
    // Inizializzazioni di sicurezza
    master.file.download_end();
 
    // Inizializzazioni locali da module_config.h
    port_id.publisher_module_rain = UINT16_MAX;
    port_id.service_module_rain = UINT16_MAX;
 
    publisher_enabled.module_rain = DEFAULT_PUBLISH_MODULE_DATA;
    publisher_enabled.port_list = DEFAULT_PUBLISH_PORT_LIST;
 
    // Configura il trasporto dal registro standard uavcan. Capacità e CANARD_MTU_MAX
    // I parametri sono fissi e configurabili dal file di configurazione
    for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
        _canard_tx_queues[ifidx] = canardTxInit(CAN_TX_QUEUE_CAPACITY, CANARD_MTU_MAX);
    }
}
 
// ***************************************************************
//       Funzioni Timing sincronizzazione e gestione Canard
// ***************************************************************
 
CanardMicrosecond canardClass::getMicros() {
    // Start Syncro o real_time
    uint32_t ts = micros();
    if(ts > _lastMicros) {
        // Standard micosecond successivo
        _currMicros += (ts - _lastMicros);
    } else if(ts < _lastMicros) {
        // Overflow registro microsecond
        _currMicros += (0xFFFFFFFFul - _lastMicros + 1 + ts);
    }
    // Backup current register micro
    _lastMicros = ts;
    return _currMicros;
}
 
CanardMicrosecond canardClass::getMicros(GetMonotonicTime_Type syncro_type) {
    // Time sincronizzato o da sincrtonizzare
    if(syncro_type == start_syncronization) _syncMicros = getMicros();
    return _syncMicros;
}
 
uint32_t canardClass::getUpTimeSecond(void) {
    return (uint32_t)(_currMicros / MEGA);
}
 
// ***************************************************************************
//   Gestione Coda messaggi in trasmissione (ciclo di svuotamento messaggi)
// ***************************************************************************
 
void canardClass::send(const CanardMicrosecond tx_deadline_usec,
                        const CanardTransferMetadata* const metadata,
                        const size_t payload_size,
                        const void* const payload) {
    for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
        (void)canardTxPush(&_canard_tx_queues[ifidx],
                            &_canard,
                            _syncMicros + tx_deadline_usec,
                            metadata,
                            payload_size,
                            payload);
    }
}
 
void canardClass::sendResponse(const CanardMicrosecond tx_deadline_usec,
                                const CanardTransferMetadata* const request_metadata,
                                const size_t payload_size,
                                const void* const payload) {
    CanardTransferMetadata meta = *request_metadata;
    meta.transfer_kind = CanardTransferKindResponse;
    send(tx_deadline_usec, &meta, payload_size, payload);
}
 
bool canardClass::transmitQueueDataPresent(void) {
    // Transmit pending frames from the prioritized TX queues managed by libcanard.
    for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++)
    {
        CanardTxQueue* const     que = &_canard_tx_queues[ifidx];
        const CanardTxQueueItem* tqi = canardTxPeek(que);  // Find the highest-priority frame.
        if(tqi != NULL) return true;
    }
    return false;
}
 
void canardClass::transmitQueue(void) {
    // Transmit pending frames from the prioritized TX queues managed by libcanard.
    for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++)
    {
        CanardTxQueue* const     que = &_canard_tx_queues[ifidx];
        const CanardTxQueueItem* tqi = canardTxPeek(que);  // Find the highest-priority frame.
        while (tqi != NULL)
        {
            // Delay Microsecond di sicurezza in Send (Migliora sicurezza RX Pacchetti)
            // Da utilizzare con CPU poco performanti in RX o con controllo Polling gestito Canard
            // N.B. Funziona perfettamente con i Nodi ma utilizzando Yakut è comunque necessario un USB/CAN
            // che utilizzi un'interrupt o molto performanete, altrimenti vengono persi pacchetti in TX
            // e questo comporta uina perdità dei messaggi RX in ricezione dal nodo (master/slave corrente)
            #if (CAN_DELAY_US_SEND > 0)
            delayMicroseconds(CAN_DELAY_US_SEND);
            #endif
            // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
            // Otherwise just drop it and move on to the next one.
            if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > _syncMicros)) {
                // Non-blocking write attempt.
                if (bxCANPush(0,
                    _syncMicros,
                    tqi->tx_deadline_usec,
                    tqi->frame.extended_can_id,
                    tqi->frame.payload_size,
                    tqi->frame.payload)) {
                    // Push CAN data
                    _canard.memory_free(&_canard, canardTxPop(que, tqi));
                    tqi = canardTxPeek(que);
                } else  {
                    // Empty Queue
                    break;
                }
            } else {
                // loop continuo per mancato aggiornamento monotonic_time su TIME_OUT
                // grandi quantità di dati trasmesse e raggiunto il TIMEOUT Subscription...
                // Remove frame per blocco in timeout BUG trasmission security !!!
                _canard.memory_free(&_canard, canardTxPop(que, tqi));
                // Test prossimo pacchetto
                tqi = canardTxPeek(que);
            }
        }
    }
}
 
// ***************************************************************
// Gestione Buffer memorizzazione RX BxCAN per Canard metodo FIFO
// ***************************************************************
 
void canardClass::receiveQueueEmpty(void) {
    _canard_rx_queue.wr_ptr=_canard_rx_queue.rd_ptr;
}
 
bool canardClass::receiveQueueDataPresent(void) {
    return _canard_rx_queue.wr_ptr!=_canard_rx_queue.rd_ptr;
}
 
uint8_t canardClass::receiveQueueElement(void) {
    if(_canard_rx_queue.wr_ptr>=_canard_rx_queue.rd_ptr) {
        return _canard_rx_queue.wr_ptr-_canard_rx_queue.rd_ptr;
    } else {
        return _canard_rx_queue.wr_ptr+(CAN_RX_QUEUE_CAPACITY-_canard_rx_queue.rd_ptr);
    }
}
 
uint8_t canardClass::receiveQueueNextElement(uint8_t currElement) {
    if(currElement + 1 < CAN_RX_QUEUE_CAPACITY) return currElement + 1;
    return 0;
}
 
void canardClass::receiveQueue(void) {
    // Leggo l'elemento disponibile in coda BUFFER RX FiFo CanardFrame + Buffer
    uint8_t getElement = receiveQueueNextElement(_canard_rx_queue.rd_ptr);
    _canard_rx_queue.rd_ptr = getElement;
    // Passaggio CanardFrame Buffered alla RxAccept CANARD
    // DeadLine a partire dal realTime assoluto
    const CanardMicrosecond timestamp_usec = getMicros();
    CanardRxTransfer        transfer;
    const int8_t canard_result = canardRxAccept(&_canard, timestamp_usec, &_canard_rx_queue.msg[getElement].frame, IFACE_CAN_IDX, &transfer, NULL);
    if (canard_result > 0) {
        _attach_rx_callback_PTR(*this, &transfer, _attach_param_PTR);
        _canard.memory_free(&_canard, (void*) transfer.payload);
    } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) {
        (void) 0;  // The frame did not complete a transfer so there is nothing to do.
        // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API.
    } else {
        LOCAL_ASSERT(false);  // No other error can possibly occur at runtime.
    }
}
 
void canardClass::receiveQueue(char *logMessage) {
    // Leggo l'elemento disponibile in coda BUFFER RX FiFo CanardFrame + Buffer
    uint8_t getElement = receiveQueueNextElement(_canard_rx_queue.rd_ptr);
    _canard_rx_queue.rd_ptr = getElement;
    // *****************************************************************************
    logMessage = itoa(_canard_rx_queue.msg[getElement].frame.payload_size, logMessage, 10);
    logMessage += 1;
    strcpy(logMessage, ",Val:");
    logMessage += 5;
    for(int iIdxPl=0; iIdxPl<_canard_rx_queue.msg[getElement].frame.payload_size; iIdxPl++) {
        strcpy(logMessage, " 0x");
        logMessage += 3;
        if(_canard_rx_queue.msg[getElement].buf[iIdxPl] < 16) {
            strcpy(logMessage, "0");
            logMessage++;
            strcpy(logMessage, itoa(_canard_rx_queue.msg[getElement].buf[iIdxPl], logMessage, HEX));
            logMessage++;
        } else {
            strcpy(logMessage, itoa(_canard_rx_queue.msg[getElement].buf[iIdxPl], logMessage, HEX));
            logMessage+=2;
        }
    }
    *logMessage = 0;
    // *****************************************************************************
    // Passaggio CanardFrame Buffered alla RxAccept CANARD
    // DeadLine a partire dal realTime assoluto
    const CanardMicrosecond timestamp_usec = getMicros();
    CanardRxTransfer        transfer;
    const int8_t canard_result = canardRxAccept(&_canard, timestamp_usec, &_canard_rx_queue.msg[getElement].frame, IFACE_CAN_IDX, &transfer, NULL);
    if (canard_result > 0) {
        _attach_rx_callback_PTR(*this, &transfer, _attach_param_PTR);
        _canard.memory_free(&_canard, (void*) transfer.payload);
    } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) {
        (void) 0;  // The frame did not complete a transfer so there is nothing to do.
        // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API.
    } else {
        LOCAL_ASSERT(false);  // No other error can possibly occur at runtime.
    }
}
 
void canardClass::setReceiveMessage_CB(void (*ptrFunction) (canardClass&, const CanardRxTransfer*, void *param), void *param) {
    _attach_rx_callback_PTR = ptrFunction;
    _attach_rx_callback = true;
    _attach_param_PTR = param;
}
 
void canardClass::enableReceiveMessage_CB(void) {
    _attach_rx_callback = true;
}
 
void canardClass::disableReceiveMessage_CB(void) {
    _attach_rx_callback = false;
}
 
// ***************************************************************************
//                 Gestione sottoscrizioni messaggi Canard
// ***************************************************************************
 
bool canardClass::rxSubscribe(
        const CanardTransferKind    transfer_kind,
        const CanardPortID          port_id,
        const size_t                extent,
        const CanardMicrosecond     transfer_id_timeout_usec) {
    // Controllo limiti di sottoscrizioni
    if(_rxSubscriptionIdx >= MAX_SUBSCRIPTION) return false;
    // Aggiunge rxSubscription con spazio RAM interna alla classe
    const int8_t res = canardRxSubscribe(&_canard,
        transfer_kind,
        port_id,
        extent,
        transfer_id_timeout_usec,
        &_rxSubscription[_rxSubscriptionIdx++]);
    return (res>=0);
}
 
uint8_t canardClass::rxSubscriptionAvaiable() {
    return MAX_SUBSCRIPTION - _rxSubscriptionIdx;
}
 
void canardClass::rxUnSubscribe(
        const CanardTransferKind    transfer_kind,
        const CanardPortID          port_id) {
    (void)canardRxUnsubscribe(&_canard, transfer_kind, port_id);
}
 
// *************************************************************************************
//  GESTIONE TIME OUT E SERVIZI RETRY ED ALTRE UTILITY FUNZIONI DI CLASSE MASTER REMOTA
// *************************************************************************************
 
bool canardClass::master_file_read_block_pending(uint32_t timeout_us)
{
    // Accedo alla sezione master con il controllo pending locale
    master.file.start_pending(timeout_us);
    // Invio la richiesta standard
    return send_file_read_block(master.file.get_server_node(), master.file.get_name(),
        master.file.get_offset_rx());
}
 
bool canardClass::send_file_read_block(CanardNodeID server_id, char * fileName, uint64_t remoteOffset)
{
    // FileRead V1.1 Handle Message
    // ***** Ricezione di file generico dalla rete UAVCAN dal nodo chiamante *****
    // Richiamo in continuazione rapida la funzione fino al riempimento del file
    // Alla fine processo il firmware Upload (eventuale) vero e proprio con i relativi check
    uavcan_file_Read_Request_1_1 remotefile = {0};
    remotefile.path.path.count = strlen(fileName);
    memcpy(remotefile.path.path.elements, fileName, remotefile.path.path.count);
    remotefile.offset = remoteOffset;
 
    uint8_t      serialized[uavcan_file_Read_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0};
    size_t       serialized_size                                                           = sizeof(serialized);
    const int8_t err = uavcan_file_Read_Request_1_1_serialize_(&remotefile, &serialized[0], &serialized_size);
    LOCAL_ASSERT(err >= 0);
    if (err >= 0)
    {
        const CanardTransferMetadata meta = {
            .priority       = CanardPriorityHigh,
            .transfer_kind  = CanardTransferKindRequest,
            .port_id        = uavcan_file_Read_1_1_FIXED_PORT_ID_,
            .remote_node_id = server_id,
            .transfer_id    = (CanardTransferID) (next_transfer_id.uavcan_file_read_data()),
        };
        // Messaggio standard ad un secondo dal timeStamp Sincronizzato
        send(MEGA, &meta, serialized_size, &serialized[0]);
        return true;
    }
    return false;
}
 
// *******************************************************************************
//  GESTIONE TIME OUT E SERVIZI RETRY ED ALTRE UTILITY FUNZIONI DI CLASSE MASTER
// *******************************************************************************
 
// ******************************** HEART BEAT ***********************************
 
// ******************************** HEART BEAT ***********************************
 
bool canardClass::master::heartbeat::is_online(bool is_heart_syncronized) {
    // ? Out of TIME Max (Sempre false)
    if (_syncMicros < _timeout_us) {
        // Richiedo Internal HeartBeat Area check ? (Possibile syncro HeartBeat Locale...)
        // Altrimenti la richiesta ha già esito positivo
        if(is_heart_syncronized) {
            // ? Siamo all'interno del tempo di HeartBeat Max (x1.5)
            if ((_syncMicros - _mastMicros) < MEGA * (TIME_PUBLISH_HEARTBEAT * 1.5)) {
                // Reset (Wait Next Syncro Master...)
                _mastMicros = _syncMicros;
                return true;
            }
            else return false;
        }        
        else return true;
    }
    else return false;
}
 
void canardClass::master::heartbeat::set_online(uint32_t dead_line_us) {
    _timeout_us = _syncMicros + dead_line_us;
    _mastMicros = _syncMicros;
}
 
CanardMicrosecond canardClass::master::heartbeat::last_online(void) {
    return _mastMicros;
}
 
// ******************************** TIME STAMP ***********************************
 
bool canardClass::master::timestamp::check_valid_syncronization(uint8_t current_transfer_id,
                                            CanardMicrosecond previous_tx_timestamp_us) {
    // Controllo coerenza messaggio per consentire l'aggiornamento timestamp
    // 1) Sequenza di transfer_id con previous
    // 2) differenza di tempo tra due timestamp remoti inferiore al massimo timeOut impostato di controllo
    if((++_previous_transfer_id == current_transfer_id) &&
        ((previous_tx_timestamp_us - _previous_msg_monotonic_us) < MASTER_MAXSYNCRO_VALID_US))
        // Riferimento locale
        return true;
    // Risincronizzo il transferID Corretto per sicurezza
    _previous_transfer_id = current_transfer_id;
    return false;
}
 
CanardMicrosecond canardClass::master::timestamp::get_timestamp_syncronized(CanardMicrosecond current_rx_timestamp_us,
                                            CanardMicrosecond previous_tx_timestamp_us) {
    // Save timestamp variabili per SET e controllo Time nella successiva chiamata time_syncronization
    // Local RealTime RX timestamp monotonic locale al tempo reale di rx_message (transfer->timestamp_usec)
    _syncronized_timestamp_us = previous_tx_timestamp_us + current_rx_timestamp_us - _previous_local_timestamp_us;
    // Aggiusto e sommo la differenza tra il timestamp reale del messaggio ricevuto con il monotonic reale attuale (al tempo di esecuzione syncro real)
    _previous_syncronized_timestamp_us = getMicros();
    _syncronized_timestamp_us += (_previous_syncronized_timestamp_us - current_rx_timestamp_us);
    return _syncronized_timestamp_us;
}
 
CanardMicrosecond canardClass::master::timestamp::get_timestamp_syncronized(void) {
    CanardMicrosecond realtime_us = getMicros();
    CanardMicrosecond diff_synconized_us = realtime_us - _previous_syncronized_timestamp_us;
    _previous_syncronized_timestamp_us = realtime_us;
    _syncronized_timestamp_us += diff_synconized_us;
    return _syncronized_timestamp_us;
}
 
CanardMicrosecond canardClass::master::timestamp::update_timestamp_message(CanardMicrosecond current_rx_timestamp_us,
                                CanardMicrosecond previous_tx_timestamp_us) {
    // Differenza tra due messaggi in microsecondi
    CanardMicrosecond difference_timestamp_us = previous_tx_timestamp_us - _previous_msg_monotonic_us;
    // Save timestamp variabili per SET e controllo Time nella successiva chiamata time_syncronization
    // Local RealTime RX timestamp monotonic locale al tempo reale di rx_message (transfer->timestamp_usec)
    _previous_local_timestamp_us = current_rx_timestamp_us;
    // RealTime callBack message RX (TX RTC uSec(Canard type) Master Remoto al tempo di send)
    _previous_msg_monotonic_us = previous_tx_timestamp_us;
    return difference_timestamp_us;
}
 
// ******************************** FILE CLIENT ***********************************
 
void canardClass::master::file::start_request(uint8_t remote_node, uint8_t *param_request_name,
                                    uint8_t param_request_len, bool is_firmware) {
    _node_id = remote_node;
    // Copio la stringa nel name file firmware disponibile su state generale (per download successivo)
    memcpy(_filename, param_request_name, param_request_len);
    _filename[param_request_len] = '\0';
    // Init varaiabili di download
    _is_firmware = is_firmware;
    _updating = true;
    _updating_eof = false;
    // Start Offset File
    _offset = 0;
    _updating_retry = 0;
    // Set first pending
    _timeout_us = _syncMicros + NODE_GETFILE_TIMEOUT_US;
}
 
char* canardClass::master::file::get_name(void) {
    return _filename;
}
 
CanardNodeID canardClass::master::file::get_server_node(void) {
    return _node_id;
}
 
bool canardClass::master::file::download_request(void) {
    return _updating;
}
 
bool canardClass::master::file::is_download_complete(void) {
    return _updating_eof;
}
 
void canardClass::master::file::download_end(void) {
    _updating = false;
    _node_id = CANARD_NODE_ID_UNSET;
}
 
bool canardClass::master::file::is_firmware(void)
{
    // Riferimento locale
    return _is_firmware;
}
 
uint64_t canardClass::master::file::get_offset_rx(void) {
    return _offset;
}
 
void canardClass::master::file::set_offset_rx(uint64_t remote_file_offset) {
    _offset = remote_file_offset;
}
 
bool canardClass::master::file::is_first_data_block(void) {
    return _offset == 0;
}
 
bool canardClass::master::file::next_retry(void) {
    if (++_updating_retry > NODE_GETFILE_MAX_RETRY) _updating_retry = NODE_GETFILE_MAX_RETRY;
    // Reset pending state
    _is_pending = false;
    return _updating_retry < NODE_GETFILE_MAX_RETRY;
}
 
bool canardClass::master::file::next_retry(uint8_t *avaiable_retry) {
    if (++_updating_retry > NODE_GETFILE_MAX_RETRY) _updating_retry = NODE_GETFILE_MAX_RETRY;
    *avaiable_retry = NODE_GETFILE_MAX_RETRY - _updating_retry;
    // Reset pending state
    _is_pending = false;
    return _updating_retry < NODE_GETFILE_MAX_RETRY;
}
 
void canardClass::master::file::start_pending(uint32_t timeout_us)
{
    // Riferimento locale
    _is_pending = true;
    _timeout_us = _syncMicros + timeout_us;
}
 
void canardClass::master::file::reset_pending(void)
{
    _is_pending = false;
    // Azzero contestualmente le retry di controllo x gestione MAX_RETRY -> ABORT
    _updating_retry = 0;
}
 
void canardClass::master::file::reset_pending(size_t message_len)
{
    _is_pending = false;
    // Azzero contestualmente le retry di controllo x gestione MAX_RETRY -> ABORT
    _updating_retry = 0;
    // Gestisco internamente l'offset di RX File
    _offset += message_len;
    // Overload per controllo EOF (se lunghezza messaggio != MAX_ARRAY_UAVCAN ... 2\6 Bytes)
    _updating_eof = (message_len != uavcan_primitive_Unstructured_1_0_value_ARRAY_CAPACITY_);
}
 
bool canardClass::master::file::event_timeout(void)
{
    if(_is_pending) return _syncMicros > _timeout_us;
    return false;
}
 
bool canardClass::master::file::is_pending(void)
{
    return _is_pending;
}
 
// ***********************************************************************************
//  GESTIONE TIME OUT E SERVIZI RETRY ED ALTRE UTILITY FUNZIONI DI CLASSE LOCALE SLAVE
// ***********************************************************************************
 
bool canardClass::slave_heartbeat_send_message(void)
{
    // ***** Trasmette alla rete UAVCAN lo stato haeartbeat del modulo *****
    // Heartbeat Fisso anche per modulo Master (Visibile a yakut o altri tools/script gestionali)
    uavcan_node_Heartbeat_1_0 heartbeat = {0};
    heartbeat.uptime = getUpTimeSecond();
    const O1HeapDiagnostics heap_diag = _memGetDiagnostics();
    if (heap_diag.oom_count > 0) {
        heartbeat.health.value = uavcan_node_Health_1_0_CAUTION;
    } else {
        heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
    }
    // Stato di heartbeat gestito dalla classe
    heartbeat.vendor_specific_status_code = flag.get_local_value_heartbeat_VSC();
    heartbeat.mode.value = flag.get_local_node_mode();
    // Trasmetto il pacchetto
    uint8_t serialized[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0};
    size_t serialized_size = sizeof(serialized);
    const int8_t err = uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, &serialized[0], &serialized_size);
    LOCAL_ASSERT(err >= 0);
    if (err >= 0) {
        const CanardTransferMetadata meta = {
            .priority = CanardPriorityNominal,
            .transfer_kind = CanardTransferKindMessage,
            .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
            .remote_node_id = CANARD_NODE_ID_UNSET,
            .transfer_id = (CanardTransferID)(next_transfer_id.uavcan_node_heartbeat()),
        };
        // Messaggio standard ad un secondo dal timeStamp Sincronizzato
        send(MEGA, &meta, serialized_size, &serialized[0]);
        return true;
    }
    return false;
}
 
bool canardClass::slave_pnp_send_request(uint64_t serial_number) {
    // PnP over Classic CAN, use message v1.0.
    uavcan_pnp_NodeIDAllocationData_1_0 msg = {0};
    // truncated uint48 unique_id_hash
    // msg.allocated_node_id.(count/element) => Solo in response non in request;
    msg.unique_id_hash = serial_number >> HASH_EXCLUDING_BIT;
    msg.unique_id_hash &= HASH_SERNUMB_MASK;
    msg.unique_id_hash |= MODULE_TYPE;
    uint8_t serialized[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0};
    size_t serialized_size = sizeof(serialized);
    const int8_t err = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, &serialized[0], &serialized_size);
    LOCAL_ASSERT(err >= 0);
    if (err >= 0) {
        const CanardTransferMetadata meta = {
            .priority = CanardPrioritySlow,
            .transfer_kind = CanardTransferKindMessage,
            .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
            .remote_node_id = CANARD_NODE_ID_UNSET,
            .transfer_id = (CanardTransferID) (next_transfer_id.uavcan_pnp_allocation()),
        };
        // Messaggio standard ad un secondo dal timeStamp Sincronizzato
        send(MEGA, &meta, serialized_size, &serialized[0]);
        return true;
    }
    return false;
}
 
void canardClass::_fillSubscriptions(const CanardTreeNode* const tree, uavcan_node_port_SubjectIDList_0_1* const obj)
{
    if (NULL != tree) {
        _fillSubscriptions(tree->lr[0], obj);
        const CanardRxSubscription* crs = (const CanardRxSubscription*)tree;
        if (crs->port_id <= CANARD_SUBJECT_ID_MAX) {
            LOCAL_ASSERT(obj->sparse_list.count < uavcan_node_port_SubjectIDList_0_1_sparse_list_ARRAY_CAPACITY_);
            obj->sparse_list.elements[obj->sparse_list.count++].value = crs->port_id;
            _fillSubscriptions(tree->lr[1], obj);
        }
    }
}
 
void canardClass::_fillServers(const CanardTreeNode* const tree, uavcan_node_port_ServiceIDList_0_1* const obj)
{
    if (NULL != tree) {
        _fillServers(tree->lr[0], obj);
        const CanardRxSubscription* crs = (const CanardRxSubscription*)tree;
        if (crs->port_id <= CANARD_SERVICE_ID_MAX) {
            (void)nunavutSetBit(&obj->mask_bitpacked_[0], sizeof(obj->mask_bitpacked_), crs->port_id, true);
            _fillServers(tree->lr[1], obj);
        }
    }
}
 
bool canardClass::slave_servicelist_send_message(void)
{
    // Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous.
    // The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU.
    // L'abilitazione del comando è facoltativa, può essere attivata/disattivata da un comando UAVCAN
    if ((publisher_enabled.port_list) &&
        (_canard.node_id <= CANARD_NODE_ID_MAX))
    {
        uavcan_node_port_List_0_1 m = {0};
        uavcan_node_port_List_0_1_initialize_(&m);
        uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.publishers);
        uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.subscribers);
 
        // Indicate which subjects we publish to. Don't forget to keep this updated if you add new publications!
        {
            size_t* const cnt = &m.publishers.sparse_list.count;
            m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_;
            m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_port_List_0_1_FIXED_PORT_ID_;
            // Aggiungo i publisher interni validi privati (quando abilitati)
            if ((port_id.publisher_module_rain <= CANARD_SUBJECT_ID_MAX)&&
                (publisher_enabled.module_rain))
            {
                m.publishers.sparse_list.elements[(*cnt)++].value = port_id.publisher_module_rain;
            }
        }
 
        // Indicate which servers and subscribers we implement.
        // We could construct the list manually but it's easier and more robust to just query libcanard for that.
        _fillSubscriptions(_canard.rx_subscriptions[CanardTransferKindMessage], &m.subscribers);
        _fillServers(_canard.rx_subscriptions[CanardTransferKindRequest], &m.servers);
        _fillServers(_canard.rx_subscriptions[CanardTransferKindResponse], &m.clients);  // For regularity.
 
        // Serialize and publish the message. Use a small buffer because we know that our message is always small.
        // Verificato massimo utilizzo a 156 bytes. Limitiamo il buffer a 256 Bytes (Come esempio UAVCAN)
        uint8_t serialized[256] = {0};
        size_t  serialized_size = uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
        if (uavcan_node_port_List_0_1_serialize_(&m, &serialized[0], &serialized_size) >= 0)
        {
            const CanardTransferMetadata meta = {
                .priority       = CanardPriorityOptional,  // Mind the priority.
                .transfer_kind  = CanardTransferKindMessage,
                .port_id        = uavcan_node_port_List_0_1_FIXED_PORT_ID_,
                .remote_node_id = CANARD_NODE_ID_UNSET,
                .transfer_id    = (CanardTransferID) (next_transfer_id.uavcan_node_port_list()),
            };
            // Send a 2 secondi
            send(MEGA * 2, &meta, serialized_size, &serialized[0]);
            return true;
        }
    }
    return false;
}
 
// ***************************************************************
//              ID di Trasferimento UAVCAN Canard
// ***************************************************************
 
uint8_t canardClass::next_transfer_id::uavcan_node_heartbeat(void) {
    return next_transfer_id::_uavcan_node_heartbeat++;
}
 
uint8_t canardClass::next_transfer_id::uavcan_node_port_list(void) {
    return next_transfer_id::_uavcan_node_port_list++;
}
 
uint8_t canardClass::next_transfer_id::uavcan_pnp_allocation(void) {
    return next_transfer_id::_uavcan_pnp_allocation++;
}
 
uint8_t canardClass::next_transfer_id::uavcan_file_read_data(void) {
    return next_transfer_id::_uavcan_file_read_data++;
}
 
uint8_t canardClass::next_transfer_id::module_rain(void) {
    return next_transfer_id::_module_rain++;
}
 
// ***************************************************************
//           Funzioni ed Utility Canard su Local Flag
// ***************************************************************
 
// ********       COMMAND REMOTI E Stati gestionali       ********
 
void canardClass::flag::request_system_restart(void) {
    _restart_required = true;
}
 
bool canardClass::flag::is_requested_system_restart(void) {
    return _restart_required;
}
 
void canardClass::flag::request_sleep(void) {
    // Con inibizione, non permetto lo sleep del modulo
    if(_inibith_sleep) return;
    // Eseguo la procedura di messa in sleep del modulo
    // Variabili, HW CAN ecc... alla fine setto la var di entrata in sleep
    _enter_sleep = true;
}
 
bool canardClass::flag::is_module_sleep(void) {
    return _enter_sleep;
}
 
void canardClass::flag::disable_sleep(void) {
    _inibith_sleep = true;
}
 
void canardClass::flag::enable_sleep(void) {
    _inibith_sleep = false;
}
 
// ********         HEARTBEAT Vendor Status Code          ********
// Set VendorStatusCode per Heratbeat in RX/TX al modulo Master/Slave
 
void canardClass::flag::set_local_power_mode(Power_Mode powerMode) {
    _heartLocalVSC.powerMode = powerMode;
}
 
void canardClass::flag::set_local_fw_uploading(bool fwUploading) {
    _heartLocalVSC.fwUploading = fwUploading;
}
 
void canardClass::flag::set_local_data_ready(bool dataReady) {
    _heartLocalVSC.dataReady = dataReady;
}
 
void canardClass::flag::set_local_module_ready(bool moduleReady) {
    _heartLocalVSC.moduleReady = moduleReady;
}
 
void canardClass::flag::set_local_module_error(bool moduleError) {
    _heartLocalVSC.moduleError = moduleError;
}
 
Power_Mode canardClass::flag::get_local_power_mode(void) {
    return _heartLocalVSC.powerMode;
}
 
bool canardClass::flag::get_local_fw_uploading(void) {
    return _heartLocalVSC.fwUploading;
}
 
bool canardClass::flag::get_local_data_ready(void) {
    return _heartLocalVSC.dataReady;
}
 
bool canardClass::flag::get_local_module_ready(void) {
    return _heartLocalVSC.moduleReady;
}
 
bool canardClass::flag::get_local_module_error(void) {
    return _heartLocalVSC.moduleError;
}
 
uint8_t canardClass::flag::get_local_value_heartbeat_VSC(void) {
    return _heartLocalVSC.uint8_val;
}
 
// Funzioni Locali per gestione interna NODE MODE x HeartBeat standard UAVCAN
 
void canardClass::flag::set_local_node_mode(uint8_t heartLocalMODE) {
    _heartLocalMODE = heartLocalMODE;
}
 
uint8_t canardClass::flag::get_local_node_mode(void) {
    return _heartLocalMODE;
}
 
// ***************************************************************
//           Funzioni ed Utility di Classe Generali
// ***************************************************************
 
void canardClass::set_canard_node_id(CanardNodeID local_id) {
    // Istanza del modulo canard
    _canard.node_id = local_id;
}
 
CanardNodeID canardClass::get_canard_node_id(void) {
    // Istanza del modulo canard
    return _canard.node_id;
}
 
bool canardClass::is_canard_node_anonymous(void) {
    // Istanza del modulo canard
    return _canard.node_id > CANARD_NODE_ID_MAX;
}
 
void canardClass::set_canard_master_id(CanardNodeID remote_id) {
    // Istanza del modulo canard
    _master_id = remote_id;
}
 
CanardNodeID canardClass::get_canard_master_id(void) {
    // Istanza del modulo canard
    return _master_id;
}
 
// ***************************************************************
//     Wrapper C++ O1Heap memory Access & Function CB Private
// ***************************************************************
 
void* canardClass::_memAllocate(CanardInstance* const ins, const size_t amount) {
    O1HeapInstance* const heap = ((canardClass*)ins->user_reference)->_heap;
    LOCAL_ASSERT(o1heapDoInvariantsHold(heap));
    return o1heapAllocate(heap, amount);
}
 
void canardClass::_memFree(CanardInstance* const ins, void* const pointer) {
    O1HeapInstance* const heap = ((canardClass*)ins->user_reference)->_heap;
    o1heapFree(heap, pointer);
}
 
O1HeapDiagnostics canardClass::_memGetDiagnostics(void) {
    return o1heapGetDiagnostics(_heap);
}