DCU_APP/GD32E508_DCU_APP/sys/task_runner.cpp

/**
 * @file task_runner.cpp
 * @brief 任务执行器实现。
 */

#include "main.h"
#include "device/adc.h"
#include "device/watchdog.h"
#include "device/spi/app_spi_command.h"
#include "device/canfd.h"
#include "device/motor_state.h"
#include "device/motor_version.h"
#include "sys/config.h"
#include "sys/ota_boot_request.h"
#include "sys/motor_ota_relay.h"
#include "sys/task_manager.h"
#include "sys/task_runner.h"
#include "comm/ethercat_slave.h"

namespace {
constexpr uint32_t kAdcFilterWindow = CONFIG_ADC_FILTER_SIZE + 1U;

/**
 * @brief Push one sample into a fixed-size sliding window buffer.
 * @param[in,out] buffer Sample buffer with `kAdcFilterWindow` elements.
 * @param[in] value New sample value.
 */
static void pushSample(float *buffer, float value)
{
    for(uint32_t i = 0U; i < CONFIG_ADC_FILTER_SIZE; ++i) {
        buffer[i] = buffer[i + 1U];
    }
    buffer[CONFIG_ADC_FILTER_SIZE] = value;
}

/**
 * @brief Apply median filtering to a fixed-size sample window.
 * @param[in] buffer Sample buffer with `kAdcFilterWindow` elements.
 * @return Median value in the current window.
 */
static float medianFilter(const float *buffer)
{
    float sorted[kAdcFilterWindow];

    for(uint32_t i = 0U; i < kAdcFilterWindow; ++i) {
        sorted[i] = buffer[i];
    }

    for(uint32_t i = 1U; i < kAdcFilterWindow; ++i) {
        const float key = sorted[i];
        uint32_t j = i;
        while((j > 0U) && (sorted[j - 1U] > key)) {
            sorted[j] = sorted[j - 1U];
            --j;
        }
        sorted[j] = key;
    }

    return sorted[kAdcFilterWindow / 2U];
}
} // namespace

struct TaskRunner::Runtime {
    Adc::Sample sample{};
    float temperature_samples[kAdcFilterWindow] = {0.0f};
    float vdda_samples[kAdcFilterWindow] = {0.0f};
    float vin_adc_samples[kAdcFilterWindow] = {0.0f};
    uint16_t last_vin_raw = 0U;
    uint16_t last_vref_raw = 0U;
    uint16_t last_temp_raw = 0U;
    uint8_t last_rx_channel = 0U;
    uint32_t last_rx_id = 0U;
    uint8_t last_rx_len = 0U;
    uint8_t last_rx_data[64] = {0};
};

TaskRunner::Runtime TaskRunner::s_runtime;

/**
 * @brief 采样 VIN 与 VREF 原始 ADC 数值。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      从 ADC DMA 缓冲中读取外部 VIN_ADC 与内部 VREFINT 通道最新值。
 */
void TaskRunner::taskAdcVrefSample()
{
    s_runtime.last_vin_raw = Adc::readVinRaw();
    s_runtime.last_vref_raw = Adc::readVrefRaw();
}

/**
 * @brief 采样内部温度通道原始 ADC 数值。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      读取 ADC DMA 缓冲中的内部温度通道最新值。
 */
void TaskRunner::taskAdcTempSample()
{
    s_runtime.last_temp_raw = Adc::readTempRaw();
}

/**
 * @brief 根据原始采样值计算工程量结果。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      输出 `vin_adc_v`、`vdda` 和 `temperature` 等可直接使用的工程值。
 */
