Odrive源码分析(六) 相关控制变量传递

本文记录下odrive源代码中相关控制模块之间变量的传递，这对理解odrive源代码至关重要。

通过前面文字的分析，odrive有两条数据链路，一条是通过中断进行实时的控制，另外一条是OS相关的操作，主要分析下中断内部的相关变量传递。

中断函数：TIM8_UP_TIM13_IRQHandler,，此中断又触发了软中断ControlLoop_IRQHandler。在ControlLoop_IRQHandler中，会进行数据的更新，主要是调用：odrv.control_loop_cb。

下面分析odrv.control_loop_cb函数内部进行的数据更新操作：

void ODrive::control_loop_cb(uint32_t timestamp) {
    last_update_timestamp_ = timestamp;
    n_evt_control_loop_++;

    // TODO: use a configurable component list for most of the following things

    MEASURE_TIME(task_times_.control_loop_misc) {
        // Reset all output ports so that we are certain about the freshness of
        // all values that we use.
        // If we forget to reset a value here the worst that can happen is that
        // this safety check doesn't work.
        // TODO: maybe we should add a check to output ports that prevents
        // double-setting the value.
        for (auto& axis: axes) {
            axis.acim_estimator_.slip_vel_.reset();
            axis.acim_estimator_.stator_phase_vel_.reset();
            axis.acim_estimator_.stator_phase_.reset();
            axis.controller_.torque_output_.reset();
            axis.encoder_.phase_.reset();
            axis.encoder_.phase_vel_.reset();
            axis.encoder_.pos_estimate_.reset();
            axis.encoder_.vel_estimate_.reset();
            axis.encoder_.pos_circular_.reset();
            axis.motor_.Vdq_setpoint_.reset();
            axis.motor_.Idq_setpoint_.reset();
            axis.open_loop_controller_.Idq_setpoint_.reset();
            axis.open_loop_controller_.Vdq_setpoint_.reset();
            axis.open_loop_controller_.phase_.reset();
            axis.open_loop_controller_.phase_vel_.reset();
            axis.open_loop_controller_.total_distance_.reset();
            axis.sensorless_estimator_.phase_.reset();
            axis.sensorless_estimator_.phase_vel_.reset();
            axis.sensorless_estimator_.vel_estimate_.reset();
        }

        uart_poll();
        odrv.oscilloscope_.update();
    }

    for (auto& axis : axes) {
        MEASURE_TIME(axis.task_times_.endstop_update) {
            axis.min_endstop_.update();
            axis.max_endstop_.update();
        }
    }

    MEASURE_TIME(task_times_.control_loop_checks) {
        for (auto& axis: axes) {
            // look for errors at axis level and also all subcomponents
            bool checks_ok = axis.do_checks(timestamp);

            // make sure the watchdog is being fed. 
            bool watchdog_ok = axis.watchdog_check();

            if (!checks_ok || !watchdog_ok) {
                axis.motor_.disarm();
            }
        }
    }

    for (auto& axis: axes) {
        // Sub-components should use set_error which will propegate to this error_
        MEASURE_TIME(axis.task_times_.thermistor_update) {
            axis.motor_.fet_thermistor_.update();
            axis.motor_.motor_thermistor_.update();
        }

        MEASURE_TIME(axis.task_times_.encoder_update)
            axis.encoder_.update();
    }

    // Controller of either axis might use the encoder estimate of the other
    // axis so we process both encoders before we continue.

    for (auto& axis: axes) {
        MEASURE_TIME(axis.task_times_.sensorless_estimator_update)
            axis.sensorless_estimator_.update();

        MEASURE_TIME(axis.task_times_.controller_update) {
            if (!axis.controller_.update()) { // uses position and velocity from encoder
                axis.error_ |= Axis::ERROR_CONTROLLER_FAILED;
            }
        }

        MEASURE_TIME(axis.task_times_.open_loop_controller_update)
            axis.open_loop_controller_.update(timestamp);

        MEASURE_TIME(axis.task_times_.motor_update)
            axis.motor_.update(timestamp); // uses torque from controller and phase_vel from encoder

        MEASURE_TIME(axis.task_times_.current_controller_update)
            axis.motor_.current_control_.update(timestamp); // uses the output of controller_ or open_loop_contoller_ and encoder_ or sensorless_estimator_ or acim_estimator_
    }

    // Tell the axis threads that the control loop has finished
    for (auto& axis: axes) {
        if (axis.thread_id_) {
            osSignalSet(axis.thread_id_, 0x0001);
        }
    }

    get_gpio(odrv.config_.error_gpio_pin).write(odrv.any_error());
}

下面这一段reset，是让相关数据过期，防止下一周期使用到了旧的数据。

    MEASURE_TIME(task_times_.control_loop_misc) {
        // Reset all output ports so that we are certain about the freshness of
        // all values that we use.
        // If we forget to reset a value here the worst that can happen is that
        // this safety check doesn't work.
        // TODO: maybe we should add a check to output ports that prevents
        // double-setting the value.
        for (auto& axis: axes) {
            axis.acim_estimator_.slip_vel_.reset();
            axis.acim_estimator_.stator_phase_vel_.reset();
            axis.acim_estimator_.stator_phase_.reset();
            axis.controller_.torque_output_.reset();
            axis.encoder_.phase_.reset();
            axis.encoder_.phase_vel_.reset();
            axis.encoder_.pos_estimate_.reset();
            axis.encoder_.vel_estimate_.reset();
            axis.encoder_.pos_circular_.reset();
            axis.motor_.Vdq_setpoint_.reset();
            axis.motor_.Idq_setpoint_.reset();
            axis.open_loop_controller_.Idq_setpoint_.reset();
            axis.open_loop_controller_.Vdq_setpoint_.reset();
            axis.open_loop_controller_.phase_.reset();
            axis.open_loop_controller_.phase_vel_.reset();
            axis.open_loop_controller_.total_distance_.reset();
            axis.sensorless_estimator_.phase_.reset();
            axis.sensorless_estimator_.phase_vel_.reset();
            axis.sensorless_estimator_.vel_estimate_.reset();
        }
    }

