ardupilot/libraries/AP_DroneCAN/AP_Canard_iface.cpp

#include "AP_Canard_iface.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_CANManager/AP_CANManager.h>
#if HAL_ENABLE_DRONECAN_DRIVERS
#include <canard/handler_list.h>
#include <canard/transfer_object.h>
#include <AP_Math/AP_Math.h>
#include <dronecan_msgs.h>
extern const AP_HAL::HAL& hal;
#define LOG_TAG "DroneCANIface"
#include <canard.h>
#include <AP_CANManager/AP_CANSensor.h>

#define DEBUG_PKTS 0

#define CANARD_MSG_TYPE_FROM_ID(x)                         ((uint16_t)(((x) >> 8U)  & 0xFFFFU))

DEFINE_HANDLER_LIST_HEADS();
DEFINE_HANDLER_LIST_SEMAPHORES();

DEFINE_TRANSFER_OBJECT_HEADS();
DEFINE_TRANSFER_OBJECT_SEMAPHORES();

#if AP_TEST_DRONECAN_DRIVERS
CanardInterface* CanardInterface::canard_ifaces[] = {nullptr, nullptr, nullptr};
CanardInterface CanardInterface::test_iface{2};
uint8_t test_node_mem_area[1024];
HAL_Semaphore test_iface_sem;
#endif

void canard_allocate_sem_take(CanardPoolAllocator *allocator) {
    if (allocator->semaphore == nullptr) {
        allocator->semaphore = NEW_NOTHROW HAL_Semaphore;
        if (allocator->semaphore == nullptr) {
            // out of memory
            CANARD_ASSERT(0);
            return;
        }
    }
    ((HAL_Semaphore*)allocator->semaphore)->take_blocking();
}

void canard_allocate_sem_give(CanardPoolAllocator *allocator) {
    if (allocator->semaphore == nullptr) {
        // it should have been allocated by canard_allocate_sem_take
        CANARD_ASSERT(0);
        return;
    }
    ((HAL_Semaphore*)allocator->semaphore)->give();
}

CanardInterface::CanardInterface(uint8_t iface_index) :
Interface(iface_index) {
#if AP_TEST_DRONECAN_DRIVERS
    if (iface_index < 3) {
        canard_ifaces[iface_index] = this;
    }
    if (iface_index == 0) {
        test_iface.init(test_node_mem_area, sizeof(test_node_mem_area), 125);
    }
    canardInitTxTransfer(&tx_transfer);
#endif
}

void CanardInterface::init(void* mem_arena, size_t mem_arena_size, uint8_t node_id) {
    canardInit(&canard, mem_arena, mem_arena_size, onTransferReception, shouldAcceptTransfer, this);
    canardSetLocalNodeID(&canard, node_id);
    initialized = true;
}

bool CanardInterface::broadcast(const Canard::Transfer &bcast_transfer) {
    if (!initialized) {
        return false;
    }
    WITH_SEMAPHORE(_sem_tx);

#if AP_TEST_DRONECAN_DRIVERS
    if (this == &test_iface) {
        test_iface_sem.take_blocking();
    }
#endif

    tx_transfer = {
        .transfer_type = bcast_transfer.transfer_type,
        .data_type_signature = bcast_transfer.data_type_signature,
        .data_type_id = bcast_transfer.data_type_id,
        .inout_transfer_id = bcast_transfer.inout_transfer_id,
        .priority = bcast_transfer.priority,
        .payload = (const uint8_t*)bcast_transfer.payload,
        .payload_len = uint16_t(bcast_transfer.payload_len),
#if CANARD_ENABLE_CANFD
        .canfd = bcast_transfer.canfd,
#endif
        .deadline_usec = AP_HAL::micros64() + (bcast_transfer.timeout_ms * 1000),
#if CANARD_MULTI_IFACE
        .iface_mask = uint8_t((1<<num_ifaces) - 1),
#endif
    };
    // do canard broadcast
    int16_t ret = canardBroadcastObj(&canard, &tx_transfer);
#if AP_TEST_DRONECAN_DRIVERS
    if (this == &test_iface) {
        test_iface_sem.give();
    }
#endif
    if (ret <= 0) {
        protocol_stats.tx_errors++;
    } else {
        protocol_stats.tx_frames += ret;
    }
    return ret > 0;
}

bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) {
    if (!initialized) {
        return false;
    }
    WITH_SEMAPHORE(_sem_tx);

    tx_transfer = {
        .transfer_type = req_transfer.transfer_type,
        .data_type_signature = req_transfer.data_type_signature,
        .data_type_id = req_transfer.data_type_id,
        .inout_transfer_id = req_transfer.inout_transfer_id,
        .priority = req_transfer.priority,
        .payload = (const uint8_t*)req_transfer.payload,
        .payload_len = uint16_t(req_transfer.payload_len),
#if CANARD_ENABLE_CANFD
        .canfd = req_transfer.canfd,
#endif
        .deadline_usec = AP_HAL::micros64() + (req_transfer.timeout_ms * 1000),
#if CANARD_MULTI_IFACE
        .iface_mask = uint8_t((1<<num_ifaces) - 1),
#endif
    };
    // do canard request
    int16_t ret = canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer);
    if (ret <= 0) {
        protocol_stats.tx_errors++;
    } else {
        protocol_stats.tx_frames += ret;
    }
    return ret > 0;
}

bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) {
    if (!initialized) {
        return false;
    }
    WITH_SEMAPHORE(_sem_tx);

    tx_transfer = {
        .transfer_type = res_transfer.transfer_type,
        .data_type_signature = res_transfer.data_type_signature,
        .data_type_id = res_transfer.data_type_id,
        .inout_transfer_id = res_transfer.inout_transfer_id,
        .priority = res_transfer.priority,
        .payload = (const uint8_t*)res_transfer.payload,
        .payload_len = uint16_t(res_transfer.payload_len),
#if CANARD_ENABLE_CANFD
        .canfd = res_transfer.canfd,
#endif
        .deadline_usec = AP_HAL::micros64() + (res_transfer.timeout_ms * 1000),
#if CANARD_MULTI_IFACE
        .iface_mask = uint8_t((1<<num_ifaces) - 1),
#endif
    };
    // do canard respond
    int16_t ret = canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer);
    if (ret <= 0) {
        protocol_stats.tx_errors++;
    } else {
        protocol_stats.tx_frames += ret;
    }
    return ret > 0;
}

void CanardInterface::onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer) {
    CanardInterface* iface = (CanardInterface*) ins->user_reference;
    iface->handle_message(*transfer);
}

bool CanardInterface::shouldAcceptTransfer(const CanardInstance* ins,
                                           uint64_t* out_data_type_signature,
                                           uint16_t data_type_id,
                                           CanardTransferType transfer_type,
                                           uint8_t source_node_id) {
    CanardInterface* iface = (CanardInterface*) ins->user_reference;
    return iface->accept_message(data_type_id, *out_data_type_signature);
}

#if AP_TEST_DRONECAN_DRIVERS
void CanardInterface::processTestRx() {
    if (!test_iface.initialized) {
        return;
    }
    WITH_SEMAPHORE(test_iface_sem);
    for (const CanardCANFrame* txf = canardPeekTxQueue(&test_iface.canard); txf != NULL; txf = canardPeekTxQueue(&test_iface.canard)) {
        if (canard_ifaces[0]) {
            canardHandleRxFrame(&canard_ifaces[0]->canard, txf, AP_HAL::micros64());   
        }
        canardPopTxQueue(&test_iface.canard);
    }
}
#endif