void TaskRunner::taskAdcProcess()
{
    auto &s = s_runtime.sample;
    const float vdda = Adc::calculateVdda(s_runtime.last_vref_raw);
    const float vin_adc_v = Adc::calculateVinAdcVoltage(s_runtime.last_vin_raw, vdda);
    const float temperature = Adc::calculateTemperature(s_runtime.last_temp_raw, vdda);
    const bool sampleValid = (s_runtime.last_vref_raw > 0U) &&
                             (s_runtime.last_vref_raw < 4095U) &&
                             (s_runtime.last_temp_raw > 0U) &&
                             (s_runtime.last_temp_raw < 4095U) &&
                             (vdda >= 2.7f) && (vdda <= 3.6f) &&
                             (temperature >= -40.0f) && (temperature <= 125.0f);

    if(sampleValid) {
        pushSample(s_runtime.vdda_samples, vdda);
        pushSample(s_runtime.vin_adc_samples, vin_adc_v);
        pushSample(s_runtime.temperature_samples, temperature);
    }

    s.vin_raw = s_runtime.last_vin_raw;
    s.vref_raw = s_runtime.last_vref_raw;
    s.temp_raw = s_runtime.last_temp_raw;
    s.vdda = medianFilter(s_runtime.vdda_samples);
    s.vin_adc_v = medianFilter(s_runtime.vin_adc_samples);
    s.temperature = medianFilter(s_runtime.temperature_samples);
    s.valid = sampleValid ? 1U : 0U;
}

/**
 * @brief 执行 CAN 周期发送任务：每 10ms 在每个使能通道上发送一帧 CANFD 测试帧。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      帧格式：扩展帧 29-bit ID，CAN-FD，DLC=64，BRS 受 CONFIG_CAN_FD_BRS_ENABLE 控制。
 *            载荷布局：
 *              [0]     固定标识 0xC0
 *              [1]     通道号
 *              [2..5]  发送序号（小端 uint32）
 *              [6..7]  系统运行秒数低 16 位
 *              [8..63] 填充 0xA0 + (index & 0x0F)
 *            periodic=true：仅允许一个邮箱保持自动重发，不主动 abort 旧帧，
 *            跳过 TEC 保护以匹配诊断行为。
 */
void TaskRunner::taskCanTxSend()
{
#if (CONFIG_CAN_PERIODIC_TEST_TX_ENABLE != 0)
    static uint32_t seq = 0U;

    for (uint8_t ch = 0U; ch < 3U; ++ch) {
        if (!CanFd::isChannelEnabled(ch)) {
            continue;
        }

        uint8_t data[64];
        data[0] = 0xC0U;
        data[1] = ch;
        const uint32_t s = seq;
        data[2] = static_cast<uint8_t>(s & 0xFFU);
        data[3] = static_cast<uint8_t>((s >> 8) & 0xFFU);
        data[4] = static_cast<uint8_t>((s >> 16) & 0xFFU);
        data[5] = static_cast<uint8_t>((s >> 24) & 0xFFU);
        data[6] = static_cast<uint8_t>(g_app_uptime_sec & 0xFFU);
        data[7] = static_cast<uint8_t>((g_app_uptime_sec >> 8) & 0xFFU);
        for (uint8_t i = 8U; i < 64U; ++i) {
            data[i] = static_cast<uint8_t>(0xA0U + (i & 0x0FU));
        }

        const uint32_t extId = 0x18FF5000U | static_cast<uint32_t>(ch);
        if (CanFd::transmitFrame(ch, extId, data, sizeof(data), true) == CanFd::CanTxResult::kOk) {
            seq++;
        }
    }
#else
    /* CONFIG_CAN_PERIODIC_TEST_TX_ENABLE=0：测试帧无业务接收节点 ACK，
     * 单发模式下会持续产生 MTEn 错误帧，整任务空跑。 */
    (void)0;
#endif
}

/**
 * @brief 批量解析并消费待处理的 CAN 接收报文。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      一次任务按预算尽量清空软件接收队列；若仍有积压，则重新置位解析任务。
 */
void TaskRunner::taskCanRxParse()
{
    uint32_t consumed = 0U;
    while(consumed < CONFIG_CAN_RX_PARSE_BUDGET) {
        if(!CanFd::consumeRxFrame(&s_runtime.last_rx_id, s_runtime.last_rx_data, &s_runtime.last_rx_len, &s_runtime.last_rx_channel)) {
            break;
        }
        if(OtaBootRequest::handleCanFrame(s_runtime.last_rx_channel,
                                          s_runtime.last_rx_id,
                                          s_runtime.last_rx_data,
                                          s_runtime.last_rx_len)) {
            OtaBootRequest::process();
            return;
        }
        if(MotorOtaRelay::onCanFrame(s_runtime.last_rx_channel,
                                     s_runtime.last_rx_id,
                                     s_runtime.last_rx_data,
                                     s_runtime.last_rx_len)) {
            consumed++;
            continue;
        }
#if (CONFIG_MOTOR_STATE_ENABLE != 0)
        MotorState::onCanFrame(s_runtime.last_rx_channel,
                               s_runtime.last_rx_id,
                               s_runtime.last_rx_data,
                               s_runtime.last_rx_len);
#endif
#if (CONFIG_MOTOR_VERSION_ENABLE != 0)
        MotorVersion::onCanFrame(s_runtime.last_rx_channel,
                                 s_runtime.last_rx_id,
                                 s_runtime.last_rx_data,
                                 s_runtime.last_rx_len);
#endif
        consumed++;
    }

    if(CanFd::hasRxFrame()) {
        TaskManager::set(1UL << TASK_CAN_RX_PARSE);
    }
}