这一段是更新限位状态

    for (auto& axis : axes) {
        MEASURE_TIME(axis.task_times_.endstop_update) {
            axis.min_endstop_.update();
            axis.max_endstop_.update();
        }
    }

各种报错检查

    MEASURE_TIME(task_times_.control_loop_checks) {
        for (auto& axis: axes) {
            // look for errors at axis level and also all subcomponents
            bool checks_ok = axis.do_checks(timestamp);

            // make sure the watchdog is being fed. 
            bool watchdog_ok = axis.watchdog_check();

            if (!checks_ok || !watchdog_ok) {
                axis.motor_.disarm();
            }
        }
    }

mos管温度和编码器位置值更新 这里关注的重点是编码器值的更新，因为后续会用到更新的值，是后续顺利进行的基础。

    for (auto& axis: axes) {
        // Sub-components should use set_error which will propegate to this error_
        MEASURE_TIME(axis.task_times_.thermistor_update) {
            axis.motor_.fet_thermistor_.update();
            axis.motor_.motor_thermistor_.update();
        }

        MEASURE_TIME(axis.task_times_.encoder_update)
            axis.encoder_.update();
    }

axis.encoder_.update()函数会更新encode里面以下几组重要的变量：

pos_estimate_counts_ ： 单位：编码器累计位置值.
pos_estimate_ ： 单位：圈数。
pos_cpr_counts_： 单位：编码器累计位置值，但是会控制在编码器分辨率范围内，比如0-4000。
vel_estimate_counts_： 单位：编码器变化速度。
phase_: 电角度。
phase_vel_： 电角度速度。

接下来调用

// Controller of either axis might use the encoder estimate of the other
// axis so we process both encoders before we continue.

for (auto& axis: axes) {
    MEASURE_TIME(axis.task_times_.sensorless_estimator_update)
        axis.sensorless_estimator_.update();

    MEASURE_TIME(axis.task_times_.controller_update) {
        if (!axis.controller_.update()) { // uses position and velocity from encoder
            axis.error_ |= Axis::ERROR_CONTROLLER_FAILED;
        }
    }

    MEASURE_TIME(axis.task_times_.open_loop_controller_update)
        axis.open_loop_controller_.update(timestamp);

    MEASURE_TIME(axis.task_times_.motor_update)
        axis.motor_.update(timestamp); // uses torque from controller and phase_vel from encoder

    MEASURE_TIME(axis.task_times_.current_controller_update)
        axis.motor_.current_control_.update(timestamp); // uses the output of controller_ or open_loop_contoller_ and encoder_ or sensorless_estimator_ or acim_estimator_
}

axis.controller_.update()会使用到encoder值对控制变量进行更新，在闭环开始时，已经对变量进行了链接：

controller_.pos_estimate_linear_src_.connect_to(&ax->encoder_.pos_estimate_);
controller_.vel_estimate_src_.connect_to(&ax->encoder_.vel_estimate_);

controller更新时会使用到外界给定的输入，比如位置控制会使用到input_pos_，速度控制会使用到input_vel__等。

然后计算后会对以下关键变量进行更新：

torque_output_ ： 力矩输出值

然后来到了最关键的更新函数: axis.motor_.update(timestamp);

此函数会用到controller更新时计算的变量torque_output_：

motor_.torque_setpoint_src_.connect_to(&controller_.torque_output_);

Motor::update会计算出以下重要的变量：

Idq_setpoint_ ： 电流环控制DQ
Vdq_setpoint_ ： 电压控制DQ.

接下来就是FOC的更新了，调用 axis.motor_.current_control_.update(timestamp)，实际上是调用的FieldOrientedController::update。

这里主要就是把上面计算过程中的变量传进FOC对象中：

void FieldOrientedController::update(uint32_t timestamp) {
    CRITICAL_SECTION() {
        ctrl_timestamp_ = timestamp;
        enable_current_control_ = enable_current_control_src_;
        Idq_setpoint_ = Idq_setpoint_src_.present();
        Vdq_setpoint_ = Vdq_setpoint_src_.present();
        phase_ = phase_src_.present();
        phase_vel_ = phase_vel_src_.present();
    }
}

到了这里，基本的计算就结束了（odrv.control_loop_cb函数）.

上面的计算完成后，接下来就是相电流标定和SVPWM更新了。

motors[0].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1) - TIM1_INIT_COUNT, current0);
motors[1].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1), current1);

motors[0].pwm_update_cb(timestamp + 3 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1) - TIM1_INIT_COUNT);
motors[1].pwm_update_cb(timestamp + 3 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1));

综上分析：基本上外界（上位机）传送目标位置或速度或力矩到controller模块，controller模块计算成变量torque，然后由motor模块计到DQ空间，之后就可以用FOC的控制流程进行反park变换，最终通过SVPWM控制三相马鞍波形输出实现电机运转，电机运转时又通过ADC采集电流进行Clark变换和Park变换得到测量的DQ，通过controller模块的PID运算实现整个系统的闭环，整个系统还是非常严谨的。