Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Test version of protected resistance measurements #714

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
+123 −24
Draft

Test version of protected resistance measurements #714

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions conf_general.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1472,10 +1472,12 @@ static int measure_r_l_imax(float current_min, float current_max,
const float res_old = mcconf->foc_motor_r;

float i_last = 0.0;
bool first_run = true;
for (float i = current_start;i < current_max;i *= 1.5) {
float res_tmp = 0.0;
fault = mcpwm_foc_measure_resistance(i, 5, false, &res_tmp);
fault = mcpwm_foc_measure_resistance(i, 5, false, &res_tmp, first_run);
i_last = i;
first_run = false;

if (fault != FAULT_CODE_NONE) {
mempools_free_mcconf(mcconf);
Expand All @@ -1487,7 +1489,7 @@ static int measure_r_l_imax(float current_min, float current_max,
}
}

fault = mcpwm_foc_measure_resistance(i_last, 100, true, r);
fault = mcpwm_foc_measure_resistance(i_last, 100, true, r, false);
if (fault != FAULT_CODE_NONE) {
mempools_free_mcconf(mcconf);
return fault;
Expand Down
1 change: 1 addition & 0 deletions datatypes.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -170,6 +170,7 @@ typedef enum {
FAULT_CODE_PHASE_FILTER,
FAULT_CODE_ENCODER_FAULT,
FAULT_CODE_LV_OUTPUT_FAULT,
FAULT_CODE_PHASE_OUTPUT_ERROR,
} mc_fault_code;

typedef enum {
Expand Down
6 changes: 0 additions & 6 deletions hwconf/teamtriforceuk/a50s_v23c/hw_a50s_v23c_core.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -260,12 +260,6 @@
#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 4000.0 // Minimum RPM to calculate the BEMF coupling from
#endif

// Don't call on boot, cal during motor config instead.
// This significatly speeds up boot time, which is important for combat robots
#ifndef MCCONF_FOC_OFFSETS_CAL_ON_BOOT
#define MCCONF_FOC_OFFSETS_CAL_ON_BOOT false
#endif

// Setting limits
#define HW_LIM_CURRENT -80.0, 80.0
#define HW_LIM_CURRENT_IN -40.0, 40.0
Expand Down
3 changes: 3 additions & 0 deletions lispBM/c_libs/vesc_c_if.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,9 @@ typedef enum {
FAULT_CODE_ENCODER_NO_MAGNET,
FAULT_CODE_ENCODER_MAGNET_TOO_STRONG,
FAULT_CODE_PHASE_FILTER,
FAULT_CODE_ENCODER_FAULT,
FAULT_CODE_LV_OUTPUT_FAULT,
FAULT_CODE_PHASE_OUTPUT_ERROR,
} mc_fault_code;

typedef union {
Expand Down
2 changes: 1 addition & 1 deletion lispBM/lispif_vesc_extensions.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3712,7 +3712,7 @@ static void measure_res_task(void *arg) {
if (lbm_start_flatten(&v, 10)) {
float res = -1.0;
mc_interface_select_motor_thread(a->motor);
mcpwm_foc_measure_resistance(a->current, a->samples, true, &res);
mcpwm_foc_measure_resistance(a->current, a->samples, true, &res, true);
mc_interface_select_motor_thread(1);

if (restart_cnt != lispif_get_restart_cnt()) {
Expand Down
1 change: 1 addition & 0 deletions motor/mc_interface.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -513,6 +513,7 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) {
case FAULT_CODE_PHASE_FILTER: return "FAULT_CODE_PHASE_FILTER";
case FAULT_CODE_ENCODER_FAULT: return "FAULT_CODE_ENCODER_FAULT";
case FAULT_CODE_LV_OUTPUT_FAULT: return "FAULT_CODE_LV_OUTPUT_FAULT";
case FAULT_CODE_PHASE_OUTPUT_ERROR: return "FAULT_CODE_PHASE_OUTPUT_ERROR";
}

return "Unknown fault";
Expand Down
124 changes: 111 additions & 13 deletions motor/mcpwm_foc.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1736,7 +1736,7 @@ int mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ra
* @return
* The fault code.
*/
int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance) {
int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance, bool settle_first) {
mc_interface_lock();

volatile motor_all_state_t *motor = get_motor_now();
Expand All @@ -1755,10 +1755,100 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
timeout_reset();
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);

// Ramp up the current slowly
while (fabsf(motor->m_iq_set - current) > 0.001) {
utils_step_towards((float*)&motor->m_iq_set, current, fabsf(current) / 200.0);

static bool plot_started = false;
static float plot_int = 0.0;
static int get_fw_version_cnt = 0;


if(settle_first) {
for(int i = 0; i < 500; i++) {
if (commands_get_fw_version_sent_cnt() != get_fw_version_cnt) {
get_fw_version_cnt = commands_get_fw_version_sent_cnt();
plot_started = false;
}
if (!plot_started) {
plot_started = true;
commands_init_plot("Time", "Current");
commands_plot_add_graph("iq_filter");
commands_plot_add_graph("id_filter");
commands_plot_add_graph("iq_set");
commands_plot_add_graph("id_trip_limit");
}
commands_plot_set_graph(0);
commands_send_plot_points(plot_int, motor->m_motor_state.iq_filter);
commands_plot_set_graph(1);
commands_send_plot_points(plot_int, motor->m_motor_state.id_filter);
commands_plot_set_graph(2);
commands_send_plot_points(plot_int, motor->m_iq_set);
commands_plot_set_graph(3);
commands_send_plot_points(plot_int, 0);
plot_int++;
chThdSleepMilliseconds(1);

// report back faults
fault = mc_interface_get_fault();
if (fault != FAULT_CODE_NONE) {
motor->m_id_set = 0.0;
motor->m_iq_set = 0.0;
motor->m_phase_override = false;
motor->m_control_mode = CONTROL_MODE_NONE;
motor->m_state = MC_STATE_OFF;
stop_pwm_hw((motor_all_state_t*)motor);

timeout_configure(tout, tout_c, tout_ksw);
mc_interface_unlock();

return fault;
}
}
}

// Change setpoint
motor->m_iq_set = current;
chThdSleepMilliseconds(1);

for(int i = 0; i < 50; i++) {
#ifdef HW_HAS_3_SHUNTS
if(i > 30) {
#else
if(i > 5) {
#endif
commands_plot_set_graph(0);
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter));
commands_plot_set_graph(1);
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.id_filter));
commands_plot_set_graph(2);
commands_send_plot_points(plot_int, motor->m_iq_set);
commands_plot_set_graph(3);

#ifdef HW_HAS_3_SHUNTS
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter) * 0.5);
if (fabs(motor->m_motor_state.id_filter) > fabs(motor->m_motor_state.iq_filter) * 0.5) {
mc_interface_fault_stop(FAULT_CODE_PHASE_OUTPUT_ERROR, &m_motor_1 != motor, false);
}
#else
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter) * 0.4);
if (fabs(motor->m_motor_state.id_filter) > fabs(motor->m_motor_state.iq_filter) * 0.4) {
mc_interface_fault_stop(FAULT_CODE_PHASE_OUTPUT_ERROR, &m_motor_1 != motor, false);
}
#endif


} else {
commands_plot_set_graph(0);
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter));
commands_plot_set_graph(1);
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.id_filter));
commands_plot_set_graph(2);
commands_send_plot_points(plot_int, motor->m_iq_set);
commands_plot_set_graph(3);
commands_send_plot_points(plot_int, 0);
}
plot_int++;
chThdSleepMilliseconds(1);

