Odrive0.5.1-FOC电机控制 encoder.cpp代码实现

最新推荐文章于 2025-06-09 16:34:33 发布

嵌入式crafter

最新推荐文章于 2025-06-09 16:34:33 发布

阅读量627

点赞数 20

分类专栏： FOC电机驱动-Odrive核心代码讲解文章标签：嵌入式硬件单片机 arm开发学习笔记算法

本文链接：https://blog.csdn.net/MOS_JBET/article/details/147153320

版权

FOC电机驱动-Odrive核心代码讲解专栏收录该内容

13 篇文章

订阅专栏

1.setup()

void Encoder::setup() {
    HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL); //启动定时器的编码器所有接口
    set_idx_subscribe(); //设置 index 事件的订阅回调（通常用于捕捉编码器 index 脉冲）

    mode_ = config_.mode;
    if(mode_ & MODE_FLAG_ABS){ //MODE_FLAG_ABS 是一个掩码，表示这是一个“绝对编码器”模式（如 SPI、SSI 等）
        abs_spi_cs_pin_init();
        abs_spi_init();
        if (axis_->controller_.config_.anticogging.pre_calibrated) {
            axis_->controller_.anticogging_valid_ = true;
        }
    }
}

2.enc_index_cb()

这段 Encoder::enc_index_cb() 是编码器 Index 中断回调的核心，它的主要任务是响应 Index 脉冲，并完成位置归零、状态标记和抗齿槽力补偿使能。

void Encoder::enc_index_cb() {
    if (config_.use_index) {
        set_circular_count(0, false); //将内部圆形（modulo）位置计数器清零
        if (config_.zero_count_on_find_idx)
            set_linear_count(0); // 性计数器也归零
        if (config_.pre_calibrated) { //判断是否预校准
            is_ready_ = true;
            if(axis_->controller_.config_.anticogging.pre_calibrated){
                axis_->controller_.anticogging_valid_ = true;
            }
        } else {
            is_ready_ = false;
        }
        index_found_ = true;
    }

    GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin); //退订中断回调（Index 只需要捕获一次）
}

3.set_idx_subscribe(bool override_enable)

void Encoder::set_idx_subscribe(bool override_enable) {
    //用户启用了 Index 功能,且 override_enable == true（强制启用）,find_idx_on_lockin_only == false（表示允许在任意时刻订阅）
    if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) {
        GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN,
                enc_index_cb_wrapper, this);
    } else if (!config_.use_index || config_.find_idx_on_lockin_only) {
        GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
    }
}

4.update_pll_gains()

用于配置位置跟踪 PLL（Phase-Locked Loop）的比例积分增益。

void Encoder::update_pll_gains() {
    pll_kp_ = 2.0f * config_.bandwidth;  //比例项，控制响应速度
    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); //积分项，增强低频响应，0.25 倍平方来自临界阻尼（critically damped）二阶系统设计

    // 防止数值不稳定。由于是离散系统，T * Kp < 1 近似等价于采样时间太大或带宽太高将导致离散控制器发散。
    if (!(current_meas_period * pll_kp_ < 1.0f)) {
        set_error(ERROR_UNSTABLE_GAIN);
    }
}

5.set_linear_count(int32_t count)

用于设置 线性（绝对）位置估计

void Encoder::set_linear_count(int32_t count) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = cpu_enter_critical();

    // Update states
    shadow_count_ = count;
    pos_estimate_counts_ = (float)count;
    tim_cnt_sample_ = count;

    //Write hardware last
    hw_config_.timer->Instance->CNT = count;

    cpu_exit_critical(prim);
}

6.set_circular_count(int32_t count, bool update_offset)

这段代码 Encoder::set_circular_count() 是 ODrive 编码器逻辑中一个 关键函数，它设置 编码器在一个机械周期（CPR）内的位置，并根据需要 更新 offset 校准值。

void Encoder::set_circular_count(int32_t count, bool update_offset) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = cpu_enter_critical();

    if (update_offset) {
        config_.offset += count - count_in_cpr_; //count - count_in_cpr_ 表示“当前位置与上一次位置的差值”,加到当前 offset 上去，就相当于把原始测量值重新对齐到这个 count 上
        config_.offset = mod(config_.offset, config_.cpr); //最后对 offset 做模运算，确保它始终在 [0, cpr) 范围内。
    }

    // Update states
    count_in_cpr_ = mod(count, config_.cpr); //将目标 count 映射到一个完整的周期 [0, CPR) 内
    pos_cpr_counts_ = (float)count_in_cpr_; //同时存一个浮点版本 pos_cpr_counts_，方便控制器内部计算（比如速度估计）

    cpu_exit_critical(prim);
}