/**
 * @brief 执行系统心跳维护任务。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      推进 OTA Boot 请求、刷新 CAN 反馈快照、启动 CAN/ADC 1Hz 推送轮次。
 */
void TaskRunner::taskHeartbeat()
{
    OtaBootRequest::process();
    CanFd::refreshFeedbackSnapshot();
    canFeedbackStartRound();
    /* ADC 调试反馈 1Hz：CONFIG_SPI_ADC_FEEDBACK_ENABLE=0 时为空操作 */
    adcFeedbackStartRound();
#if (CONFIG_MOTOR_STATE_ENABLE != 0)
    MotorState::checkOffline();
#endif
}

/**
 * @brief 启动多电机状态 0xC5 推送轮次（250ms 独立节拍）。
 * @note  与 taskHeartbeat 解耦：不连带加速 OTA Boot 轮询 / CAN 快照 / ADC 推送 /
 *        checkOffline 离线判定（3s 阈值）。仅触发本轮 P0/P1 分页推送，
 *        实际 SPI 传输由 motorStatePushStep() 在主循环按铃协议推进。
 */
void TaskRunner::taskMotorStatePush()
{
#if (CONFIG_MOTOR_STATE_ENABLE != 0)
    motorStateStartRound();
#endif
}

/**
 * @brief 拷贝当前 ADC 工程量快照（供 SPI 0xC4 等只读路径）。
 * @param[out] out  非空时写入最新 Sample
 * @return true 已写入（不保证 sample.valid==1）
 */
bool TaskRunner::copyAdcSample(Adc::Sample *out)
{
    if (out == nullptr) {
        return false;
    }
    *out = s_runtime.sample;
    return true;
}

/**
 * @brief 5s 诊断任务：默认仅状态变化/错误增量；可选全量 UART 摘要。
 * @note  CONFIG_UART_TEMP_PERIODIC_PRINT=1 时打印 ADC/CAN/ECAT 全量行（调试用）。
 *        默认=0：相对上次快照有 CAN 错误计数增量、ADC valid 跳变、ECAT ready 变化或 ISR 超阈时才打印。
 *        采用定点整数格式，避免依赖浮点 printf。
 */
