N3_sub/libraries/AP_KDECAN/examples/KDECAN_sniffer/KDECAN_sniffer.cpp

/*
  simple KDECAN network sniffer as an ArduPilot firmware
 */
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>

#if (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) && HAL_WITH_UAVCAN

#include <AP_HAL/CAN.h>
#include <AP_HAL/Semaphores.h>

#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_HAL_Linux/CAN.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/CAN.h>
#endif

#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>

#include <cinttypes>

void setup();
void loop();

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

#define debug_can(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)

#define NUM_ESCS 8

class KDECAN_sniffer {
public:
    KDECAN_sniffer() {
        for (uint8_t i = 0; i < NUM_ESCS; i++) {
            _esc_info[i].mcu_id = 0xA5961824E7BD3C00 | i;
        }               
    }

    void init(void);
    void loop(void);
    void print_stats(void);
    void send_enumeration(uint8_t num);

private:
    uint8_t _driver_index = 0;
    uint8_t _interface = 0;
    uavcan::ICanDriver* _can_driver;
    uint8_t _mask_received_pwm = 0;

    struct esc_info {
        uint8_t node_id;
        uint64_t mcu_id;
        uint64_t enum_timeout;

        esc_info() : node_id(1), mcu_id(0), enum_timeout(0) {}
    } _esc_info[NUM_ESCS];
    
    uint8_t _max_node_id = 0;

    static const uint8_t BROADCAST_NODE_ID = 1;

    static const uint8_t ESC_INFO_OBJ_ADDR = 0;
    static const uint8_t SET_PWM_OBJ_ADDR = 1;
    static const uint8_t VOLTAGE_OBJ_ADDR = 2;
    static const uint8_t CURRENT_OBJ_ADDR = 3;
    static const uint8_t RPM_OBJ_ADDR = 4;
    static const uint8_t TEMPERATURE_OBJ_ADDR = 5;
    static const uint8_t GET_PWM_INPUT_OBJ_ADDR = 6;
    static const uint8_t GET_PWM_OUTPUT_OBJ_ADDR = 7;
    static const uint8_t MCU_ID_OBJ_ADDR = 8;
    static const uint8_t UPDATE_NODE_ID_OBJ_ADDR = 9;
    static const uint8_t START_ENUM_OBJ_ADDR = 10;
    static const uint8_t TELEMETRY_OBJ_ADDR = 11;
};

static struct {
    uint32_t frame_id;
    uint32_t count;
} counters[100];

static void count_msg(uint32_t frame_id)
{
    for (uint16_t i=0; i<ARRAY_SIZE(counters); i++) {
        if (counters[i].frame_id == frame_id) {
            counters[i].count++;
            break;
        }
        if (counters[i].frame_id == 0) {
            counters[i].frame_id = frame_id;
            counters[i].count++;
            break;
        }
    }
}

void KDECAN_sniffer::init(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
    AP_HAL::CANManager* can_mgr = new ChibiOS::CANManager;
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
    AP_HAL::CANManager* can_mgr = new Linux::CANManager;
#endif
    
    if (can_mgr == nullptr) {
        AP_HAL::panic("Couldn't allocate CANManager, something is very wrong");
    }

    const_cast <AP_HAL::HAL&> (hal).can_mgr[_driver_index] = can_mgr;
    can_mgr->begin(1000000, _interface);
    can_mgr->initialized(true);

    if (!can_mgr->is_initialized()) {
        debug_can("Can not initialised\n");
        return;
    }

    _can_driver = can_mgr->get_driver();

    if (_can_driver == nullptr) {
        debug_can("KDECAN: no CAN driver\n\r");
        return;
    }

    debug_can("KDECAN: init done\n\r");
}