7.run_index_search()

转动电机直到触发编码器 Index 脉冲，从而确定当前位置是一个完整转数的起点。

bool Encoder::run_index_search() {
    config_.use_index = true; //开启 Index 检测功能
    index_found_ = false; //清空状态，标记“还没找到 Index”
    //如果允许双向搜索，并且当前方向没设定，那就默认转正方向。
    if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
        axis_->motor_.config_.direction = 1;
    }
    set_idx_subscribe(); //开启 GPIO 中断，订阅 Index 脉冲（也就是 enc_index_cb()）

    //调用电机控制逻辑，让电机按 lockin 策略缓慢加速旋转，直到触发 Index 或达到预设条件。
    bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin); 
    return status;
}

8.run_direction_find()

判断电机实际正方向是不是跟编码器增计数方向一致。

bool Encoder::run_direction_find() {
    int32_t init_enc_val = shadow_count_;
    axis_->motor_.config_.direction = 1; // 必须先按正方向测试

    Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
    lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
    lockin_config.finish_on_distance = true;
    lockin_config.finish_on_enc_idx = false;
    lockin_config.finish_on_vel = false;
    bool status = axis_->run_lockin_spin(lockin_config); //跑 lockin（电机缓慢加速 3 秒）

    if (status) {
        // Check response and direction
        if (shadow_count_ > init_enc_val + 8) {
            // motor same dir as encoder
            axis_->motor_.config_.direction = 1;
        } else if (shadow_count_ < init_enc_val - 8) {
            // motor opposite dir as encoder
            axis_->motor_.config_.direction = -1;
        } else {
            axis_->motor_.config_.direction = 0;
        }
    }

    return status;
}

9.run_offset_calibration()

这个 Encoder::run_offset_calibration() 函数是 ODrive 编码器偏移量校准（Offset Calibration）的核心实现逻辑。在整个编码器初始化流程中，它确保我们能对准电机电角度的“0 点”，使编码器计数和电机控制坐标系完美同步。