void TaskRunner::taskDiagnosticPrint()
{
    const auto *diag = CanFd::diag();
    const auto &s = s_runtime.sample;

#if (CONFIG_UART_TEMP_PERIODIC_PRINT != 0)
    /* ---- Debug 全量摘要：周期刷 ADC / CAN 吞吐 / ECAT ISR 统计 ---- */
    {
        const int32_t temp_x100 = static_cast<int32_t>(s.temperature * 100.0f);
        const uint32_t vdda_mV = static_cast<uint32_t>(s.vdda * 1000.0f);
        const uint32_t vin_adc_mV = static_cast<uint32_t>(s.vin_adc_v * 1000.0f);

#if (CONFIG_UART_DIAG_LEVEL == 0)
        printf("[APP] uptime=%lus valid=%u, temp_raw=%u, vref_raw=%u, temp=%ld.%02ld C, vdda=%lu.%03lu V, vin_adc=%lu.%03lu V\r\n",
               (unsigned long)g_app_uptime_sec,
               static_cast<unsigned int>(s.valid),
               static_cast<unsigned int>(s.temp_raw),
               static_cast<unsigned int>(s.vref_raw),
               static_cast<long>(temp_x100 / 100),
               static_cast<long>((temp_x100 >= 0 ? temp_x100 : -temp_x100) % 100),
               static_cast<unsigned long>(vdda_mV / 1000U),
               static_cast<unsigned long>(vdda_mV % 1000U),
               static_cast<unsigned long>(vin_adc_mV / 1000U),
               static_cast<unsigned long>(vin_adc_mV % 1000U));
#else
        printf("[APP] uptime=%lus valid=%u, temp=%ld.%02ld C, vdda=%lu.%03lu V, vin_adc=%lu.%03lu V, rx_ch=%u, rx_id=0x%08lX, rx_len=%u, tx_ch=%u, can_tx=%lu, can_rx=%lu, sw_drop=%lu, hw_full=%lu, hw_over=%lu, tx_abort=%lu, can_err=%lu\r\n",
               (unsigned long)g_app_uptime_sec,
               static_cast<unsigned int>(s.valid),
               static_cast<long>(temp_x100 / 100),
               static_cast<long>((temp_x100 >= 0 ? temp_x100 : -temp_x100) % 100),
               static_cast<unsigned long>(vdda_mV / 1000U),
               static_cast<unsigned long>(vdda_mV % 1000U),
               static_cast<unsigned long>(vin_adc_mV / 1000U),
               static_cast<unsigned long>(vin_adc_mV % 1000U),
               static_cast<unsigned int>(s_runtime.last_rx_channel),
               static_cast<unsigned long>(s_runtime.last_rx_id),
               static_cast<unsigned int>(s_runtime.last_rx_len),
               static_cast<unsigned int>(diag->last_tx_channel),
               static_cast<unsigned long>(diag->tx_count),
               static_cast<unsigned long>(diag->rx_count),
               static_cast<unsigned long>(diag->rx_drop_count),
               static_cast<unsigned long>(diag->rx_fifo_full_count),
               static_cast<unsigned long>(diag->rx_fifo_overfull_count),
               static_cast<unsigned long>(diag->tx_abort_count),
               static_cast<unsigned long>(diag->error_count));
#endif

        if (EthercatSlave::isEscReady()) {
            uint32_t maxCyc = 0U, avgCyc = 0U, cnt = 0U;
            EthercatSlave::getIsrStats(&maxCyc, &avgCyc, &cnt);
            /* 180MHz: cycles / 180 = µs;用 *1000/180 获得亚微秒精度避免浮点 */
            const uint32_t maxUs_x1000 = maxCyc * 1000U / 180U;
            const uint32_t avgUs_x1000 = avgCyc * 1000U / 180U;
            printf("[ECAT] periodicIsr cnt=%lu max=%lu.%03luus avg=%lu.%03luus\r\n",
                   (unsigned long)cnt,
                   (unsigned long)(maxUs_x1000 / 1000U), (unsigned long)(maxUs_x1000 % 1000U),
                   (unsigned long)(avgUs_x1000 / 1000U), (unsigned long)(avgUs_x1000 % 1000U));
        } else {
            printf("[ECAT] ESC not detected (chip id=0x%08lX), EtherCAT disabled\r\n",
                   (unsigned long)EthercatSlave::getChipId());
        }
    }
#else
    /* ---- 默认静默路径：仅错误增量 / 状态变化，无 uptime 心跳 ---- */
    {
        static uint32_t s_prev_rx_drop = 0U;
        static uint32_t s_prev_rx_fifo_full = 0U;
        static uint32_t s_prev_rx_fifo_overfull = 0U;
        static uint32_t s_prev_tx_abort = 0U;
        static uint32_t s_prev_error = 0U;
        static uint8_t  s_prev_adc_valid = 0xFFU; /* 0xFF=尚未采样 */
        static bool     s_prev_esc_ready = false;
        static bool     s_esc_ready_inited = false;
        static uint32_t s_prev_ecat_max_cyc = 0U;

        const uint32_t rx_drop = diag->rx_drop_count;
        const uint32_t rx_full = diag->rx_fifo_full_count;
        const uint32_t rx_over = diag->rx_fifo_overfull_count;
        const uint32_t tx_abort = diag->tx_abort_count;
        const uint32_t can_err = diag->error_count;

        if (rx_drop > s_prev_rx_drop || rx_full > s_prev_rx_fifo_full
            || rx_over > s_prev_rx_fifo_overfull || tx_abort > s_prev_tx_abort
            || can_err > s_prev_error) {
            printf("[CAN-DIAG] uptime=%lus err=%lu(+%lu) drop=%lu(+%lu) full=%lu(+%lu) over=%lu(+%lu) abort=%lu(+%lu)\r\n",
                   (unsigned long)g_app_uptime_sec,
                   (unsigned long)can_err,
                   (unsigned long)(can_err - s_prev_error),
                   (unsigned long)rx_drop,
                   (unsigned long)(rx_drop - s_prev_rx_drop),
                   (unsigned long)rx_full,
                   (unsigned long)(rx_full - s_prev_rx_fifo_full),
                   (unsigned long)rx_over,
                   (unsigned long)(rx_over - s_prev_rx_fifo_overfull),
                   (unsigned long)tx_abort,
                   (unsigned long)(tx_abort - s_prev_tx_abort));
            s_prev_rx_drop = rx_drop;
            s_prev_rx_fifo_full = rx_full;
            s_prev_rx_fifo_overfull = rx_over;
            s_prev_tx_abort = tx_abort;
            s_prev_error = can_err;
        }

        if (s_prev_adc_valid == 0xFFU) {
            s_prev_adc_valid = s.valid;
        } else if (s.valid != s_prev_adc_valid) {
            printf("[APP] adc valid %u -> %u (uptime=%lus)\r\n",
                   static_cast<unsigned int>(s_prev_adc_valid),
                   static_cast<unsigned int>(s.valid),
                   (unsigned long)g_app_uptime_sec);
            s_prev_adc_valid = s.valid;
        }

        const bool esc_ready = EthercatSlave::isEscReady();
        if (!s_esc_ready_inited) {
            s_prev_esc_ready = esc_ready;
            s_esc_ready_inited = true;
        } else if (esc_ready != s_prev_esc_ready) {
            printf("[ECAT] ESC ready %u -> %u chip_id=0x%08lX (uptime=%lus)\r\n",
                   static_cast<unsigned int>(s_prev_esc_ready ? 1U : 0U),
                   static_cast<unsigned int>(esc_ready ? 1U : 0U),
                   (unsigned long)EthercatSlave::getChipId(),
                   (unsigned long)g_app_uptime_sec);
            s_prev_esc_ready = esc_ready;
        }

        if (esc_ready) {
            uint32_t maxCyc = 0U, avgCyc = 0U, cnt = 0U;
            EthercatSlave::getIsrStats(&maxCyc, &avgCyc, &cnt);
            /* 阈值单位 µs → CPU 周期（180 MHz） */
            const uint32_t threshCyc = CONFIG_ETHERCAT_ISR_MAX_US_THRESHOLD * 180U;
            if (maxCyc > threshCyc && maxCyc > s_prev_ecat_max_cyc) {
                const uint32_t maxUs_x1000 = maxCyc * 1000U / 180U;
                printf("[ECAT] ISR max over threshold: max=%lu.%03luus thr=%luus cnt=%lu (uptime=%lus)\r\n",
                       (unsigned long)(maxUs_x1000 / 1000U),
                       (unsigned long)(maxUs_x1000 % 1000U),
                       (unsigned long)CONFIG_ETHERCAT_ISR_MAX_US_THRESHOLD,
                       (unsigned long)cnt,
                       (unsigned long)g_app_uptime_sec);
                s_prev_ecat_max_cyc = maxCyc;
            }
        }
    }
#endif
}

