Merge pull request #293 from tinymovr/firmware/calibration_fixes

Calibration Fixes

.vscode/settings.json

@@ -13,6 +13,5 @@
         "test_*.py"
     ],
     "python.testing.pytestEnabled": false,
-    "python.testing.unittestEnabled": true,
-    "python.formatting.provider": "black"
+    "python.testing.unittestEnabled": true
 }

firmware/src/adc/adc.c

@@ -58,11 +58,11 @@ static ADCState adc_state = {0};
 static ADCConfig adc_config = {
     .I_phase_offset = {0},
     .Iphase_limit = 60.0f,
-    .I_phase_offset_tau = 0.5f,
+    .I_phase_offset_tau = 0.1f,
     .temp_tau = 1.0
 };
 
-void ADC_Init(void)
+void ADC_init(void)
 {
     // Compute tau-dependent variables
     adc_state.I_phase_offset_D = 1.0f - powf(EPSILON, -1.0f / (adc_config.I_phase_offset_tau * PWM_FREQ_HZ));
@@ -183,6 +183,13 @@ void ADC_AIO_Init(void)
     pac5xxx_tile_register_write(ADDR_CFGAIO5, HP_DIS_LP_EN_PR1 | OFFSET_EN | CAL_OFFSET_DIS | LP_HP_EN_1us);
 }
 
+void ADC_reset(void)
+{
+    adc_config.I_phase_offset.A = 0;
+    adc_config.I_phase_offset.B = 0;
+    adc_config.I_phase_offset.C = 0;
+}
+
 void ADC_DTSE_Init(void)
 {
     //========================================================================================================

firmware/src/adc/adc.h

@@ -104,7 +104,8 @@ typedef struct
     float temp_tau;
 } ADCConfig;
 
-void ADC_Init(void);
+void ADC_init(void);
+void ADC_reset(void);
 float adc_get_mcu_temp(void);
 void ADC_GetPhaseCurrents(FloatTriplet *phc);
 void ADC_update(void);

firmware/src/common.h

@@ -87,8 +87,9 @@
 
 #define ERROR_FLAG_MAX_SIZE (5u)
 
-#define ENCODER_TICKS (8192)
-#define ENCODER_TICKS_FLOAT (8192.f)
+#define ENCODER_BITS (13)
+#define ENCODER_TICKS (1 << ENCODER_BITS)
+#define ENCODER_TICKS_FLOAT ((float)(ENCODER_TICKS))
 
 #define HALL_SECTORS (6)
 #define HALL_SECTOR_ANGLE (TWOPI / HALL_SECTORS)
@@ -134,7 +135,6 @@ static const float threehalfpi = 4.7123889f;
 static const float pi = PI;
 static const float halfpi = PI * 0.5f;
 static const float quarterpi = PI * 0.25f;
-//static const int32_t timer_freq_hz = ACLK_FREQ_HZ / (pow(2, TXCTL_PS_DIV)); // (alternative)
 static const int32_t timer_freq_hz = ACLK_FREQ_HZ >> TXCTL_PS_DIV;
 static const float twopi_by_enc_ticks = TWOPI / ENCODER_TICKS;
 static const float twopi_by_hall_sectors = TWOPI / HALL_SECTORS;

firmware/src/config.h

@@ -42,7 +42,7 @@
 
 // Encoder rectification lookup table size
 #define ECN_BITS (6)
-#define ECN_SIZE (2 << ECN_BITS)
+#define ECN_SIZE (1 << ECN_BITS)
 
 // UART
 #define UART_ENUM UARTB

firmware/src/encoder/hall.c

@@ -37,6 +37,11 @@ TM_RAMFUNC uint8_t hall_get_errors(void)
     return state.errors;
 }
 
