Merge pull request #365 from tinymovr/firmware/abnormal_calibration_v…

…oltage_check

Abnormal calibration voltage check

docs/protocol/reference.rst

@@ -759,6 +759,8 @@ Flags:
 
 - POLE_PAIRS_OUT_OF_RANGE
 
+- ABNORMAL_CALIBRATION_VOLTAGE
+
 sensors.user_frame.position_estimate
 -------------------------------------------------------------------
 

firmware/src/can/can_endpoints.h

@@ -10,7 +10,7 @@
 #include <src/common.h>
 #include <src/tm_enums.h>
 
-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);
 

firmware/src/motor/motor.c

@@ -83,17 +83,30 @@ 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 (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;
+                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 +121,7 @@ bool motor_calibrate_resistance(void)
     return true;
 }
 
+
 bool motor_calibrate_inductance(void)
 {
     if (!motor_get_is_gimbal())

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)

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

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