Odrive0.5.1-FOC电机控制核心代码 Odrive motor.cpp(.hpp)代码实现（一）

本文链接：https://blog.youkuaiyun.com/MOS_JBET/article/details/147070157

00 前言

研究FOC电机控制核心代码 Odrive motor.cpp(.hpp)Foc代码实现

参考资料: 【自制FOC驱动器】深入浅出讲解FOC算法与SVPWM技术 - 知乎

01 bool Motor::arm()

这段代码是 ODrive 电机 PWM 驱动的 "预启动（Arming）" 逻辑，它的核心作用是准备 PWM 输出但不立即激活，确保电机在安全条件下启动。

bool Motor::arm() {

    // Reset controller states, integrators, setpoints, etc.
    axis_->controller_.reset(); // 重置位置/速度环控制器（如清空PID积分器）
    reset_current_control(); // 重置电流环控制器（如清空Iq/Id积分器）

    // Wait until the interrupt handler triggers twice. This gives
    // the control loop the correct time quota to set up modulation timings.
    if (!axis_->wait_for_current_meas())
        return axis_->error_ |= Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
    next_timings_valid_ = false;  // 标记"下一次PWM调制信号尚未准备好"
    safety_critical_arm_motor_pwm(*this); // 安全关键操作：解锁PWM硬件
    return true;
}

// 重置电流环控制器（如清空Iq/Id积分器）
void Motor::reset_current_control() {
    current_control_.v_current_control_integral_d = 0.0f;
    current_control_.v_current_control_integral_q = 0.0f;
    current_control_.acim_rotor_flux = 0.0f;
    current_control_.Ibus = 0.0f;
}

02 void Motor::update_current_controller_gains()

这段代码是 ODrive 电流环控制器参数（PID 增益）的自动调谐逻辑。它基于电机的相电阻（phase resistance）和相电感（phase inductance）动态计算电流环的比例增益（P gain）和积分增益（I gain）。这是 ODrive 实现高性能电流控制的关键部分。

根据电机参数（phase_inductance 和 phase_resistance）和控制带宽（current_control_bandwidth），自动计算电流环的 P 增益和 I 增益。这是一种基于模型的控制器调参（Model-Based Tuning），确保电流环的动态响应最优。

当电机相电阻（phase_resistance）或相电感（phase_inductance）发生变化时调用（例如温度变化导致电阻变化，或更换电机后参数不同）。

开发者提到未来可能支持手动触发调参或自动 Hook 机制（目前需外部调用）。

// @brief Tune the current controller based on phase resistance and inductance
// This should be invoked whenever one of these values changes.
// TODO: allow update on user-request or update automatically via hooks
void Motor::update_current_controller_gains() {
    // Calculate current control gains
    current_control_.p_gain = config_.current_control_bandwidth * config_.phase_inductance;
    float plant_pole = config_.phase_resistance / config_.phase_inductance;
    current_control_.i_gain = plant_pole * current_control_.p_gain;
}

关键变量解析

变量	物理意义	单位
config_.current_control_bandwidth	电流环控制带宽（目标闭环带宽）	rad/s
config_.phase_inductance	电机相电感（L）	H (Henry)
config_.phase_resistance	电机相电阻（R）	Ω (Ohm)
current_control_.p_gain	电流环比例增益（Kp）	V/A (或等效单位)
current_control_.i_gain	电流环积分增益（Ki）	V/(A·s)
plant_pole	电机电气系统极点（-R/L）	rad/s

03 void Motor::DRV8301_setup()

这段代码是 ODrive 中配置 DRV8301 栅极驱动器（Gate Driver）的关键部分，主要完成：

自动计算电流采样增益：根据硬件参数（如分流电阻阻值）和用户设定的电流范围，选择最优的 DRV8301 内部放大器增益。
配置过流保护（OCP）：设置硬件过流触发阈值，防止电机或驱动器损坏。
初始化 DRV8301 寄存器：通过 SPI 写入配置参数，确保栅极驱动器正常工作。

关键变量/参数

变量/参数	说明
hw_config_.shunt_conductance	分流电导（= 1 / 分流电阻阻值），单位：S（西门子）
config_.requested_current_range	用户设定的最大电流测量范围（如 ±75A）
max_output_swing = 1.35V	DRV8301 电流采样放大器的最大线性输出范围
kMargin = 0.90	安全裕量，避免放大器饱和
kTripMargin = 1.0	过流触发阈值相对于线性范围的倍数

解析都在代码注释中：