+TM_RAMFUNC uint8_t *hall_get_error_ptr(void)
+{
+	return &(state.errors);
+}
+
 TM_RAMFUNC int16_t hall_get_angle(void)
 {
     return state.angle;

firmware/src/encoder/hall.h

@@ -35,6 +35,8 @@ typedef struct
 void hall_init(void);
 
 uint8_t hall_get_errors(void);
+uint8_t *hall_get_error_ptr(void);
+
 int16_t hall_get_angle(void);
 void hall_update(bool check_error);
 uint8_t hall_get_sector(void);

firmware/src/encoder/ma7xx.c

@@ -51,10 +51,11 @@ TM_RAMFUNC int16_t ma7xx_get_angle_raw(void)
 
 TM_RAMFUNC int16_t ma7xx_get_angle_rectified(void)
 {
+    const uint8_t offset_bits = (ENCODER_BITS - ECN_BITS);
     const int16_t angle = state.angle;
-    const int16_t off_1 = config.rec_table[angle>>ECN_BITS];
-	const int16_t off_2 = config.rec_table[((angle>>ECN_BITS) + 1) % ECN_SIZE];
-	const int16_t off_interp = off_1 + ((off_2 - off_1)* (angle - ((angle>>ECN_BITS)<<ECN_BITS))>>ECN_BITS);
+    const int16_t off_1 = config.rec_table[angle>>offset_bits];
+	const int16_t off_2 = config.rec_table[((angle>>offset_bits) + 1) % ECN_SIZE];
+	const int16_t off_interp = off_1 + ((off_2 - off_1)* (angle - ((angle>>offset_bits)<<offset_bits))>>offset_bits);
 	return angle + off_interp;
 }
 

firmware/src/main.c

@@ -38,7 +38,7 @@ int main(void)
     encoder_init();
     UART_Init();
 	observer_init();
-	ADC_Init();
+	ADC_init();
     CAN_init();
     Timer_Init();
     Watchdog_init();

firmware/src/motor/calibration.c

@@ -27,14 +27,30 @@
 #include <src/system/system.h>
 #include <src/motor/calibration.h>
 
-static inline void set_epos_and_wait(float angle, float I_setpoint);
-static inline void wait_a_while(void);
+static inline void set_epos_and_wait(float angle, float I_setpoint)
+{
+    FloatTriplet modulation_values = {0.0f};
+    float pwm_setpoint = (I_setpoint * motor_get_phase_resistance()) / system_get_Vbus();
+    our_clampc(&pwm_setpoint, -PWM_LIMIT, PWM_LIMIT);
+    SVM(pwm_setpoint * fast_cos(angle), pwm_setpoint * fast_sin(angle),
+        &modulation_values.A, &modulation_values.B, &modulation_values.C);
+    gate_driver_set_duty_cycle(&modulation_values);
+    WaitForControlLoopInterrupt();
+}
+
+static inline void wait_pwm_cycles(uint32_t cycles)
+{
+    for (uint32_t i = 0; i < cycles; i++)
+    {
+        WaitForControlLoopInterrupt();
+    }
+}
 
 bool CalibrateADCOffset(void)
 {
     // We only need to wait here, the ADC loop will
     // perform the offset calibration automatically
-    wait_a_while();
+    wait_pwm_cycles(10000);
     return true;
 }
 
@@ -230,7 +246,7 @@ bool calibrate_hall_sequence(void)
     }
     else
     {
-        uint8_t *error_ptr = motor_get_error_ptr();
+        uint8_t *error_ptr = hall_get_error_ptr();
         *error_ptr |= ENCODER_ERRORS_CALIBRATION_FAILED;
     }
     return success;
@@ -239,19 +255,19 @@ bool calibrate_hall_sequence(void)
 bool calibrate_offset_and_rectification(void)
 {
     // Size below is an arbitrary large number ie > ECN_SIZE * npp
-    int16_t error_ticks[ECN_SIZE * 24];
+    float error_ticks[ECN_SIZE * 20];
     const int16_t npp = motor_get_pole_pairs();
     const int16_t n = ECN_SIZE * npp;
-    const int16_t nconv = 50;
+    const int16_t nconv = 100;
     const float delta = 2 * PI * npp / (n * nconv);
     const float e_pos_to_ticks = ((float)ENCODER_TICKS) / (2 * PI * npp);
     float e_pos_ref = 0.f;
     const float I_setpoint = motor_get_I_cal();
-    ma7xx_clear_rec_table();
     int16_t *lut = ma7xx_get_rec_table_ptr();
-    wait_a_while();
-    int16_t offset_raw = ma7xx_get_angle_raw();
-    // Perform measuerments, store only mean of F + B error
+    set_epos_and_wait(e_pos_ref, I_setpoint);
+    wait_pwm_cycles(5000);
+    const uint16_t offset_idx = ma7xx_get_angle_raw() >> (ENCODER_BITS - ECN_BITS);
+
     for (uint32_t i = 0; i < n; i++)
     {
         for (uint8_t j = 0; j < nconv; j++)
@@ -263,7 +279,6 @@ bool calibrate_offset_and_rectification(void)
         const float pos_meas = observer_get_pos_estimate();
         error_ticks[i] = (int16_t)(e_pos_ref * e_pos_to_ticks - pos_meas);
     }
-    offset_raw = (offset_raw + ma7xx_get_angle_raw()) / 2;
     for (uint32_t i = 0; i < n; i++)
     {
         for (uint8_t j = 0; j < nconv; j++)
@@ -273,15 +288,15 @@ bool calibrate_offset_and_rectification(void)
         }
         WaitForControlLoopInterrupt();
         const float pos_meas = observer_get_pos_estimate();
-        error_ticks[n - i - 1] = (int16_t)(0.5f * ((float)error_ticks[n - i - 1] + e_pos_ref * e_pos_to_ticks - pos_meas));
+        error_ticks[n - i - 1] += (int16_t)(e_pos_ref * e_pos_to_ticks - pos_meas);
     }
     gate_driver_set_duty_cycle(&three_phase_zero);
     gate_driver_disable();
 
     // FIR filtering and map measurements to lut
     for (int16_t i=0; i<ECN_SIZE; i++)
     {
-        int32_t acc = 0;
+        float acc = 0;
         for (int16_t j = 0; j < ECN_SIZE; j++)
         {
             int16_t read_idx = -ECN_SIZE / 2 + j + i * npp;
@@ -295,44 +310,26 @@ bool calibrate_offset_and_rectification(void)
             }
             acc += error_ticks[read_idx];
         }
