add coherent sampling and 1.5 cycle pwm phase advance

madcowswe · Feb 2, 2019 · a86fb3e59b2df134b9ece10717ce100f40c9ba61 · a86fb3e
1 parent 7384f2a
commit a86fb3e59b2df134b9ece10717ce100f40c9ba61
diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
@@ -148,7 +148,7 @@ bool Axis::run_sensorless_spin_up() {
        float phase = wrap_pm_pi(config_.ramp_up_distance * x);
        float I_mag = config_.spin_up_current * x;
        x += current_meas_period / config_.ramp_up_time;
-  
-        if (!motor_.update(I_mag, phase))
+  
+        if (!motor_.update(I_mag, phase, 0.0f))
            return error_ |= ERROR_MOTOR_FAILED, false;
        return x < 1.0f;
    });
@@ -162,7 +162,7 @@ bool Axis::run_sensorless_spin_up() {
        vel += config_.spin_up_acceleration * current_meas_period;
        phase = wrap_pm_pi(phase + vel * current_meas_period);
        float I_mag = config_.spin_up_current;
-  
-        if (!motor_.update(I_mag, phase))
+  
+        if (!motor_.update(I_mag, phase, vel))
            return error_ |= ERROR_MOTOR_FAILED, false;
        return vel < config_.spin_up_target_vel;
    });
@@ -184,7 +184,7 @@ bool Axis::run_sensorless_control_loop() {
        float current_setpoint;
        if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
            return error_ |= ERROR_CONTROLLER_FAILED, false;
-  
-        if (!motor_.update(current_setpoint, sensorless_estimator_.phase_))
+  
+        if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
            return false; // set_error should update axis.error_
        return true;
    });
@@ -200,7 +200,8 @@ bool Axis::run_closed_loop_control_loop() {
        float current_setpoint;
        if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
            return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
-  
-        if (!motor_.update(current_setpoint, encoder_.phase_))
+  
+        float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
+  
+        if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
            return false; // set_error should update axis.error_
        return true;
    });

diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
@@ -252,14 +252,35 @@ void Encoder::update_pll_gains() {
    }
 }
+  
+void Encoder::sample_now() {
+  
+    switch (config_.mode) {
+  
+        case MODE_INCREMENTAL: {
+  
+            tim_cnt_sample_ = (int16_t)hw_config_.timer->Instance->CNT;
+  
+        } break;
+  
+
+  
+        case MODE_HALL: {
+  
+            // do nothing: samples already captured in general GPIO capture
+  
+        } break;
+  
+
+  
+        case MODE_SINCOS: {
+  
+            sincos_sample_s_ = get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f;
+  
+            sincos_sample_c_ = get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f;
+  
+        } break;
+  
+
+  
+        default: {
+  
+           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
+  
+        } break;
+  
+    }
+  
+}
+  
+
 bool Encoder::update() {
    // update internal encoder state.
    int32_t delta_enc = 0;
    switch (config_.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)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
+  
+            int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
            delta_enc = (int32_t)delta_enc_16; //sign extend
        } break;
@@ -279,10 +300,7 @@ bool Encoder::update() {
        } break;
        case MODE_SINCOS: {
-  
-            float c = get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f;
-  
-            float s = get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f;
-  
-
-  
-            float phase = fast_atan2(s, c);
+  
+            float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
            int fake_count = (int)(1000.0f * phase);
            //CPR = 6283 = 2pi * 1k

diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp
@@ -57,6 +57,7 @@ class Encoder {
    bool run_index_search();
    bool run_offset_calibration();
+  
+    void sample_now();
    bool update();
    void update_pll_gains();
@@ -78,8 +79,11 @@ class Encoder {
    float pll_kp_ = 0.0f;   // [count/s / count]
    float pll_ki_ = 0.0f;   // [(count/s^2) / count]
+  
+    int16_t tim_cnt_sample_ = 0; // 
    // Updated by low_level pwm_adc_cb
    uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
+  
+    float sincos_sample_s_ = 0.0f;
+  
+    float sincos_sample_c_ = 0.0f;
    // Communication protocol definitions
    auto make_protocol_definitions() {

diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp
@@ -539,6 +539,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
            axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
        }
        // Prepare hall readings
+  
+        // TODO move this to inside encoder update function
        decode_hall_samples(axis.encoder_, GPIO_port_samples[axis_num]);
        // Trigger axis thread
        axis.signal_current_meas();
@@ -553,18 +554,30 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
 }
 void tim_update_cb(TIM_HandleTypeDef* htim) {
-  
-    int portsamples_arr;
+  
+
+  
+    // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
+  
+    // If we are counting down, we just sampled in SVM vector 7, with zero current
+  
+    bool counting_down = htim->Instance->CR1 & TIM_CR1_DIR;
+  
+    if (counting_down)
+  
+        return;
+  
+
+  
+    int sample_ch;
+  
+    Axis* axis;
    if (htim == &htim1) {
-  
-        portsamples_arr = 0;
+  
+        sample_ch = 0;
+  
+        axis = axes[0];
    } else if (htim == &htim8) {
-  
-        portsamples_arr = 1;
+  
+        sample_ch = 1;
+  
+        axis = axes[1];
    } else {
        low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK);
        return;
    }
+  
+    axis->encoder_.sample_now();
+  
+
    for (int i = 0; i < num_GPIO; ++i) {
-  
-        GPIO_port_samples[portsamples_arr][i] = GPIOs_to_samp[i]->IDR;
+  
+        GPIO_port_samples[sample_ch][i] = GPIOs_to_samp[i]->IDR;
    }
 }

diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
@@ -323,15 +323,15 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
 }
 // We should probably make FOC Current call FOC Voltage to avoid duplication.