bool Encoder::run_offset_calibration() {
    const float start_lock_duration = 1.0f;
    const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);

    // 检查是否找到 Index → 不然报错返回
    if (config_.use_index && !index_found_) {
        set_error(ERROR_INDEX_NOT_FOUND_YET);
        return false;
    }

    // Sync shadow_count_ ← count_in_cpr_
    shadow_count_ = count_in_cpr_;

    //计算驱动电压:这是给电机“注入电压”时的幅值，跟你电机类型、配置有关.
    float voltage_magnitude;
    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
        voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
    else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
        voltage_magnitude = axis_->motor_.config_.calibration_current;
    else
        return false;

    // 初始锁定（居中）:电机锁定在 0 电角度，持续 1 秒钟，为后续扫描做准备（避免动中校准）
    int i = 0;
    axis_->run_control_loop([&](){
        if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f)) 
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
        return ++i < start_lock_duration * current_meas_hz;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    int32_t init_enc_val = shadow_count_;
    int64_t encvaluesum = 0;

    // 正向电角扫描:在电角度空间匀速旋转一次（扇区），采样 encoder 输出并累加
    i = 0;
    axis_->run_control_loop([&]() {
        float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
        float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;
        
        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    // 方向判断:很像 run_direction_find()，也能顺便帮你确定 motor 和 encoder 的相对方向
    if (shadow_count_ > init_enc_val + 8) {
        // motor same dir as encoder
        axis_->motor_.config_.direction = 1;
    } else if (shadow_count_ < init_enc_val - 8) {
        // motor opposite dir as encoder
        axis_->motor_.config_.direction = -1;
    } else {
        // Encoder response error
        set_error(ERROR_NO_RESPONSE);
        return false;
    }

    // CPR 验证,超重要逻辑：确认你设定的 CPR 和电机 极对数 是否一致
    // 防止你设错配置，比如把编码器 CPR 设成了 2048 实际是 8192
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
    float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
    calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
    if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
        set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
        return false;
    }

    // 反向电角扫描:与正向一样的过程，只是反方向扫回来
    i = 0;
    axis_->run_control_loop([&]() {
        float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
        float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;
        
        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    // 偏移计算:取中值作为编码器偏移位置（相对于电角度中心）
    // offset_float是子采样补偿（防止小误差导致 jitter）
    config_.offset = encvaluesum / (num_steps * 2);
    int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
    config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f;  // add 0.5 to center-align state to phase

    is_ready_ = true;
    return true;
}

**10.decode_hall(uint8_t hall_state, int32_t* hall_cnt)**

这个函数 decode_hall 是一个用于霍尔编码器（三相）解码的小工具函数。

它把 3 个霍尔传感器的数字信号（hall_state）解码成一个标准的位置序号（0~5），用于跟踪电机的当前电角度位置。

static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
    switch (hall_state) {
        case 0b001: *hall_cnt = 0; return true;
        case 0b011: *hall_cnt = 1; return true;
        case 0b010: *hall_cnt = 2; return true;
        case 0b110: *hall_cnt = 3; return true;
        case 0b100: *hall_cnt = 4; return true;
        case 0b101: *hall_cnt = 5; return true;
        default: return false;
    }
}

11.ams_parity、cui_parity、abs_spi_cb

这段代码是 ODrive 中绝对值编码器 SPI 数据读取与解析的核心逻辑。它涵盖了对 AMS, CUI, 和 RLS 三种 SPI 绝对编码器的支持，并在 DMA 读取完成回调中解析位置值、校验奇偶性、更新内部状态。

支持的编码器类型：

AMS（如 AS5047P）：需要偶校验 + 错误标志判断
CUI（如 AMT21）：需要特殊方式奇偶校验（返回 ~v & 3）
RLS（如 RM44）：无校验，右移2位获得有效数据

//经典的位运算方式，用于快速求 16 位值的奇偶校验（返回 1 表示奇数个 1）
uint8_t ams_parity(uint16_t v) {
    v ^= v >> 8;
    v ^= v >> 4;
    v ^= v >> 2;
    v ^= v >> 1;
    return v & 1;
}

//CUI 编码器奇偶校验方式特殊，返回值不是 1 位，而是 2 位：
uint8_t cui_parity(uint16_t v) {
    v ^= v >> 8;
    v ^= v >> 4;
    v ^= v >> 2;
    return ~v & 3;
}

void Encoder::abs_spi_cb(){
    HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);

    axis_->motor_.log_timing(TIMING_LOG_SPI_END);

    uint16_t pos;

    switch (mode_) {
        case MODE_SPI_ABS_AMS: {
            uint16_t rawVal = abs_spi_dma_rx_[0];
            pos = rawVal & 0x3fff; //第 14 位是 错误标志（1 = error) 第 15 位是 偶校验 低 14 位是实际角度
        } break;

        case MODE_SPI_ABS_CUI: {
            uint16_t rawVal = abs_spi_dma_rx_[0];
            // check if parity is correct
            if (cui_parity(rawVal)) {
                return;
            }
            pos = rawVal & 0x3fff; //低 14 位为角度值，其余为奇偶校验
        } break;

        case MODE_SPI_ABS_RLS: {
            uint16_t rawVal = abs_spi_dma_rx_[0];
            pos = (rawVal >> 2) & 0x3fff; //数据右移两位，直接取中间 14 位为角度
        } break;

        default: {
           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
           return;
        } break;
    }

    pos_abs_ = pos;
    abs_spi_pos_updated_ = true;
    if (config_.pre_calibrated) {
        is_ready_ = true;
    }
}

12.Encoder::update()

PLL（锁相环）

ODrive 中使用的是一个简化的一阶数字 PLL（锁相环）结构，核心任务是：利用编码器读数（离散跳变）估算连续相位；利用估算相位变化量，推算电机的角速度。

