From 0ffd0221a4164f1c47b1a09b67ea24dfa7803f31 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Wed, 17 Jul 2024 12:43:21 +0300 Subject: [PATCH 1/2] add abnormal calibration voltage check --- docs/protocol/reference.rst | 2 ++ firmware/src/can/can_endpoints.h | 2 +- firmware/src/config.h | 2 ++ firmware/src/motor/motor.c | 13 +++++++++++++ firmware/src/tm_enums.h | 3 ++- studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml | 2 +- 6 files changed, 21 insertions(+), 3 deletions(-) diff --git a/docs/protocol/reference.rst b/docs/protocol/reference.rst index baf94b7f..0e48ccb4 100644 --- a/docs/protocol/reference.rst +++ b/docs/protocol/reference.rst @@ -759,6 +759,8 @@ Flags: - POLE_PAIRS_OUT_OF_RANGE +- ABNORMAL_CALIBRATION_VOLTAGE + sensors.user_frame.position_estimate ------------------------------------------------------------------- diff --git a/firmware/src/can/can_endpoints.h b/firmware/src/can/can_endpoints.h index a606d63f..952f3baa 100644 --- a/firmware/src/can/can_endpoints.h +++ b/firmware/src/can/can_endpoints.h @@ -10,7 +10,7 @@ #include #include -static const uint32_t avlos_proto_hash = 1031937702; +static const uint32_t avlos_proto_hash = 641680925; extern uint8_t (*avlos_endpoints[97])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); extern uint32_t _avlos_get_proto_hash(void); diff --git a/firmware/src/config.h b/firmware/src/config.h index 91644f40..77bf4570 100644 --- a/firmware/src/config.h +++ b/firmware/src/config.h @@ -31,6 +31,8 @@ #define PWM_FREQ_HZ (20000) #define SYSTICK_FREQ_HZ (1000) +#define MAX_CALIBRATION_POWER 30.0f // W + // Control parameters #define PWM_LIMIT (0.8f) #define I_INTEGRATOR_DECAY_FACTOR (0.995f) diff --git a/firmware/src/motor/motor.c b/firmware/src/motor/motor.c index c186fea7..a4605169 100644 --- a/firmware/src/motor/motor.c +++ b/firmware/src/motor/motor.c @@ -83,17 +83,29 @@ bool motor_calibrate_resistance(void) float V_setpoint = 0.0f; FloatTriplet I_phase_meas = {0.0f}; FloatTriplet modulation_values = {0.0f}; + for (uint32_t i = 0; i < CAL_R_LEN; i++) { ADC_get_phase_currents(&I_phase_meas); + + if (powf(V_setpoint, 2) / MAX_PHASE_RESISTANCE > MAX_CALIBRATION_POWER) + { + uint8_t *error_ptr = motor_get_error_ptr(); + *error_ptr |= MOTOR_ERRORS_ABNORMAL_CALIBRATION_VOLTAGE; + gate_driver_set_duty_cycle(&three_phase_zero); + return false; + } + V_setpoint += CAL_V_GAIN * (I_cal - I_phase_meas.A); const float pwm_setpoint = V_setpoint / system_get_Vbus(); SVM(pwm_setpoint, 0.0f, &modulation_values.A, &modulation_values.B, &modulation_values.C); gate_driver_set_duty_cycle(&modulation_values); wait_for_control_loop_interrupt(); } + const float R = our_fabsf(V_setpoint / I_cal); gate_driver_set_duty_cycle(&three_phase_zero); + if ((R <= MIN_PHASE_RESISTANCE) || (R >= MAX_PHASE_RESISTANCE)) { uint8_t *error_ptr = motor_get_error_ptr(); @@ -108,6 +120,7 @@ bool motor_calibrate_resistance(void) return true; } + bool motor_calibrate_inductance(void) { if (!motor_get_is_gimbal()) diff --git a/firmware/src/tm_enums.h b/firmware/src/tm_enums.h index b5182773..dcf35f4d 100644 --- a/firmware/src/tm_enums.h +++ b/firmware/src/tm_enums.h @@ -66,7 +66,8 @@ typedef enum MOTOR_ERRORS_PHASE_RESISTANCE_OUT_OF_RANGE = (1 << 0), MOTOR_ERRORS_PHASE_INDUCTANCE_OUT_OF_RANGE = (1 << 1), MOTOR_ERRORS_POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE = (1 << 2), - MOTOR_ERRORS_POLE_PAIRS_OUT_OF_RANGE = (1 << 3) + MOTOR_ERRORS_POLE_PAIRS_OUT_OF_RANGE = (1 << 3), + MOTOR_ERRORS_ABNORMAL_CALIBRATION_VOLTAGE = (1 << 4) } motor_errors_flags; typedef enum diff --git a/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml b/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml index e91876bb..04c926a2 100644 --- a/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml +++ b/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml @@ -334,7 +334,7 @@ remote_attributes: setter_name: motor_set_I_cal summary: The calibration current. - name: errors - flags: [PHASE_RESISTANCE_OUT_OF_RANGE, PHASE_INDUCTANCE_OUT_OF_RANGE, POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE, POLE_PAIRS_OUT_OF_RANGE] + flags: [PHASE_RESISTANCE_OUT_OF_RANGE, PHASE_INDUCTANCE_OUT_OF_RANGE, POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE, POLE_PAIRS_OUT_OF_RANGE, ABNORMAL_CALIBRATION_VOLTAGE] meta: {dynamic: True} getter_name: motor_get_errors summary: Any motor/calibration errors, as a bitmask From 49a2ea338bd4772517717a57c684c975282c2bae Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Mon, 29 Jul 2024 14:11:00 +0300 Subject: [PATCH 2/2] Refactor abnormal voltage check to be more robust --- firmware/src/config.h | 2 -- firmware/src/motor/motor.c | 3 ++- firmware/src/motor/motor.h | 4 ++++ 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/firmware/src/config.h b/firmware/src/config.h index 77bf4570..91644f40 100644 --- a/firmware/src/config.h +++ b/firmware/src/config.h @@ -31,8 +31,6 @@ #define PWM_FREQ_HZ (20000) #define SYSTICK_FREQ_HZ (1000) -#define MAX_CALIBRATION_POWER 30.0f // W - // Control parameters #define PWM_LIMIT (0.8f) #define I_INTEGRATOR_DECAY_FACTOR (0.995f) diff --git a/firmware/src/motor/motor.c b/firmware/src/motor/motor.c index a4605169..4465136d 100644 --- a/firmware/src/motor/motor.c +++ b/firmware/src/motor/motor.c @@ -88,7 +88,8 @@ bool motor_calibrate_resistance(void) { ADC_get_phase_currents(&I_phase_meas); - if (powf(V_setpoint, 2) / MAX_PHASE_RESISTANCE > MAX_CALIBRATION_POWER) + // + if (V_setpoint > MAX_CALIBRATION_VOLTAGE && I_phase_meas.A < MIN_CALIBRATION_CURRENT) { uint8_t *error_ptr = motor_get_error_ptr(); *error_ptr |= MOTOR_ERRORS_ABNORMAL_CALIBRATION_VOLTAGE; diff --git a/firmware/src/motor/motor.h b/firmware/src/motor/motor.h index 27a5616a..6f9ed6f9 100644 --- a/firmware/src/motor/motor.h +++ b/firmware/src/motor/motor.h @@ -27,11 +27,15 @@ #define MAX_PHASE_RESISTANCE (1.0f) #define MIN_PHASE_INDUCTANCE (5e-6f) #define MAX_PHASE_INDUCTANCE (1e-3f) +#define MAX_CALIBRATION_VOLTAGE (0.5f) // V +#define MIN_CALIBRATION_CURRENT (0.2f) // A #elif defined BOARD_REV_M5 #define MIN_PHASE_RESISTANCE (0.5f) #define MAX_PHASE_RESISTANCE (20.0f) #define MIN_PHASE_INDUCTANCE (1e-5f) #define MAX_PHASE_INDUCTANCE (1e-2f) +#define MAX_CALIBRATION_VOLTAGE (5.0f) // V +#define MIN_CALIBRATION_CURRENT (0.1f) // A #endif #define CAL_R_LEN (2 * PWM_FREQ_HZ)