From fefa091de22bc8ad00292ff476a0b87a0f1c709d Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 19:17:31 -0500 Subject: [PATCH 1/9] Handle current scaling factor for HW versions <= 3.3 --- Firmware/MotorControl/low_level.c | 16 ++++++++++++++-- Firmware/MotorControl/utils.h | 1 + 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 961d4f7b..97ee3a9b 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -39,6 +39,8 @@ float vbus_voltage = 12.0f; #define POLE_PAIRS 7 static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); +#define CURRENT_SCALING_FACTOR (1.31578947f) + // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { @@ -619,7 +621,12 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } else { ADCValue = HAL_ADC_GetValue(hadc); } - float current = phase_current_from_adcval(motor, ADCValue); + if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ + float current = phase_current_from_adcval(motor, ADCValue)*CURRENT_SCALING_FACTOR; + } else { + float current = phase_current_from_adcval(motor, ADCValue); + } + if (current_meas_not_DC_CAL) { // ADC2 and ADC3 record the phB and phC currents concurrently, @@ -1293,7 +1300,12 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - float Ilim = motor->current_control.current_lim; + if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ + float Ilim = motor->current_control.current_lim*CURRENT_SCALING_FACTOR; + } else{ + float Ilim = motor->current_control.current_lim; + } + bool limited = false; if (Iq > Ilim) { limited = true; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 542b7dd7..1acfc657 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -70,6 +70,7 @@ #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +#define MACRO_CONSTRAIN(amt,low,high) (((amt)<(low)) ? (low) : ((amt > high) ? (high) : (amt))) // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform From 62be001ca29555283d50a4a231ce531d5e80253f Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 19:17:31 -0500 Subject: [PATCH 2/9] Revert "Handle current scaling factor for HW versions <= 3.3" This reverts commit fefa091de22bc8ad00292ff476a0b87a0f1c709d. --- Firmware/MotorControl/low_level.c | 16 ++-------------- Firmware/MotorControl/utils.h | 1 - 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 97ee3a9b..961d4f7b 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -39,8 +39,6 @@ float vbus_voltage = 12.0f; #define POLE_PAIRS 7 static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); -#define CURRENT_SCALING_FACTOR (1.31578947f) - // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { @@ -621,12 +619,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } else { ADCValue = HAL_ADC_GetValue(hadc); } - if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ - float current = phase_current_from_adcval(motor, ADCValue)*CURRENT_SCALING_FACTOR; - } else { - float current = phase_current_from_adcval(motor, ADCValue); - } - + float current = phase_current_from_adcval(motor, ADCValue); if (current_meas_not_DC_CAL) { // ADC2 and ADC3 record the phB and phC currents concurrently, @@ -1300,12 +1293,7 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ - float Ilim = motor->current_control.current_lim*CURRENT_SCALING_FACTOR; - } else{ - float Ilim = motor->current_control.current_lim; - } - + float Ilim = motor->current_control.current_lim; bool limited = false; if (Iq > Ilim) { limited = true; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 1acfc657..542b7dd7 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -70,7 +70,6 @@ #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) -#define MACRO_CONSTRAIN(amt,low,high) (((amt)<(low)) ? (low) : ((amt > high) ? (high) : (amt))) // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform From 63d8a12f6ee1328e5695ff2a5aa29397337d1e24 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:48:43 -0500 Subject: [PATCH 3/9] Shunt conductance depends on HW verison --- Firmware/MotorControl/low_level.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 961d4f7b..04fa245f 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -39,6 +39,14 @@ float vbus_voltage = 12.0f; #define POLE_PAIRS 7 static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); +#if HW_VERSION_MAJOR == 3 + #if HW_VERSION_MINOR < 4 + #define SHUNT_RESISTANCE (666e-6) + #else + #define SHUNT_RESISTANCE (500e-6) + #endif +#endif + // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { @@ -83,7 +91,7 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] + .shunt_conductance = 1.0f/SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic @@ -175,7 +183,7 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] + .shunt_conductance = 1.0f/SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic From d9b13efac28a6486008ac8237dc9a994b2b9fcb6 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:49:09 -0500 Subject: [PATCH 4/9] Check for max current based on HW version --- Firmware/MotorControl/low_level.c | 10 +++++++++- Firmware/MotorControl/low_level.h | 1 + 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 04fa245f..3062fd60 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -144,6 +144,7 @@ Motor_t motors[] = { .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, }, + .max_allowed_current = 0.0f, }, { // M1 .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t @@ -233,7 +234,8 @@ Motor_t motors[] = { .calib_anticogging = false, .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, - } + }, + .max_allowed_current = 0.0f, } }; const int num_motors = sizeof(motors)/sizeof(motors[0]); @@ -402,6 +404,7 @@ static void DRV8301_setup(Motor_t* motor) { local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: motor->phase_current_rev_gain = 1.0f/10.0f; break; @@ -416,6 +419,11 @@ static void DRV8301_setup(Motor_t* motor) { break; } + float margin = 0.95f; + float max_input = margin * 0.3f * motor->shunt_conductance; + float max_swing = margin * 1.6f * motor->shunt_conductance / motor->phase_current_rev_gain; + motor->max_allowed_current = MACRO_MIN(max_input, max_swing); + local_regs->SndCmd = true; DRV8301_writeData(gate_driver, local_regs); local_regs->RcvCmd = true; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 12eac1ba..4293f935 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -146,6 +146,7 @@ typedef struct { int timing_log_index; uint16_t timing_log[TIMING_LOG_SIZE]; Anticogging_t anticogging; + float max_allowed_current; } Motor_t; typedef struct{ From 4a868daaf09b5dd09ce56d6f63a3d550038e6d44 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:50:13 -0500 Subject: [PATCH 5/9] Fix GAIN --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3062fd60..44488489 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -421,7 +421,7 @@ static void DRV8301_setup(Motor_t* motor) { float margin = 0.95f; float max_input = margin * 0.3f * motor->shunt_conductance; - float max_swing = margin * 1.6f * motor->shunt_conductance / motor->phase_current_rev_gain; + float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; motor->max_allowed_current = MACRO_MIN(max_input, max_swing); local_regs->SndCmd = true; From f115a74dde2a9ccb463307474d67d24430a70201 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:50:32 -0500 Subject: [PATCH 6/9] Clamp to the lowest of (current_lim) and (max_allowed_current) --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 44488489..79617b44 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1309,7 +1309,7 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - float Ilim = motor->current_control.current_lim; + float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->max_allowed_current); bool limited = false; if (Iq > Ilim) { limited = true; From 161651ae484dd5dfccda5370f8a6c9ba59406e02 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 21:11:38 -0500 Subject: [PATCH 7/9] Move max_allowed_current to current_control struct --- Firmware/MotorControl/low_level.c | 10 +++++----- Firmware/MotorControl/low_level.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 79617b44..a4fe5379 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -104,6 +104,7 @@ Motor_t motors[] = { .final_v_alpha = 0.0f, .final_v_beta = 0.0f, .Iq = 0.0f, + .max_allowed_current = 0.0f, }, // .rotor_mode = ROTOR_MODE_SENSORLESS, // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, @@ -144,7 +145,6 @@ Motor_t motors[] = { .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, }, - .max_allowed_current = 0.0f, }, { // M1 .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t @@ -197,6 +197,7 @@ Motor_t motors[] = { .final_v_alpha = 0.0f, .final_v_beta = 0.0f, .Iq = 0.0f, + .max_allowed_current = 0.0f, }, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { @@ -234,8 +235,7 @@ Motor_t motors[] = { .calib_anticogging = false, .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, - }, - .max_allowed_current = 0.0f, + } } }; const int num_motors = sizeof(motors)/sizeof(motors[0]); @@ -422,7 +422,7 @@ static void DRV8301_setup(Motor_t* motor) { float margin = 0.95f; float max_input = margin * 0.3f * motor->shunt_conductance; float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; - motor->max_allowed_current = MACRO_MIN(max_input, max_swing); + motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); local_regs->SndCmd = true; DRV8301_writeData(gate_driver, local_regs); @@ -1309,7 +1309,7 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->max_allowed_current); + float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); bool limited = false; if (Iq > Ilim) { limited = true; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 4293f935..4806bbcb 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -71,6 +71,7 @@ typedef struct { float final_v_alpha; // [V] float final_v_beta; // [V] float Iq; + float max_allowed_current; } Current_control_t; typedef enum { @@ -146,7 +147,6 @@ typedef struct { int timing_log_index; uint16_t timing_log[TIMING_LOG_SIZE]; Anticogging_t anticogging; - float max_allowed_current; } Motor_t; typedef struct{ From cd2de216a55886d1f0a7d0f4d2f2303902ac1fad Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 24 Nov 2017 14:41:08 -0800 Subject: [PATCH 8/9] fix float literals, safer margin, extra instruction --- Firmware/MotorControl/low_level.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index a4fe5379..e07ff2d6 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -41,9 +41,9 @@ static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_C #if HW_VERSION_MAJOR == 3 #if HW_VERSION_MINOR < 4 - #define SHUNT_RESISTANCE (666e-6) + #define SHUNT_RESISTANCE (666e-6f) #else - #define SHUNT_RESISTANCE (500e-6) + #define SHUNT_RESISTANCE (500e-6f) #endif #endif @@ -401,10 +401,12 @@ static void DRV8301_setup(Motor_t* motor) { local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; // 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 local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: motor->phase_current_rev_gain = 1.0f/10.0f; break; @@ -419,7 +421,7 @@ static void DRV8301_setup(Motor_t* motor) { break; } - float margin = 0.95f; + float margin = 0.90f; float max_input = margin * 0.3f * motor->shunt_conductance; float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); From 638b829d9744c72e778fdcbdf5f82b3d0f9a5f66 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 24 Nov 2017 16:14:34 -0800 Subject: [PATCH 9/9] fine tune current sense adjustment --- Firmware/MotorControl/low_level.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index e07ff2d6..7b00312c 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -40,8 +40,8 @@ float vbus_voltage = 12.0f; static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); #if HW_VERSION_MAJOR == 3 - #if HW_VERSION_MINOR < 4 - #define SHUNT_RESISTANCE (666e-6f) + #if HW_VERSION_MINOR <= 3 + #define SHUNT_RESISTANCE (675e-6f) #else #define SHUNT_RESISTANCE (500e-6f) #endif