-        acc /= ECN_SIZE;
-        int16_t write_idx = i + (offset_raw >> ECN_BITS);
+        acc = acc / ((float)(ECN_SIZE * 2));
+        int16_t write_idx = i + offset_idx;
         if (write_idx > (ECN_SIZE - 1))
         {
             write_idx -= ECN_SIZE;
         }
         lut[write_idx] = (int16_t)acc;
     }
-    wait_a_while();
+    wait_pwm_cycles(5000);
     ma7xx_set_rec_calibrated();
     return true;
 }
 
 void reset_calibration(void)
 {
+    ADC_reset();
     encoder_reset();
     observer_reset();
     motor_reset_calibration();
-    wait_a_while();
+    wait_pwm_cycles(5000);
 }
 
-static inline void set_epos_and_wait(float angle, float I_setpoint)
-{
-    FloatTriplet modulation_values = {0.0f};
-    float pwm_setpoint = (I_setpoint * motor_get_phase_resistance()) / system_get_Vbus();
-    our_clampc(&pwm_setpoint, -PWM_LIMIT, PWM_LIMIT);
-    SVM(pwm_setpoint * fast_cos(angle), pwm_setpoint * fast_sin(angle),
-        &modulation_values.A, &modulation_values.B, &modulation_values.C);
-    gate_driver_set_duty_cycle(&modulation_values);
-    WaitForControlLoopInterrupt();
-}
 
-static inline void wait_a_while(void)
-{
-    // Wait a while for the observer to settle
-    // TODO: This is a bit of a hack, can be improved!
-    for (int i = 0; i < 5000; i++)
-    {
-        WaitForControlLoopInterrupt();
-    }
-}

firmware/src/observer/observer.c

@@ -24,7 +24,7 @@
 static ObserverState state = {0};
 
 static ObserverConfig config = {
-		.track_bw = 300.0f,
+		.track_bw = 350.0f,
 		.kp = 0.0f,
 		.ki = 0.0f,
 };

firmware/src/system/system.c

@@ -105,18 +105,19 @@ TM_RAMFUNC void system_update(void)
     {
         state.errors |= ERRORS_UNDERVOLTAGE;
     }
-    const uint8_t drv_status = pac5xxx_tile_register_read(ADDR_STATDRV);
-    if (drv_status > 0)
-    {
-        state.errors |= ((drv_status & 0x7) << 4);
-    }
-    const uint8_t drv_fault = pac5xxx_tile_register_read(ADDR_DRV_FLT);
-    if (drv_fault > 0)
-    {
-        // We use 0x5 mask because we ignore CHARGE_PUMP_FAULT_STAT
-        // for the time being, as it seems to always be set.
-        state.errors |= ((drv_fault & 0x5) << 1);
-    }
+    // const uint8_t drv_status = pac5xxx_tile_register_read(ADDR_STATDRV);
+    // if (drv_status > 0)
+    // {
+    //     state.errors |= ((drv_status & 0x56) << 1);
+    // }
+    // const uint8_t drv_fault = pac5xxx_tile_register_read(ADDR_DRV_FLT);
+    // if (drv_fault > 0)
+    // {
+    //     // We use 0x5 mask because we ignore CHARGE_PUMP_FAULT_STAT
+    //     // for the time being, as it seems to always be set, and
+    //     // ERRORS_DRVXX_DISABLE, as it seems to give spurious errors.
+    //     state.errors |= ((drv_fault & 0x5) << 1);
+    // }
 }
 
 void system_reset(void)

firmware/src/utils/utils.h

@@ -24,7 +24,7 @@
 
 #if __ARM_FEATURE_FMA && __ARM_FP&4 && !__SOFTFP__ && !BROKEN_VFP_ASM
 
-PAC5XXX_RAMFUNC static inline float fast_sqrt(float x)
+static inline float fast_sqrt(float x)
 {
 	__asm__ ("vsqrt.f32 %0, %1" : "=t"(x) : "t"(x));
 	return x;

studio/Python/tests/test_board.py

@@ -28,7 +28,7 @@
 A = ureg.ampere
 ticks = ureg.ticks
 s = ureg.second
-tsleep = 0.18
+tsleep = 0.19
 
 
 class TestBoard(TMTestCase):
@@ -94,7 +94,6 @@ def test_d_position_control(self):
             self.assertAlmostEqual(
                 i * 1000 * ticks, self.tm.encoder.position_estimate, delta=1000 * ticks
             )
-            time.sleep(tsleep)
 
     def test_e_velocity_control(self):
         """
@@ -105,7 +104,7 @@ def test_e_velocity_control(self):
         self.tm.controller.velocity_mode()
         self.check_state(2)
 
-        R = 14
+        R = 15
 
         velocity_pairs = []