bool Encoder::update() {
    // update internal encoder state.
    int32_t delta_enc = 0;
    int32_t pos_abs_latched = pos_abs_; //LATCH

    switch (mode_) {
        case MODE_INCREMENTAL: { //增量式编码器，通过定时器计数更新位置
            //TODO: use count_in_cpr_ instead as shadow_count_ can overflow
            //or use 64 bit
            int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
            delta_enc = (int32_t)delta_enc_16; //sign extend
        } break;

        case MODE_HALL: { //霍尔编码器，通过解析霍尔状态切换位置
            int32_t hall_cnt;
            if (decode_hall(hall_state_, &hall_cnt)) {
                delta_enc = hall_cnt - count_in_cpr_;
                delta_enc = mod(delta_enc, 6);
                if (delta_enc > 3)
                    delta_enc -= 6;
            } else {
                if (!config_.ignore_illegal_hall_state) {
                    set_error(ERROR_ILLEGAL_HALL_STATE);
                    return false;
                }
            }
        } break;

        case MODE_SINCOS: { //正弦/余弦模拟信号编码器
            float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
            int fake_count = (int)(1000.0f * phase);
            //CPR = 6283 = 2pi * 1k

            delta_enc = fake_count - count_in_cpr_;
            delta_enc = mod(delta_enc, 6283);
            if (delta_enc > 6283/2)
                delta_enc -= 6283;
        } break;
        
        case MODE_SPI_ABS_RLS:
        case MODE_SPI_ABS_AMS:
        case MODE_SPI_ABS_CUI: 
        case MODE_SPI_ABS_AEAT: { //多种 SPI 接口的绝对式编码器（AMS、CUI、RLS、AEAT）
            if (abs_spi_pos_updated_ == false) {
                // Low pass filter the error
                spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
                if (spi_error_rate_ > 0.005f)
                    set_error(ERROR_ABS_SPI_COM_FAIL);
            } else {
                // Low pass filter the error
                spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
            }

            abs_spi_pos_updated_ = false;
            delta_enc = pos_abs_latched - count_in_cpr_; //LATCH
            delta_enc = mod(delta_enc, config_.cpr);
            if (delta_enc > config_.cpr/2) {
                delta_enc -= config_.cpr;
            }

        }break;
        default: {
           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
           return false;
        } break;
    }
    
    //计数更新流程
    shadow_count_ += delta_enc; //主增量位置，可能会溢出。
    count_in_cpr_ += delta_enc; 
    count_in_cpr_ = mod(count_in_cpr_, config_.cpr); //当前位置在一个 CPR 范围内的值（mod 之后）。

    if(mode_ & MODE_FLAG_ABS)
        count_in_cpr_ = pos_abs_latched;

    // 锁相环 PLL 滤波器:用于平滑位置和速度估计，关键参数为 pll_kp_、pll_ki_。
    // 如果速度估计很小，还会自动 snap 到 0，防止抖动。
    pos_estimate_counts_ += current_meas_period * vel_estimate_counts_; //整体位置估计（可能 > CPR，适合全局定位）
    pos_cpr_counts_      += current_meas_period * vel_estimate_counts_; //仅在 CPR 范围内的位置估计（周期性），用于电角度和控制
    
    // 计算位置误差（离散检测器）:比较预测位置与实际编码器值的差值。
    float delta_pos_counts = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_counts_));
    float delta_pos_cpr_counts = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_counts_));
    delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, 0.5f * (float)(config_.cpr));
    
    // PLL 反馈：修正位置和速度
    // 这是典型的 PI 控制结构，模仿锁相环的思想：位置偏差 -> 修正估计位置（P）,位置偏差积分 -> 修正速度估计（I）
    // 这样既能保证跟踪精度，也具备一定滤波特性，抗抖动。
    pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
    pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;
    pos_cpr_counts_ = fmodf_pos(pos_cpr_counts_, (float)(config_.cpr));
    vel_estimate_counts_ += current_meas_period * pll_ki_ * delta_pos_cpr_counts;
    bool snap_to_zero_vel = false;
    
    //慢速防抖 snap-to-zero
    if (std::abs(vel_estimate_counts_) < 0.5f * current_meas_period * pll_ki_) {
        vel_estimate_counts_ = 0.0f;
        snap_to_zero_vel = true;
    }

    // 导出估算值给控制器
    float pos_cpr_last = pos_cpr_;
    pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
    vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr;
    pos_cpr_= pos_cpr_counts_ / (float)config_.cpr;
    float delta_pos_cpr = wrap_pm(pos_cpr_ - pos_cpr_last, 0.5f);
    pos_circular_ += delta_pos_cpr;
    pos_circular_ = fmodf_pos(pos_circular_, axis_->controller_.config_.circular_setpoint_range);

    // 编码器插值（用于高分辨率控制):对于增量编码器，跳变是离散的（如每 1 tick 才有更新）
    // 插值 = 利用估计速度进行线性推测，补足跳变之间的空白
    int32_t corrected_enc = count_in_cpr_ - config_.offset;
    if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
        interpolation_ = 0.5f; //静止
    } else if (delta_enc > 0) {
        interpolation_ = 0.0f; //刚跳变（+1）
    } else if (delta_enc < 0) {
        interpolation_ = 1.0f; //刚跳变（-1）
    } else {
        interpolation_ += current_meas_period * vel_estimate_counts_;
        if (interpolation_ > 1.0f) interpolation_ = 1.0f;
        if (interpolation_ < 0.0f) interpolation_ = 0.0f;
    }
    float interpolated_enc = corrected_enc + interpolation_; //最后得到一个更“平滑”的 interpolated_enc 用于电角度等计算

    //电角度计算（电机控制核心）:电角度 phase_ 是控制电机所需的核心变量，使用插值后的编码器位置计算得到。
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
    float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
    phase_ = wrap_pm_pi(ph);

    vel_estimate_valid_ = true;
    pos_estimate_valid_ = true;
    return true;
}