void KDECAN_sniffer::loop(void)
{
    if (_can_driver == nullptr) {
        return;
    }
    
    uavcan::CanFrame empty_frame { (0 | uavcan::CanFrame::FlagEFF), nullptr, 0 };
    const uavcan::CanFrame* select_frames[uavcan::MaxCanIfaces] { &empty_frame };
    uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1);

    uavcan::CanSelectMasks inout_mask;
    inout_mask.read = 1 << _interface;

    uavcan::CanSelectMasks in_mask = inout_mask;
    _can_driver->select(inout_mask, select_frames, timeout);

    if (in_mask.read & inout_mask.read) {
        uavcan::CanFrame frame;
        uavcan::MonotonicTime time;
        uavcan::UtcTime utc_time;
        uavcan::CanIOFlags flags {};

        int16_t res = _can_driver->getIface(_interface)->receive(frame, time, utc_time, flags);
        
        if (res == 1) {
            uint32_t id = frame.id & uavcan::CanFrame::MaskExtID;
            uint8_t object_address = id & 0xFF;
            uint8_t esc_num = uint8_t((id >> 8) & 0xFF);

            count_msg(id);

            uint8_t i = 0;
            uint8_t n = NUM_ESCS;

            if (esc_num != BROADCAST_NODE_ID) {
                for (; i < NUM_ESCS; i++) {
                    if (object_address == UPDATE_NODE_ID_OBJ_ADDR) {
                        if (_esc_info[i].mcu_id == be64toh(*((be64_t*) &(frame.data[0])))) {
                            n = i + 1;
                            break;                            
                        }
                    } else if (_esc_info[i].node_id == esc_num) {
                        n = i + 1;
                        break;
                    }
                }
            }

            while (i < n) {
                uavcan::CanFrame res_frame;

                switch (object_address) {
                    case ESC_INFO_OBJ_ADDR: {
                        uint8_t info[5] { 1, 2, 3, 4, 0 };

                        res_frame.dlc = 5;
                        memcpy(res_frame.data, info, 5);

                        break;
                    }
                    case SET_PWM_OBJ_ADDR: {
                        if ((1 << (esc_num - 2) & _mask_received_pwm) && _mask_received_pwm != ((1 << _max_node_id) - 1)) {
                            count_msg(0xFFFFFFF0);
                            _mask_received_pwm = 0;
                        }

                        _mask_received_pwm |= 1 << (esc_num - 2);

                        if (_mask_received_pwm == ((1 << _max_node_id) - 1)) {
                            count_msg(0xFFFFFFFF);
                            _mask_received_pwm = 0;
                        }

                        res_frame.dlc = 0;

                        break;
                    }
                    case UPDATE_NODE_ID_OBJ_ADDR: {
                        if (_esc_info[i].enum_timeout != 0 && _esc_info[i].enum_timeout >= AP_HAL::micros64()) {
                            _esc_info[i].node_id = esc_num;
                            _max_node_id = MAX(_max_node_id, esc_num - 2 + 1);
                            hal.console->printf("Set node ID %d for ESC %d\n", esc_num, i);
                        }

                        _esc_info[i].enum_timeout = 0;

                        res_frame.dlc = 1;
                        memcpy(res_frame.data, &(_esc_info[i].node_id), 1);

                        break;
                    }
                    case START_ENUM_OBJ_ADDR: {
                        _esc_info[i].enum_timeout = AP_HAL::micros64() + be16toh(*((be16_t*) &(frame.data[0]))) * 1000;
                        hal.console->printf("Starting enumeration for ESC %d, timeout %" PRIu64 "\n", i, _esc_info[i].enum_timeout);
                        i++;
                        continue;
                    }
                    case TELEMETRY_OBJ_ADDR: {
                        uint8_t data[7] {};
                        *((be16_t*) &data[0]) = htobe16(get_random16());
                        *((be16_t*) &data[2]) = htobe16(get_random16());
                        *((be16_t*) &data[4]) = htobe16(get_random16());
                        data[6] = uint8_t(float(rand()) / RAND_MAX * 40.0f + 15);

                        res_frame.dlc = 7;
                        memcpy(res_frame.data, data, 7);
                        break;
                    }
                    case VOLTAGE_OBJ_ADDR:
                    case CURRENT_OBJ_ADDR:
                    case RPM_OBJ_ADDR:
                    case TEMPERATURE_OBJ_ADDR:
                    case GET_PWM_INPUT_OBJ_ADDR:
                    case GET_PWM_OUTPUT_OBJ_ADDR:
                    case MCU_ID_OBJ_ADDR:
                    default:
                        // discard frame
                        return;
                }

                res_frame.id = (_esc_info[i].node_id << 16) | object_address | uavcan::CanFrame::FlagEFF;
                timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::millis() + 500);
                int16_t res2 = _can_driver->getIface(_interface)->send(res_frame, timeout, 0);

                if (res2 == 1) {
                    i++;
                }
            }
        }
    }
}

void KDECAN_sniffer::print_stats(void)
{
    hal.console->printf("%lu\n", AP_HAL::micros());
    for (uint16_t i=0;i<100;i++) {
        if (counters[i].frame_id == 0) {
            break;
        }
        hal.console->printf("0x%08" PRIX32 ": %" PRIu32 "\n", counters[i].frame_id, counters[i].count);
        counters[i].count = 0;
    }
    hal.console->printf("\n");
}

void KDECAN_sniffer::send_enumeration(uint8_t num)
{
    if (_esc_info[num].enum_timeout == 0 || AP_HAL::micros64() > _esc_info[num].enum_timeout) {
        _esc_info[num].enum_timeout = 0;
        hal.console->printf("Not running enumeration for ESC %d\n", num);
        return;
    }

    while (true) {
        uint8_t mcu[8] {};
        *((be64_t*) mcu) = htobe64(_esc_info[num].mcu_id);
        uavcan::CanFrame res_frame { (_esc_info[num].node_id << 16) | START_ENUM_OBJ_ADDR | uavcan::CanFrame::FlagEFF,
                                     mcu,
                                     8 };
        uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1);

        int16_t res = _can_driver->getIface(_interface)->send(res_frame, timeout, 0);

        if (res == 1) {
            return;
        }
    }
}

static KDECAN_sniffer sniffer;

void setup(void)
{
    hal.scheduler->delay(2000);
    hal.console->printf("Starting KDECAN sniffer\n");
    sniffer.init();
}

void loop(void)
{
    sniffer.loop();
    static uint32_t last_print_ms;
    uint32_t now = AP_HAL::millis();
    if (now - last_print_ms >= 1000) {
        last_print_ms = now;
        sniffer.print_stats();
    }

    if (hal.console->available() >= 3) {
        char c = hal.console->read();

        if (c == 'e') {
            c = hal.console->read();

            if (c == ' ') {
                c = hal.console->read();

                if (c >= '0' && c < '9') {
                    uint8_t num = c - '0';
                    sniffer.send_enumeration(num);
                }
            }
        } else if (c == 'r') {
            hal.console->printf("rebooting\n");
            hal.scheduler->reboot(false);
        }
    }

    // auto-reboot for --upload
    if (hal.console->available() > 50) {
        hal.console->printf("rebooting\n");
        hal.scheduler->reboot(false);
    }
}

AP_HAL_MAIN();

#else

#include <stdio.h>

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

static void loop() { }
static void setup()
{
    printf("Board not currently supported\n");
}

AP_HAL_MAIN();
#endif