/**
 * @brief 执行 EtherCAT 周期/事件处理(非实时应用层 PDO 映射)。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      由周期 ISR(EXTI0 SYNC0)或 ESC 事件 ISR(EXTI1 IRQ)置位 TASK_ETHERCAT_PROCESS。
 */
void TaskRunner::taskEthercatProcess()
{
    EthercatSlave::process();
}

/**
 * @brief 初始化任务执行器运行时上下文。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      执行一次完整 ADC 采样，并同步填充运行时缓存的原始值起点。
 */
void TaskRunner::init()
{
    Adc::collectSample(&s_runtime.sample);
    s_runtime.last_vin_raw = s_runtime.sample.vin_raw;
    s_runtime.last_vref_raw = s_runtime.sample.vref_raw;
    s_runtime.last_temp_raw = s_runtime.sample.temp_raw;

    for(uint32_t i = 0U; i < kAdcFilterWindow; ++i) {
        s_runtime.temperature_samples[i] = s_runtime.sample.temperature;
        s_runtime.vdda_samples[i] = s_runtime.sample.vdda;
        s_runtime.vin_adc_samples[i] = s_runtime.sample.vin_adc_v;
    }
}

/**
 * @brief 执行当前所有已置位的待处理任务。
 * @param[in]  无。
 * @param[out] 无。
 * @retval    无返回值。
 * @note      先原子取出任务位图；若当前没有待处理任务则直接返回。
 */