// report back faults
fault = mc_interface_get_fault();
if (fault != FAULT_CODE_NONE) {
motor->m_id_set = 0.0;
Expand All @@ -1772,20 +1862,26 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl
mc_interface_unlock();

return fault;
}
chThdSleepMilliseconds(1);
}
}

// Wait for the current to rise and the motor to lock.
chThdSleepMilliseconds(50);


// Sample
motor->m_samples.avg_current_tot = 0.0;
motor->m_samples.avg_voltage_tot = 0.0;
motor->m_samples.sample_num = 0;

int cnt = 0;
while (motor->m_samples.sample_num < samples) {
while (motor->m_samples.sample_num < samples) {
commands_plot_set_graph(0);
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter));
commands_plot_set_graph(1);
commands_send_plot_points(plot_int, fabs(motor->m_motor_state.id_filter));
commands_plot_set_graph(2);
commands_send_plot_points(plot_int, motor->m_iq_set);
commands_plot_set_graph(3);
commands_send_plot_points(plot_int, 0);
plot_int++;
chThdSleepMilliseconds(1);
cnt++;
// Timeout
Expand Down Expand Up @@ -2239,10 +2335,12 @@ int mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
motor->m_conf->foc_current_kp = 0.001;
motor->m_conf->foc_current_ki = 1.0;

bool first_run = true;
float i_last = 0.0;
for (float i = 2.0;i < (motor->m_conf->l_current_max / 2.0);i *= 1.5) {
float r_tmp = 0.0;
fault = mcpwm_foc_measure_resistance(i, 20, false, &r_tmp);
fault = mcpwm_foc_measure_resistance(i, 20, false, &r_tmp, first_run);
first_run = false;
if (fault != FAULT_CODE_NONE || r_tmp == 0.0) {
goto exit_measure_res_ind;
}
Expand All @@ -2260,7 +2358,7 @@ int mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
i_last = (motor->m_conf->l_current_max / 2.0);
#endif

fault = mcpwm_foc_measure_resistance(i_last, 200, true, res);
fault = mcpwm_foc_measure_resistance(i_last, 200, true, res, false);
if (fault == FAULT_CODE_NONE && *res != 0.0) {
motor->m_conf->foc_motor_r = *res;
mcpwm_foc_set_current(0.0);
Expand Down
2 changes: 1 addition & 1 deletion motor/mcpwm_foc.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ float mcpwm_foc_get_est_lambda(void);
float mcpwm_foc_get_est_res(void);
float mcpwm_foc_get_est_ind(void);
int mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted);
int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance);
int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance, bool settle_first);
int mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *ld_lq_diff, float *inductance);
int mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr, float *ld_lq_diff, float *inductance);

Expand Down
2 changes: 1 addition & 1 deletion terminal.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -323,7 +323,7 @@ void terminal_process_string(char *str) {
mcconf->motor_type = MOTOR_TYPE_FOC;
mc_interface_set_configuration(mcconf);
float tmp_r = 0.0;
int fault = mcpwm_foc_measure_resistance(current, 2000, true, &tmp_r);
int fault = mcpwm_foc_measure_resistance(current, 2000, true, &tmp_r, true);
if(fault == FAULT_CODE_NONE) {
commands_printf("Resistance: %.6f ohm\n", (double)tmp_r);
} else {
Expand Down