Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 15:34:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

introduce InputPort and OutputPort, don't use NAN

Browse Source 
The InputPort/OutputPort infrastructure facilitates safer
data paths between components: OutputPorts store a value
and the age of the value measured in number of control loop
iterations. InputPorts can be connected to various sources,
for instance an OutputPort. InputPorts expose the values to
consumers in the form of std::optional to reflect the fact
that an InputPort can be dangling or connected to a stale
OutputPort.
This commit is contained in:
Samuel Sadok
2020-09-23 16:20:21 +02:00
parent 4b83470bda
commit 7fd0806d49
31 changed files with 833 additions and 519 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/foc.cpp
+104 -80
View File
@@ -3,26 +3,34 @@
#include <board.h>
Motor::Error AlphaBetaFrameController::on_measurement(
float vbus_voltage, std::array<float, 3> currents,
std::optional<float> vbus_voltage,
std::optional<std::array<float, 3>> currents,
uint32_t input_timestamp) {
// Clarke transform
float Ialpha = currents[0];
float Ibeta = one_by_sqrt3 * (currents[1] - currents[2]);
return on_measurement(vbus_voltage, Ialpha, Ibeta, input_timestamp);
std::optional<float2D> Ialpha_beta;
if (currents.has_value()) {
// Clarke transform
Ialpha_beta = {
(*currents)[0],
one_by_sqrt3 * ((*currents)[1] - (*currents)[2])
};
}
return on_measurement(vbus_voltage, Ialpha_beta, input_timestamp);
}
Motor::Error AlphaBetaFrameController::get_output(
uint32_t output_timestamp, float (&pwm_timings)[3], float* ibus) {
float mod_alpha = NAN;
float mod_beta = NAN;
Motor::Error status = get_alpha_beta_output(output_timestamp, &mod_alpha, &mod_beta, ibus);
uint32_t output_timestamp, float (&pwm_timings)[3],
std::optional<float>* ibus) {
std::optional<float2D> mod_alpha_beta;
Motor::Error status = get_alpha_beta_output(output_timestamp, &mod_alpha_beta, ibus);
if (status != Motor::ERROR_NONE) {
return status;
} else if (std::isnan(mod_alpha) || std::isnan(mod_alpha)) {
} else if (!mod_alpha_beta.has_value() || std::isnan(mod_alpha_beta->first) || std::isnan(mod_alpha_beta->second)) {
return Motor::ERROR_MODULATION_IS_NAN;
} else if (SVM(mod_alpha, mod_beta, &pwm_timings[0], &pwm_timings[1], &pwm_timings[2]) != 0) {
} else if (SVM(mod_alpha_beta->first, mod_alpha_beta->second, &pwm_timings[0], &pwm_timings[1], &pwm_timings[2]) != 0) {
return Motor::ERROR_MODULATION_MAGNITUDE;
}
@@ -32,27 +40,26 @@ Motor::Error AlphaBetaFrameController::get_output(
void FieldOrientedController::reset() {
v_current_control_integral_d_ = 0.0f;
v_current_control_integral_q_ = 0.0f;
vbus_voltage_measured_ = NAN;
Ialpha_measured_ = NAN;
Ibeta_measured_ = NAN;
vbus_voltage_measured_ = std::nullopt;
Ialpha_beta_measured_ = std::nullopt;
}
Motor::Error FieldOrientedController::on_measurement(
float vbus_voltage, float Ialpha, float Ibeta,
uint32_t input_timestamp) {
std::optional<float> vbus_voltage, std::optional<float2D> Ialpha_beta,
uint32_t input_timestamp) {
// Store the measurements for later processing.
i_timestamp_ = input_timestamp;
vbus_voltage_measured_ = vbus_voltage;
Ialpha_measured_ = Ialpha;
Ibeta_measured_ = Ibeta;
Ialpha_beta_measured_ = Ialpha_beta;
return Motor::ERROR_NONE;
}
ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output(
uint32_t output_timestamp, float* mod_alpha, float* mod_beta, float* ibus) {
uint32_t output_timestamp, std::optional<float2D>* mod_alpha_beta,
std::optional<float>* ibus) {
if (std::isnan(vbus_voltage_measured_) || std::isnan(Ialpha_measured_) || std::isnan(Ibeta_measured_)) {
if (!vbus_voltage_measured_.has_value() || !Ialpha_beta_measured_.has_value()) {
// FOC didn't receive a current measurement yet.
return Motor::ERROR_CONTROLLER_INITIALIZING;
} else if (abs((int32_t)(i_timestamp_ - ctrl_timestamp_)) > MAX_CONTROL_LOOP_UPDATE_TO_CURRENT_UPDATE_DELTA) {
@@ -64,56 +71,66 @@ ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output(
// rate than current sensor updates. In this case we can reuse mod_d and
// mod_q from a previous iteration.
// Fetch member variables into local variables to make the optimizer's life easier.
float vbus_voltage = vbus_voltage_measured_;
float Ialpha = Ialpha_measured_;
float Ibeta = Ibeta_measured_;
float Vd = Vd_setpoint_;
float Vq = Vq_setpoint_;
float Id_setpoint = Id_setpoint_;
float Iq_setpoint = Iq_setpoint_;
float phase = phase_;
float phase_vel = phase_vel_;
if (std::isnan(phase) || std::isnan(phase_vel)) {
return Motor::ERROR_UNKNOWN_PHASE;
}
// Park transform
float I_phase = phase + phase_vel * ((float)(int32_t)(i_timestamp_ - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
float c_I = our_arm_cos_f32(I_phase);
float s_I = our_arm_sin_f32(I_phase);
float Id = c_I * Ialpha + s_I * Ibeta;
float Iq = c_I * Ibeta - s_I * Ialpha;
Iq_measured_ += I_measured_report_filter_k_ * (Iq - Iq_measured_);
Id_measured_ += I_measured_report_filter_k_ * (Id - Id_measured_);
// Current error
float Ierr_d = Id_setpoint - Id;
float Ierr_q = Iq_setpoint - Iq;
if (enable_current_control_) {
// Check for current sense saturation
if (std::isnan(Ierr_d) || std::isnan(Ierr_q)) {
return Motor::ERROR_UNKNOWN_CURRENT;
}
// Apply PI control (V{d,q}_setpoint act as feed-forward terms in this mode)
Vd += v_current_control_integral_d_ + Ierr_d * p_gain_;
Vq += v_current_control_integral_q_ + Ierr_q * p_gain_;
}
if (std::isnan(vbus_voltage)) {
if (!Vdq_setpoint_.has_value()) {
return Motor::ERROR_UNKNOWN_VOLTAGE_COMMAND;
} else if (!phase_.has_value() || !phase_vel_.has_value()) {
return Motor::ERROR_UNKNOWN_PHASE_ESTIMATE;
} else if (!vbus_voltage_measured_.has_value()) {
return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE;
}
auto [Vd, Vq] = *Vdq_setpoint_;
float phase = *phase_;
float phase_vel = *phase_vel_;
float vbus_voltage = *vbus_voltage_measured_;
std::optional<float2D> Idq;
// Park transform
if (Ialpha_beta_measured_.has_value()) {
auto [Ialpha, Ibeta] = *Ialpha_beta_measured_;
float I_phase = phase + phase_vel * ((float)(int32_t)(i_timestamp_ - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
float c_I = our_arm_cos_f32(I_phase);
float s_I = our_arm_sin_f32(I_phase);
Idq = {
c_I * Ialpha + s_I * Ibeta,
c_I * Ibeta - s_I * Ialpha
};
Id_measured_ += I_measured_report_filter_k_ * (Idq->first - Id_measured_);
Iq_measured_ += I_measured_report_filter_k_ * (Idq->second - Iq_measured_);
} else {
Id_measured_ = 0.0f;
Iq_measured_ = 0.0f;
}
float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
float V_to_mod = 1.0f / mod_to_V;
float mod_d = V_to_mod * Vd;
float mod_q = V_to_mod * Vq;
float mod_d;
float mod_q;
if (enable_current_control_) {
// Current control mode
if (!pi_gains_.has_value()) {
return Motor::ERROR_UNKNOWN_GAINS;
} else if (!Idq.has_value()) {
return Motor::ERROR_UNKNOWN_CURRENT_MEASUREMENT;
} else if (!Idq_setpoint_.has_value()) {
return Motor::ERROR_UNKNOWN_CURRENT_COMMAND;
}
auto [p_gain, i_gain] = *pi_gains_;
auto [Id, Iq] = *Idq;
auto [Id_setpoint, Iq_setpoint] = *Idq_setpoint_;
float Ierr_d = Id_setpoint - Id;
float Ierr_q = Iq_setpoint - Iq;
// Apply PI control (V{d,q}_setpoint act as feed-forward terms in this mode)
mod_d = V_to_mod * (Vd + v_current_control_integral_d_ + Ierr_d * p_gain);
mod_q = V_to_mod * (Vq + v_current_control_integral_q_ + Ierr_q * p_gain);
// Vector modulation saturation, lock integrator if saturated
// TODO make maximum modulation configurable
float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
@@ -124,25 +141,34 @@ ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output(
v_current_control_integral_d_ *= 0.99f;
v_current_control_integral_q_ *= 0.99f;
} else {
v_current_control_integral_d_ += Ierr_d * (i_gain_ * current_meas_period);
v_current_control_integral_q_ += Ierr_q * (i_gain_ * current_meas_period);
v_current_control_integral_d_ += Ierr_d * (i_gain * current_meas_period);
v_current_control_integral_q_ += Ierr_q * (i_gain * current_meas_period);
}
} else {
// Voltage control mode
mod_d = V_to_mod * Vd;
mod_q = V_to_mod * Vq;
}
// Inverse park transform
float pwm_phase = phase_ + phase_vel_ * ((float)(int32_t)(output_timestamp - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
float pwm_phase = phase + phase_vel * ((float)(int32_t)(output_timestamp - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
float c_p = our_arm_cos_f32(pwm_phase);
float s_p = our_arm_sin_f32(pwm_phase);
float mod_alpha_temp = c_p * mod_d - s_p * mod_q;
float mod_beta_temp = c_p * mod_q + s_p * mod_d;
float mod_alpha = c_p * mod_d - s_p * mod_q;
float mod_beta = c_p * mod_q + s_p * mod_d;
// Report final applied voltage in stationary frame (for sensorless estimator)
final_v_alpha_ = mod_to_V * mod_alpha_temp;
final_v_beta_ = mod_to_V * mod_beta_temp;
final_v_alpha_ = mod_to_V * mod_alpha;
final_v_beta_ = mod_to_V * mod_beta;
*mod_alpha = mod_alpha_temp;
*mod_beta = mod_beta_temp;
*ibus = mod_d * Id + mod_q * Iq;
*mod_alpha_beta = {mod_alpha, mod_beta};
if (Idq.has_value()) {
auto [Id, Iq] = *Idq;
*ibus = mod_d * Id + mod_q * Iq;
}
return Motor::ERROR_NONE;
}
@@ -150,11 +176,9 @@ void FieldOrientedController::update(uint32_t timestamp) {
CRITICAL_SECTION() {
ctrl_timestamp_ = timestamp;
enable_current_control_ = enable_current_control_src_;
Id_setpoint_ = Id_setpoint_src_ ? *Id_setpoint_src_ : NAN;
Iq_setpoint_ = Iq_setpoint_src_ ? *Iq_setpoint_src_ : NAN;
Vd_setpoint_ = Vd_setpoint_src_ ? *Vd_setpoint_src_ : NAN;
Vq_setpoint_ = Vq_setpoint_src_ ? *Vq_setpoint_src_ : NAN;
phase_ = phase_src_ ? *phase_src_ : NAN;
phase_vel_ = phase_vel_src_ ? *phase_vel_src_ : NAN;
Idq_setpoint_ = Idq_setpoint_src_.get_current();
Vdq_setpoint_ = Vdq_setpoint_src_.get_current();
phase_ = phase_src_.get_current();
phase_vel_ = phase_vel_src_.get_current();
}
}