void TaskRunner::runPending()
{
    Watchdog::feed();

    /* poll() 含按铃超时看门狗(cancelRing)，必须每轮无条件执行。
     * 放在 canFeedbackPushStep() 前确保卡死的按铃先被清掉再推送下一帧。 */
    AppSpiCommandBus.poll();

    /* 0xB0 进 BOOT：必须每圈检查，不能只靠 1Hz TASK_HEARTBEAT。
     * 否则 Flash 写/复位被拖到下一秒，ESP32 HELLO 一直 role=app 超时。 */
    if (OtaBootRequest::isRebootPending()) {
        OtaBootRequest::process();
    }

    /* 电机 OTA relay：活跃时每圈推进状态机（等 ACK / 超时 / 发下一包）。
     * 非活跃时立即返回，开销可忽略。 */
    MotorOtaRelay::process();
    /* 数据发送推进：主循环每圈调用，按 SysTick 钟表控制每包间隔
     * （替代原靠 tick100us 计数 + TASK_MOTOR_OTA_TX 的方式，消除计数器竞争）。 */
    MotorOtaRelay::onTxTick();

    /* CAN 反馈主动推送：GD32 APP 循环推送 3 通道 × 2 页反馈帧到 ESP32，
     * 替代 ESP32 主动轮询。非阻塞：上一帧未读走时立即返回。
     * 每次主循环迭代调用一次，ESP32 读走一帧后 CS 释放中断唤醒主循环继续推送。 */
    canFeedbackPushStep();
    /* 多电机状态（0xC5）：优先级介于 CAN 反馈与 ADC 之间；ring 忙则跳过。 */
    motorStatePushStep();
    /* ADC 调试反馈（0xC4）：优先级低于 CAN；ring 忙则跳过。可 config 关闭。 */
    adcFeedbackPushStep();
    /* 多电机版本（0xC6）：事件驱动、低频，优先级最低；无 dirty 版本时立即返回。 */
    motorVersionPushStep();

    /* 周期路径( SYNC0 ISR preempt=0 )用 s_periodicSeq 序列号通知主循环,
     * 不再走 task_flags,避免 BASEPRI 临界区与 preempt=0 的 RMW 竞态。
     * 优先于 takePending() 处理,确保周期数据及时消费。
     * ESC 未焊接时 selfTest 失败,isEscReady()=false,中断未使能,
     * s_periodicSeq 不会递增,此分支自然不触发——双重守卫仅为防御性编程。 */
    if (EthercatSlave::isEscReady() && EthercatSlave::hasPendingPeriodic()) {
        EthercatSlave::markPeriodicProcessed();
        taskEthercatProcess();
    }

    const uint32_t pending = TaskManager::takePending();
    if(pending == 0U) return;

    if(pending & (1UL << TASK_ADC_VREF_SAMPLE))  taskAdcVrefSample();
    if(pending & (1UL << TASK_ADC_TEMP_SAMPLE))  taskAdcTempSample();
    if(pending & (1UL << TASK_ADC_PROCESS))      taskAdcProcess();
    if(pending & (1UL << TASK_CAN_RX_PARSE))     taskCanRxParse();
    if(pending & (1UL << TASK_CAN_TX_SEND))      taskCanTxSend();
    if(pending & (1UL << TASK_HEARTBEAT))        taskHeartbeat();
    if(pending & (1UL << TASK_MOTOR_STATE_PUSH)) taskMotorStatePush();
    if(pending & (1UL << TASK_DIAGNOSTIC_PRINT)) taskDiagnosticPrint();
    if(pending & (1UL << TASK_ETHERCAT_PROCESS)) taskEthercatProcess();
}