void CanardInterface::processTx(bool raw_commands_only = false) {
    WITH_SEMAPHORE(_sem_tx);

    for (uint8_t iface = 0; iface < num_ifaces; iface++) {
        if (ifaces[iface] == NULL) {
            continue;
        }
        auto txq = canard.tx_queue;
        if (txq == nullptr) {
            return;
        }
        // volatile as the value can change at any time during can interrupt
        // we need to ensure that this is not optimized
        volatile const auto *stats = ifaces[iface]->get_statistics();
        uint64_t last_transmit_us = stats==nullptr?0:stats->last_transmit_us;
        bool iface_down = true;
        if (stats == nullptr || (AP_HAL::micros64() - last_transmit_us) < 200000UL) {
            /*
            We were not able to queue the frame for
            sending. Only mark the send as failing if the
            interface is active. We consider an interface as
            active if it has had successful transmits for some time.
            */
            iface_down = false;
        } 
        // scan through list of pending transfers
        while (true) {
            auto txf = &txq->frame;
            if (raw_commands_only &&
                CANARD_MSG_TYPE_FROM_ID(txf->id) != UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID &&
                CANARD_MSG_TYPE_FROM_ID(txf->id) != COM_HOBBYWING_ESC_RAWCOMMAND_ID) {
                // look at next transfer
                txq = txq->next;
                if (txq == nullptr) {
                    break;
                }
                continue;
            }
            AP_HAL::CANFrame txmsg {};
            txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len);
            memcpy(txmsg.data, txf->data, txf->data_len);
            txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
#if HAL_CANFD_SUPPORTED
            txmsg.canfd = txf->canfd;
#endif
            bool write = true;
            bool read = false;
            ifaces[iface]->select(read, write, &txmsg, 0);
            if (!write) {
                // if there is no space then we need to start from the
                // top of the queue, so wait for the next loop
                if (!iface_down) {
                    break;
                } else {
                    txf->iface_mask &= ~(1U<<iface);
                }
            } else if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
                // try sending to interfaces, clearing the mask if we succeed
                if (ifaces[iface]->send(txmsg, txf->deadline_usec, 0) > 0) {
                    txf->iface_mask &= ~(1U<<iface);
                } else {
                    // if we fail to send then we try sending on next interface
                    if (!iface_down) {
                        break;
                    } else {
                        txf->iface_mask &= ~(1U<<iface);
                    }
                }
            }
            // look at next transfer
            txq = txq->next;
            if (txq == nullptr) {
                break;
            }
        }
    }

}

void CanardInterface::update_rx_protocol_stats(int16_t res)
{
    switch (res) {
    case CANARD_OK:
        protocol_stats.rx_frames++;
        break;
    case -CANARD_ERROR_OUT_OF_MEMORY:
        protocol_stats.rx_error_oom++;
        break;
    case -CANARD_ERROR_INTERNAL:
        protocol_stats.rx_error_internal++;
        break;
    case -CANARD_ERROR_RX_INCOMPATIBLE_PACKET:
        protocol_stats.rx_ignored_not_wanted++;
        break;
    case -CANARD_ERROR_RX_WRONG_ADDRESS:
        protocol_stats.rx_ignored_wrong_address++;
        break;
    case -CANARD_ERROR_RX_NOT_WANTED:
        protocol_stats.rx_ignored_not_wanted++;
        break;
    case -CANARD_ERROR_RX_MISSED_START:
        protocol_stats.rx_error_missed_start++;
        break;
    case -CANARD_ERROR_RX_WRONG_TOGGLE:
        protocol_stats.rx_error_wrong_toggle++;
        break;
    case -CANARD_ERROR_RX_UNEXPECTED_TID:
        protocol_stats.rx_ignored_unexpected_tid++;
        break;
    case -CANARD_ERROR_RX_SHORT_FRAME:
        protocol_stats.rx_error_short_frame++;
        break;
    case -CANARD_ERROR_RX_BAD_CRC:
        protocol_stats.rx_error_bad_crc++;
        break;
    default:
        // mark all other errors as internal
        protocol_stats.rx_error_internal++;
        break;
    }
}