// @brief Set up the gate drivers
void Motor::DRV8301_setup() {
    // for reference:
    // 20V/V on 500uOhm gives a range of +/- 150A
    // 40V/V on 500uOhm gives a range of +/- 75A
    // 20V/V on 666uOhm gives a range of +/- 110A
    // 40V/V on 666uOhm gives a range of +/- 55A

    constexpr float kMargin = 0.90f; // 安全裕量，避免放大器饱和.通过合理选择增益和预留裕量，确保电流信号始终处于放大器的线性工作区间
    constexpr float kTripMargin = 1.0f; // 过流触发阈值相对于线性范围的倍数
    constexpr float max_output_swing = 1.35f; // DRV8301 电流采样放大器的最大线性输出范围
    float max_unity_gain_current = kMargin * max_output_swing * hw_config_.shunt_conductance; // [A]
    float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V]

    // DRV8301 支持的增益选项：10V/V、20V/V、40V/V、80V/V。
    std::array<std::pair<float, DRV8301_ShuntAmpGain_e>, 4> gain_choices = { 
        std::make_pair(10.0f, DRV8301_ShuntAmpGain_10VpV),
        std::make_pair(20.0f, DRV8301_ShuntAmpGain_20VpV),
        std::make_pair(40.0f, DRV8301_ShuntAmpGain_40VpV),
        std::make_pair(80.0f, DRV8301_ShuntAmpGain_80VpV)
    };

    // 反向查找（从大到小），选择 不大于理论值的最小增益（即保证测量范围 ≥ 用户需求）。
    auto gain_snap_down = std::lower_bound(gain_choices.crbegin(), gain_choices.crend(), requested_gain, 
    [](std::pair<float, DRV8301_ShuntAmpGain_e> pair, float val){
        return pair.first > val;
    });
    
    if(gain_snap_down == gain_choices.crend())
       --gain_snap_down;

    //电流采样的倒数增益（用于将 ADC 电压值转换为实际电流值）。 例如：若增益=20V/V，则phase_current_rev_gain_ = 1/20 = 0.05。
    phase_current_rev_gain_ = 1.0f / gain_snap_down->first;
    //电流控制器的最大允许电流（由硬件增益决定）。
    current_control_.max_allowed_current = max_unity_gain_current * phase_current_rev_gain_;
    // 硬件过流触发阈值（略高于max_allowed_current，防止误触发）。
    current_control_.overcurrent_trip_level = (kTripMargin / kMargin) * current_control_.max_allowed_current;

    // 配置 DRV8301 寄存器
    DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
    DRV8301_enable(&gate_driver_);
    DRV8301_setupSpi(&gate_driver_, local_regs);

    local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; //过流保护模式（此处设为锁存关闭，触发后需手动复位）。
    // Overcurrent set to approximately 150A at 100degC. This may need tweaking.
    local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; //过流阈值电压（0.73V 对应 MOSFET Vds 检测电平，约 150A 时触发）。
    local_regs->Ctrl_Reg_2.GAIN = gain_snap_down->second; //写入最终选择的放大器增益值。

    //通过 SPI 接口将配置写入 DRV8301，并读取回传值确保配置成功。
    local_regs->SndCmd = true;
    DRV8301_writeData(&gate_driver_, local_regs);
    local_regs->RcvCmd = true;
    DRV8301_readData(&gate_driver_, local_regs);
}

04 bool Motor::check_DRV_fault()

检查 DRV8301 是否触发硬件故障（如过流、过热、欠压等）。

通过 DRV8301_getFaultType() 可获取的具体故障可能包括：

GVDD_UV：栅极驱动电压欠压
PVDD_UV：电源电压欠压
OTSD：过热关断
OCP：过流保护触发
FETHA_OC/FETLA_OC：半桥高/低侧 MOSFET 过流

故障处理流程

硬件响应：DRV8301 检测到故障后，会自动关闭 PWM 输出（具体行为由 OC_MODE 配置决定，如锁存关闭或自动恢复）。

软件响应：check_DRV_fault() 检测到故障后，ODrive 会记录错误码（如 axis_->error_ |= Axis::ERROR_DRV_FAULT）。通常需要手动复位或重新初始化 DRV8301 才能恢复运行。

bool Motor::check_DRV_fault() {
    //TODO: make this pin configurable per motor ch
    GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin);
    if (nFAULT_state == GPIO_PIN_RESET) {
        // Update DRV Fault Code
        gate_driver_exported_.drv_fault = (GateDriverIntf::DrvFault)DRV8301_getFaultType(&gate_driver_);
        // Update/Cache all SPI device registers
        // DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
        // local_regs->RcvCmd = true;
        // DRV8301_readData(&gate_driver_, local_regs);
        return false;
    };
    return true;
}

05 void Motor::set_error(Motor::Error error)

这段代码是 ODrive 电机控制系统中错误处理的核心函数，用于设置电机错误状态并触发安全保护机制。

典型调用场景
- 过流保护触发时
- 温度过高时
- 通信超时或传感器故障时

void Motor::set_error(Motor::Error error){
    error_ |= error;
    axis_->error_ |= Axis::ERROR_MOTOR_FAILED; //记录错误类型到电机和轴（Axis）的错误寄存器。
    safety_critical_disarm_motor_pwm(*this); //立即安全关闭 PWM 输出（disarm）。
    update_brake_current(); //更新刹车电流状态（如启用动态制动）。
}