From 4829ee236d60574a46f465aa4fcb695f1253ef3b Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 12:25:27 +0300 Subject: [PATCH 01/21] fix size calculation --- firmware/src/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/firmware/src/config.h b/firmware/src/config.h index ab0bd536..bfcbe617 100644 --- a/firmware/src/config.h +++ b/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 From b865c9f239f0fb98a9696a90b680b2f3ab761b0c Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 12:26:00 +0300 Subject: [PATCH 02/21] rearrange delays --- studio/Python/tests/test_board.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py index 16464edb..4f0b89f8 100644 --- a/studio/Python/tests/test_board.py +++ b/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): """ From 0ed79aa69c4839829f91302d11d1e5caad85eaa0 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 12:26:10 +0300 Subject: [PATCH 03/21] increase speed in test --- studio/Python/tests/test_board.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py index 4f0b89f8..2dd9c1bc 100644 --- a/studio/Python/tests/test_board.py +++ b/studio/Python/tests/test_board.py @@ -104,7 +104,7 @@ def test_e_velocity_control(self): self.tm.controller.velocity_mode() self.check_state(2) - R = 14 + R = 15 velocity_pairs = [] From e6101f981f83626ca21d14975aca7ac96614a8ab Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 12:26:33 +0300 Subject: [PATCH 04/21] refactor for readability --- firmware/src/encoder/ma7xx.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/firmware/src/encoder/ma7xx.c b/firmware/src/encoder/ma7xx.c index 617298cc..e40a402a 100644 --- a/firmware/src/encoder/ma7xx.c +++ b/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); + 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); return angle + off_interp; } From 7dc9ca62f78912e1f5aa09b2b5575c8f17aa628e Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 12:26:44 +0300 Subject: [PATCH 05/21] increase bandwidth --- firmware/src/observer/observer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/firmware/src/observer/observer.c b/firmware/src/observer/observer.c index aebfadbb..ddadf125 100644 --- a/firmware/src/observer/observer.c +++ b/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, }; From fa417b1ffddfec4596bdb91494955e5e0add8884 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 12:29:30 +0300 Subject: [PATCH 06/21] calculate size constant using bit count --- firmware/src/common.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/firmware/src/common.h b/firmware/src/common.h index 36206436..a05bf7b2 100644 --- a/firmware/src/common.h +++ b/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) From 236caf462f5584c3944073b77a1769f292628d8a Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 12:29:42 +0300 Subject: [PATCH 07/21] fix some offset related errors --- firmware/src/motor/calibration.c | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index e1249f28..ab8ed088 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -239,19 +239,20 @@ 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]; + int16_t error_ticks_f[ECN_SIZE * 20]; + int16_t error_ticks_r[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(); + set_epos_and_wait(e_pos_ref, I_setpoint); wait_a_while(); - int16_t offset_raw = ma7xx_get_angle_raw(); - // Perform measuerments, store only mean of F + B error + int32_t offset_raw = ma7xx_get_angle_raw(); + for (uint32_t i = 0; i < n; i++) { for (uint8_t j = 0; j < nconv; j++) @@ -261,7 +262,7 @@ bool calibrate_offset_and_rectification(void) } WaitForControlLoopInterrupt(); const float pos_meas = observer_get_pos_estimate(); - error_ticks[i] = (int16_t)(e_pos_ref * e_pos_to_ticks - pos_meas); + error_ticks_f[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++) @@ -273,11 +274,13 @@ 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_r[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(); + const uint16_t offset_idx = offset_raw >> (ENCODER_BITS - ECN_BITS); + // FIR filtering and map measurements to lut for (int16_t i=0; i> ECN_BITS); + acc = acc / (ECN_SIZE * 2); + int16_t write_idx = i + offset_idx; if (write_idx > (ECN_SIZE - 1)) { write_idx -= ECN_SIZE; From 2e9f67e32c3ffdc7c0a33a04cce59f81717c3039 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 5 Sep 2023 23:42:55 +0300 Subject: [PATCH 08/21] scale fir filtered series --- firmware/src/motor/calibration.c | 1 + 1 file changed, 1 insertion(+) diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index ab8ed088..a2fdbfa7 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -299,6 +299,7 @@ bool calibrate_offset_and_rectification(void) acc += error_ticks_f[read_idx] + error_ticks_r[read_idx]; } acc = acc / (ECN_SIZE * 2); + acc = (int32_t)((acc - offset_raw) * 1.1f) + offset_raw; int16_t write_idx = i + offset_idx; if (write_idx > (ECN_SIZE - 1)) { From 1b8c69bd0d0c2a8cd369f4af1ccd221fcb13d002 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Wed, 6 Sep 2023 00:47:09 +0300 Subject: [PATCH 09/21] accumulate in single array --- firmware/src/motor/calibration.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index a2fdbfa7..6dce8cec 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -239,8 +239,7 @@ 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_f[ECN_SIZE * 20]; - int16_t error_ticks_r[ECN_SIZE * 20]; + int16_t error_ticks[ECN_SIZE * 20]; const int16_t npp = motor_get_pole_pairs(); const int16_t n = ECN_SIZE * npp; const int16_t nconv = 100; @@ -262,7 +261,7 @@ bool calibrate_offset_and_rectification(void) } WaitForControlLoopInterrupt(); const float pos_meas = observer_get_pos_estimate(); - error_ticks_f[i] = (int16_t)(e_pos_ref * e_pos_to_ticks - pos_meas); + 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++) @@ -274,7 +273,7 @@ bool calibrate_offset_and_rectification(void) } WaitForControlLoopInterrupt(); const float pos_meas = observer_get_pos_estimate(); - error_ticks_r[n - i - 1] = (int16_t)(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(); @@ -296,7 +295,7 @@ bool calibrate_offset_and_rectification(void) { read_idx -= n; } - acc += error_ticks_f[read_idx] + error_ticks_r[read_idx]; + acc += error_ticks[read_idx]; } acc = acc / (ECN_SIZE * 2); acc = (int32_t)((acc - offset_raw) * 1.1f) + offset_raw; From 2db3b700a1c991053db8531c42f4479561cfdfab Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Mon, 18 Sep 2023 23:16:53 +0300 Subject: [PATCH 10/21] compute offset at start --- firmware/src/motor/calibration.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index 6dce8cec..ad2b7b1f 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -239,7 +239,7 @@ 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 * 20]; + 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 = 100; @@ -250,7 +250,9 @@ bool calibrate_offset_and_rectification(void) int16_t *lut = ma7xx_get_rec_table_ptr(); set_epos_and_wait(e_pos_ref, I_setpoint); wait_a_while(); - int32_t offset_raw = ma7xx_get_angle_raw(); + float offset_obs = observer_get_pos_estimate(); + const uint16_t offset_idx = ma7xx_get_angle_raw() >> (ENCODER_BITS - ECN_BITS); + const float cf = motor_get_cf(); for (uint32_t i = 0; i < n; i++) { @@ -263,7 +265,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++) @@ -278,12 +279,10 @@ bool calibrate_offset_and_rectification(void) gate_driver_set_duty_cycle(&three_phase_zero); gate_driver_disable(); - const uint16_t offset_idx = offset_raw >> (ENCODER_BITS - ECN_BITS); - // FIR filtering and map measurements to lut for (int16_t i=0; i (ECN_SIZE - 1)) { From 1631d50b2347a5b3b9b58561dc9b47d09060ad9c Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Mon, 18 Sep 2023 23:17:46 +0300 Subject: [PATCH 11/21] ignore some errors --- firmware/src/system/system.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/firmware/src/system/system.c b/firmware/src/system/system.c index 4f7ad078..46ecc0aa 100644 --- a/firmware/src/system/system.c +++ b/firmware/src/system/system.c @@ -108,13 +108,14 @@ TM_RAMFUNC void system_update(void) const uint8_t drv_status = pac5xxx_tile_register_read(ADDR_STATDRV); if (drv_status > 0) { - state.errors |= ((drv_status & 0x7) << 4); + 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. + // 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); } } From 755626fa74e99fc76987723a360c56f45a3cf9ba Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Mon, 18 Sep 2023 23:18:38 +0300 Subject: [PATCH 12/21] discard erroneous computation --- firmware/src/motor/calibration.c | 1 - 1 file changed, 1 deletion(-) diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index ad2b7b1f..8ebd364c 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -296,7 +296,6 @@ bool calibrate_offset_and_rectification(void) } acc += error_ticks[read_idx]; } - acc = (int32_t)((acc - offset_raw) * 1.1f) + offset_raw; acc = acc / ((float)(ECN_SIZE * 2)); int16_t write_idx = i + offset_idx; if (write_idx > (ECN_SIZE - 1)) From c75a2861476f2f9715c08a44d64d979d09e69425 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Mon, 18 Sep 2023 23:19:33 +0300 Subject: [PATCH 13/21] remove deprecated setting --- .vscode/settings.json | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 59df6df2..9e3ad64e 100644 --- a/.vscode/settings.json +++ b/.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 } \ No newline at end of file From be43e6832f91f232bb7d17b1d4dd0d957f59cce4 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Mon, 18 Sep 2023 23:58:11 +0300 Subject: [PATCH 14/21] remove unused variables --- firmware/src/motor/calibration.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index 8ebd364c..26eff708 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -250,9 +250,7 @@ bool calibrate_offset_and_rectification(void) int16_t *lut = ma7xx_get_rec_table_ptr(); set_epos_and_wait(e_pos_ref, I_setpoint); wait_a_while(); - float offset_obs = observer_get_pos_estimate(); const uint16_t offset_idx = ma7xx_get_angle_raw() >> (ENCODER_BITS - ECN_BITS); - const float cf = motor_get_cf(); for (uint32_t i = 0; i < n; i++) { From 1cdf06b27a5710aea1b20f7527fff938e62d20ed Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 19 Sep 2023 02:15:50 +0300 Subject: [PATCH 15/21] fix hall error being misplaced as motor error --- firmware/src/encoder/hall.c | 5 +++++ firmware/src/encoder/hall.h | 2 ++ firmware/src/motor/calibration.c | 2 +- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/firmware/src/encoder/hall.c b/firmware/src/encoder/hall.c index d3f386df..5ce0c1c7 100644 --- a/firmware/src/encoder/hall.c +++ b/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; diff --git a/firmware/src/encoder/hall.h b/firmware/src/encoder/hall.h index 3cd395b6..bea71aac 100644 --- a/firmware/src/encoder/hall.h +++ b/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); diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index 26eff708..0c5536ea 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -230,7 +230,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; From d18c9931e010481a5f177c42c3343ccad5aa9437 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 19 Sep 2023 14:10:01 +0300 Subject: [PATCH 16/21] rename function --- firmware/src/adc/adc.c | 2 +- firmware/src/adc/adc.h | 2 +- firmware/src/main.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/firmware/src/adc/adc.c b/firmware/src/adc/adc.c index c90cd45c..73096774 100644 --- a/firmware/src/adc/adc.c +++ b/firmware/src/adc/adc.c @@ -62,7 +62,7 @@ static ADCConfig adc_config = { .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)); diff --git a/firmware/src/adc/adc.h b/firmware/src/adc/adc.h index 8ebf467c..04a763df 100644 --- a/firmware/src/adc/adc.h +++ b/firmware/src/adc/adc.h @@ -104,7 +104,7 @@ typedef struct float temp_tau; } ADCConfig; -void ADC_Init(void); +void ADC_init(void); float adc_get_mcu_temp(void); void ADC_GetPhaseCurrents(FloatTriplet *phc); void ADC_update(void); diff --git a/firmware/src/main.c b/firmware/src/main.c index 4a96126f..0b861e6e 100644 --- a/firmware/src/main.c +++ b/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(); From 89340609a07477e2faa467572deec8f2bdb4317a Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Tue, 19 Sep 2023 14:35:13 +0300 Subject: [PATCH 17/21] reset calibration --- firmware/src/adc/adc.c | 9 ++++++++- firmware/src/adc/adc.h | 1 + firmware/src/motor/calibration.c | 15 ++++++++------- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/firmware/src/adc/adc.c b/firmware/src/adc/adc.c index 73096774..a4be73c5 100644 --- a/firmware/src/adc/adc.c +++ b/firmware/src/adc/adc.c @@ -58,7 +58,7 @@ 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 }; @@ -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) { //======================================================================================================== diff --git a/firmware/src/adc/adc.h b/firmware/src/adc/adc.h index 04a763df..5c19e218 100644 --- a/firmware/src/adc/adc.h +++ b/firmware/src/adc/adc.h @@ -105,6 +105,7 @@ typedef struct } ADCConfig; void ADC_init(void); +void ADC_reset(void); float adc_get_mcu_temp(void); void ADC_GetPhaseCurrents(FloatTriplet *phc); void ADC_update(void); diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index 0c5536ea..a8cffe52 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -28,13 +28,13 @@ #include static inline void set_epos_and_wait(float angle, float I_setpoint); -static inline void wait_a_while(void); +static inline void wait_pwm_cycles(uint32_t cycles); 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; } @@ -249,7 +249,7 @@ bool calibrate_offset_and_rectification(void) const float I_setpoint = motor_get_I_cal(); int16_t *lut = ma7xx_get_rec_table_ptr(); set_epos_and_wait(e_pos_ref, I_setpoint); - wait_a_while(); + 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++) @@ -302,17 +302,18 @@ bool calibrate_offset_and_rectification(void) } 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) @@ -326,11 +327,11 @@ static inline void set_epos_and_wait(float angle, float I_setpoint) WaitForControlLoopInterrupt(); } -static inline void wait_a_while(void) +static inline void wait_pwm_cycles(uint32_t cycles) { // 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++) + for (uint32_t i = 0; i < cycles; i++) { WaitForControlLoopInterrupt(); } From d6ddb423f93842171d3850963b02f1770ed642ba Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Thu, 21 Sep 2023 13:34:05 +0300 Subject: [PATCH 18/21] disable driver errors returning spurious values --- firmware/src/system/system.c | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/firmware/src/system/system.c b/firmware/src/system/system.c index 46ecc0aa..2a5e4d0b 100644 --- a/firmware/src/system/system.c +++ b/firmware/src/system/system.c @@ -105,19 +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 & 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); - } + // 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) From e35b00160778e0e0ca57ce672fa04bbaaa276cf6 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Thu, 21 Sep 2023 13:48:09 +0300 Subject: [PATCH 19/21] rearrange functions --- firmware/src/motor/calibration.c | 39 +++++++++++++++----------------- 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index a8cffe52..bade3463 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -27,8 +27,24 @@ #include #include -static inline void set_epos_and_wait(float angle, float I_setpoint); -static inline void wait_pwm_cycles(uint32_t cycles); +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) { @@ -316,23 +332,4 @@ void reset_calibration(void) 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_pwm_cycles(uint32_t cycles) -{ - // Wait a while for the observer to settle - // TODO: This is a bit of a hack, can be improved! - for (uint32_t i = 0; i < cycles; i++) - { - WaitForControlLoopInterrupt(); - } -} From b7c3767503e26eaa3fdd9f239904bc59c0f8e453 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Thu, 21 Sep 2023 13:48:23 +0300 Subject: [PATCH 20/21] remove unneeded attribute --- firmware/src/utils/utils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/firmware/src/utils/utils.h b/firmware/src/utils/utils.h index 98a87b61..41829b65 100644 --- a/firmware/src/utils/utils.h +++ b/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; From 2079c33a2363c66a6d6109c1f15d0a19769e48bd Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Thu, 21 Sep 2023 13:48:34 +0300 Subject: [PATCH 21/21] remove obsolete definition --- firmware/src/common.h | 1 - 1 file changed, 1 deletion(-) diff --git a/firmware/src/common.h b/firmware/src/common.h index a05bf7b2..91cbfc67 100644 --- a/firmware/src/common.h +++ b/firmware/src/common.h @@ -135,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;