void CanardInterface::processRx() {
    AP_HAL::CANFrame rxmsg;
    for (uint8_t i=0; i<num_ifaces; i++) {
        while(true) {
            if (ifaces[i] == NULL) {
                break;
            }
            bool read_select = true;
            bool write_select = false;
            ifaces[i]->select(read_select, write_select, nullptr, 0);
            if (!read_select) { // No data pending
                break;
            }
            CanardCANFrame rx_frame {};

            //palToggleLine(HAL_GPIO_PIN_LED);
            uint64_t timestamp;
            AP_HAL::CANIface::CanIOFlags flags;
            if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) {
                break;
            }

            if (!rxmsg.isExtended()) {
                // 11 bit frame, see if we have a handler
                if (aux_11bit_driver != nullptr) {
                    aux_11bit_driver->handle_frame(rxmsg);
                }
                continue;
            }

            rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc);
            memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len);
#if HAL_CANFD_SUPPORTED
            rx_frame.canfd = rxmsg.canfd;
#endif
            rx_frame.id = rxmsg.id;
#if CANARD_MULTI_IFACE
            rx_frame.iface_id = i;
#endif
            {
                WITH_SEMAPHORE(_sem_rx);

                const int16_t res = canardHandleRxFrame(&canard, &rx_frame, timestamp);
                if (res == -CANARD_ERROR_RX_MISSED_START) {
                    // this might remaining frames from a message that we don't accept, so check
                    uint64_t dummy_signature;
                    if (shouldAcceptTransfer(&canard,
                                        &dummy_signature,
                                        extractDataType(rx_frame.id),
                                        extractTransferType(rx_frame.id),
                                        1)) { // doesn't matter what we pass here
                        update_rx_protocol_stats(res);
                    } else {
                        protocol_stats.rx_ignored_not_wanted++;
                    }
                } else {
                    update_rx_protocol_stats(res);
                }
            }
        }
    }
}

void CanardInterface::process(uint32_t duration_ms) {
#if AP_TEST_DRONECAN_DRIVERS
    const uint64_t deadline = AP_HAL::micros64() + duration_ms*1000;
    while (AP_HAL::micros64() < deadline) {
        processTestRx();
        hal.scheduler->delay_microseconds(1000);
    }
#else
    const uint64_t deadline = AP_HAL::micros64() + duration_ms*1000;
    while (true) {
        processRx();
        processTx();
        {
            WITH_SEMAPHORE(_sem_rx);
            WITH_SEMAPHORE(_sem_tx);
            canardCleanupStaleTransfers(&canard, AP_HAL::micros64());
        }
        const uint64_t now = AP_HAL::micros64();
        if (now < deadline) {
            IGNORE_RETURN(sem_handle.wait(deadline - now));
        } else {
            break;
        }
    }
#endif
}

bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface)
{
    if (num_ifaces > HAL_NUM_CAN_IFACES) {
        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Num Ifaces Exceeded\n");
        return false;
    }
    if (can_iface == nullptr) {
        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface Null\n");
        return false;
    }
    if (ifaces[num_ifaces] != nullptr) {
        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface already added\n");
        return false;
    }
    ifaces[num_ifaces] = can_iface;
    if (ifaces[num_ifaces] == nullptr) {
        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Can't alloc uavcan::iface\n");
        return false;
    }
    if (!can_iface->set_event_handle(&sem_handle)) {
        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Setting event handle failed\n");
        return false;
    }
    AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "DroneCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces));
    num_ifaces++;
    return true;
}

// add an 11 bit auxillary driver
bool CanardInterface::add_11bit_driver(CANSensor *sensor)
{
    if (aux_11bit_driver != nullptr) {
        // only allow one
        return false;
    }
    aux_11bit_driver = sensor;
    return true;
}

// handler for outgoing frames for auxillary drivers
bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
{
    const uint64_t tx_deadline_us = AP_HAL::micros64() + timeout_us;
    bool ret = false;
    for (uint8_t iface = 0; iface < num_ifaces; iface++) {
        if (ifaces[iface] == NULL) {
            continue;
        }
        ret |= ifaces[iface]->send(out_frame, tx_deadline_us, 0) > 0;
    }
    return ret;
}

#endif // #if HAL_ENABLE_DRONECAN_DRIVERS