-  
-bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
-  
-    float c = our_arm_cos_f32(phase);
-  
-    float s = our_arm_sin_f32(phase);
+  
+bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
+  
+    float c = our_arm_cos_f32(pwm_phase);
+  
+    float s = our_arm_sin_f32(pwm_phase);
    float v_alpha = c*v_d - s*v_q;
    float v_beta  = c*v_q + s*v_d;
    return enqueue_voltage_timings(v_alpha, v_beta);
 }
-  
-bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
+  
+bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase) {
    // Syntactic sugar
    CurrentControl_t& ictrl = current_control_;
@@ -349,11 +349,12 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
    float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
    // Park transform
-  
-    float c = our_arm_cos_f32(phase);
-  
-    float s = our_arm_sin_f32(phase);
-  
-    float Id = c * Ialpha + s * Ibeta;
-  
-    float Iq = c * Ibeta - s * Ialpha;
-  
-    ictrl.Iq_measured = Iq;
+  
+    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;
+  
+    ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured);
+  
+    ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured);
    // Current error
    float Ierr_d = Id_des - Id;
@@ -387,8 +388,10 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
    ictrl.Ibus = mod_d * Id + mod_q * Iq;
    // Inverse park transform
-  
-    float mod_alpha = c * mod_d - s * mod_q;
-  
-    float mod_beta  = c * mod_q + s * mod_d;
+  
+    float c_p = our_arm_cos_f32(pwm_phase);
+  
+    float s_p = our_arm_sin_f32(pwm_phase);
+  
+    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 sensorles estimator)
    ictrl.final_v_alpha = mod_to_V * mod_alpha;
@@ -403,19 +406,22 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
 }
-  
-bool Motor::update(float current_setpoint, float phase) {
+  
+bool Motor::update(float current_setpoint, float phase, float phase_vel) {
    current_setpoint *= config_.direction;
    phase *= config_.direction;
+  
+    phase_vel *= config_.direction;
+  
+
+  
+    float pwm_phase = phase + 1.5f * current_meas_period * phase_vel;
    // Execute current command
    // TODO: move this into the mot
    if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
-  
-        if(!FOC_current(0.0f, current_setpoint, phase)){
+  
+        if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){
            return false;
        }
    } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
        //In gimbal motor mode, current is reinterptreted as voltage.
-  
-        if(!FOC_voltage(0.0f, current_setpoint, phase))
+  
+        if(!FOC_voltage(0.0f, current_setpoint, pwm_phase))
            return false;
    } else {
        set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE);

diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp
@@ -47,6 +47,8 @@ class Motor {
        float final_v_beta; // [V]
        float Iq_setpoint; // [A]
        float Iq_measured; // [A]
+  
+        float Id_measured; // [A]
+  
+        float I_measured_report_filter_k;
        float max_allowed_current; // [A]
        float overcurrent_trip_level; // [A]
    };
@@ -119,9 +121,9 @@ class Motor {
    bool run_calibration();
    bool enqueue_modulation_timings(float mod_alpha, float mod_beta);
    bool enqueue_voltage_timings(float v_alpha, float v_beta);
-  
-    bool FOC_voltage(float v_d, float v_q, float phase);
-  
-    bool FOC_current(float Id_des, float Iq_des, float phase);
-  
-    bool update(float current_setpoint, float phase);
+  
+    bool FOC_voltage(float v_d, float v_q, float pwm_phase);
+  
+    bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase);
+  
+    bool update(float current_setpoint, float phase, float phase_vel);
    const MotorHardwareConfig_t& hw_config_;
    const GateDriverHardwareConfig_t gate_driver_config_;
@@ -160,6 +162,8 @@ class Motor {
        .final_v_beta = 0.0f,
        .Iq_setpoint = 0.0f,
        .Iq_measured = 0.0f,
+  
+        .Id_measured = 0.0f,
+  
+        .I_measured_report_filter_k = 1.0f,
        .max_allowed_current = 0.0f,
        .overcurrent_trip_level = 0.0f,
    };
@@ -190,6 +194,8 @@ class Motor {
                make_protocol_property("final_v_beta", &current_control_.final_v_beta),
                make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
                make_protocol_property("Iq_measured", &current_control_.Iq_measured),
+  
+                make_protocol_property("Id_measured", &current_control_.Id_measured),
+  
+                make_protocol_property("I_measured_report_filter_k", &current_control_.I_measured_report_filter_k),
                make_protocol_ro_property("max_allowed_current", &current_control_.max_allowed_current),
                make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level)
            ),

diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py
@@ -106,6 +106,7 @@ def did_close(evt):
        while not cancellation_token.is_set():
            plt.clf()
            plt.plot(vals)
+  
+            plt.legend(list(range(len(vals))))
            fig.canvas.draw()
            fig.canvas.start_event_loop(1/plot_rate)