From fe0abbaa782220848b156a0a7883d017f5e10c15 Mon Sep 17 00:00:00 2001 From: Yannis Date: Thu, 24 Nov 2022 23:30:50 +0200 Subject: [PATCH] Optimize Main Loop (#227) * only make adc measurements if the inverter is on * make ram functions * better busy loop measurement * tidy up variable semantics * fix power calculation * optimize offset filter * increase gain * make filter k a constant * increase protection limit * improve legibility * move function to ram * inline functions --- firmware/src/adc/adc.c | 60 ++++++++++++++-------------- firmware/src/adc/adc.h | 21 +++++----- firmware/src/can/can.c | 2 +- firmware/src/can/can_func.c | 12 +++--- firmware/src/controller/controller.c | 6 +-- firmware/src/motor/calibration.h | 2 +- firmware/src/scheduler/scheduler.c | 16 ++++---- 7 files changed, 61 insertions(+), 58 deletions(-) diff --git a/firmware/src/adc/adc.c b/firmware/src/adc/adc.c index a6c9b70f..55be022a 100644 --- a/firmware/src/adc/adc.c +++ b/firmware/src/adc/adc.c @@ -18,11 +18,9 @@ #include #include #include +#include #include -// Resistor value of diffamp AIO RC filter -#define AIORES_VALUE 100.0f - #define AIO0to5_DIFF_AMP_MODE 0x40u #define AIO6789_IO_MODE 0x00u #define HP_DIS_LP_EN_PR1 0x20u @@ -55,16 +53,17 @@ void ADC_AIO_Init(void); void ADC_DTSE_Init(void); -static struct ADCState adc; +static ADCState adc_state = {0}; -static struct ADCConfig config = { - .Iphase_limit = 40.0f, - .I_filter_k = 0.6f, - .I_phase_offset_tau = 0.2f +static ADCConfig adc_config = { + .Iphase_limit = 60.0f, + .I_phase_offset_tau = 0.2f, + .I_phase_offset_k = 0.0f }; void ADC_Init(void) { + adc_config.I_phase_offset_k = PWM_PERIOD_S / adc_config.I_phase_offset_tau; // --- Begin CAFE2 Initialization // Write all CAFE registers @@ -104,8 +103,7 @@ void ADC_Init(void) pac5xxx_tile_register_write(ADDR_CFGDRV3, 0x00); // Configure overcurrent protection - uint16_t lpdac_val = config.Iphase_limit * ONE_OVER_SHUNT_SCALING_FACTOR; - // Why we need to write 2 bits of a 10-bit value to another register is beyond me... + uint16_t lpdac_val = (uint16_t)(adc_config.Iphase_limit * ONE_OVER_SHUNT_SCALING_FACTOR); pac5xxx_tile_register_write(ADDR_LPDACH, (lpdac_val >> 2) & 0xFFu); // 8b MSB pac5xxx_tile_register_write(ADDR_LPDACL, lpdac_val & 0x03u); // 2b LSB @@ -220,44 +218,46 @@ void ADC_DTSE_Init(void) PAC5XXX_RAMFUNC int16_t adc_get_mcu_temp(void) { - return adc.temp; + return adc_state.temp; } PAC5XXX_RAMFUNC void ADC_GetPhaseCurrents(struct FloatTriplet *phc) { - phc->A = adc.I_phase_meas.A; + phc->A = adc_state.I_phase_meas.A; if (motor_phases_swapped()) { - phc->B = adc.I_phase_meas.C; - phc->C = adc.I_phase_meas.B; + phc->B = adc_state.I_phase_meas.C; + phc->C = adc_state.I_phase_meas.B; } else { - phc->B = adc.I_phase_meas.B; - phc->C = adc.I_phase_meas.C; + phc->B = adc_state.I_phase_meas.B; + phc->C = adc_state.I_phase_meas.C; } } PAC5XXX_RAMFUNC void ADC_update(void) { - // TODO: Try doing below transformations in integer domain - const float I_phase_offset_k = PWM_PERIOD_S / config.I_phase_offset_tau; - adc.I_phase_offset.A += (((float)PAC55XX_ADC->DTSERES6.VAL * SHUNT_SCALING_FACTOR) - adc.I_phase_offset.A) * I_phase_offset_k; - adc.I_phase_offset.B += (((float)PAC55XX_ADC->DTSERES8.VAL * SHUNT_SCALING_FACTOR) - adc.I_phase_offset.B) * I_phase_offset_k; - adc.I_phase_offset.C += (((float)PAC55XX_ADC->DTSERES10.VAL * SHUNT_SCALING_FACTOR) - adc.I_phase_offset.C) * I_phase_offset_k; - - const float i_a = (((float)PAC55XX_ADC->DTSERES14.VAL * SHUNT_SCALING_FACTOR) - adc.I_phase_offset.A); - const float i_b = (((float)PAC55XX_ADC->DTSERES16.VAL * SHUNT_SCALING_FACTOR) - adc.I_phase_offset.B); - const float i_c = (((float)PAC55XX_ADC->DTSERES18.VAL * SHUNT_SCALING_FACTOR) - adc.I_phase_offset.C); - adc.I_phase_meas.A = ((1.0f - config.I_filter_k) * i_a) - (config.I_filter_k * (i_b + i_c)); - adc.I_phase_meas.B = ((1.0f - config.I_filter_k) * i_b) - (config.I_filter_k * (i_a + i_c)); - adc.I_phase_meas.C = ((1.0f - config.I_filter_k) * i_c) - (config.I_filter_k * (i_a + i_b)); - + if (gate_driver_is_enabled() == true) + { + // TODO: Try doing below transformations in integer domain + adc_state.I_phase_offset.A += (((float)PAC55XX_ADC->DTSERES6.VAL * SHUNT_SCALING_FACTOR) - adc_state.I_phase_offset.A) * adc_config.I_phase_offset_k; + adc_state.I_phase_offset.B += (((float)PAC55XX_ADC->DTSERES8.VAL * SHUNT_SCALING_FACTOR) - adc_state.I_phase_offset.B) * adc_config.I_phase_offset_k; + adc_state.I_phase_offset.C += (((float)PAC55XX_ADC->DTSERES10.VAL * SHUNT_SCALING_FACTOR) - adc_state.I_phase_offset.C) * adc_config.I_phase_offset_k; + + const float i_a = (((float)PAC55XX_ADC->DTSERES14.VAL * SHUNT_SCALING_FACTOR) - adc_state.I_phase_offset.A); + const float i_b = (((float)PAC55XX_ADC->DTSERES16.VAL * SHUNT_SCALING_FACTOR) - adc_state.I_phase_offset.B); + const float i_c = (((float)PAC55XX_ADC->DTSERES18.VAL * SHUNT_SCALING_FACTOR) - adc_state.I_phase_offset.C); + adc_state.I_phase_meas.A = ((1.0f - I_FILTER_K) * i_a) - (I_FILTER_K * (i_b + i_c)); + adc_state.I_phase_meas.B = ((1.0f - I_FILTER_K) * i_b) - (I_FILTER_K * (i_a + i_c)); + adc_state.I_phase_meas.C = ((1.0f - I_FILTER_K) * i_c) - (I_FILTER_K * (i_a + i_b)); + } + // Internal MCU temperature sensor reading at FTTEMP temperature in ADC counts. uint16_t TTEMPS = 0; memcpy(&TTEMPS, (const size_t *)0x0010041C, sizeof(uint16_t)); // Temperature in oC at time of internal temperature sensor const int32_t FTTEMP = 27; // READ_UINT16(0x0010041E); const int32_t temp_val = (int32_t)(PAC55XX_ADC->DTSERES2.VAL); - adc.temp = ((((FTTEMP + 273) * ((temp_val * 100) + 12288)) / (((int16_t)TTEMPS * 100) + 12288)) - 273); + adc_state.temp = ((((FTTEMP + 273) * ((temp_val * 100) + 12288)) / (((int16_t)TTEMPS * 100) + 12288)) - 273); } diff --git a/firmware/src/adc/adc.h b/firmware/src/adc/adc.h index 04f40ba8..848396af 100644 --- a/firmware/src/adc/adc.h +++ b/firmware/src/adc/adc.h @@ -18,7 +18,7 @@ #ifndef ADC_ADC_H_ #define ADC_ADC_H_ -#include "src/common.h" +#include #ifdef CAFE_ARCH2 typedef enum @@ -77,7 +77,6 @@ typedef enum #elif defined BOARD_REV_M5 #define SHUNT_SCALING_FACTOR (0.008056f) #define ADC_GAIN_VAL GAINx2 - #endif // Inverse of sensing multiplier @@ -86,23 +85,25 @@ typedef enum // VBus scaling factor #define VBUS_SCALING_FACTOR (0.0128f) -struct ADCState +#define I_FILTER_K (0.6f) + +typedef struct { int16_t temp; struct FloatTriplet I_phase_meas; struct FloatTriplet I_phase_offset; -}; +} ADCState; -struct ADCConfig +typedef struct { float Iphase_limit; - float I_filter_k; float I_phase_offset_tau; -}; + float I_phase_offset_k; +} ADCConfig; void ADC_Init(void); -PAC5XXX_RAMFUNC int16_t adc_get_mcu_temp(void); -PAC5XXX_RAMFUNC void ADC_GetPhaseCurrents(struct FloatTriplet *phc); -PAC5XXX_RAMFUNC void ADC_update(void); +int16_t adc_get_mcu_temp(void); +void ADC_GetPhaseCurrents(struct FloatTriplet *phc); +void ADC_update(void); #endif /* ADC_ADC_H_ */ diff --git a/firmware/src/can/can.c b/firmware/src/can/can.c index b64044d1..0a86d1c8 100644 --- a/firmware/src/can/can.c +++ b/firmware/src/can/can.c @@ -152,7 +152,7 @@ void CAN_restore_config(CANConfig *config_) config = *config_; } -void CAN_task(void) { +PAC5XXX_RAMFUNC void CAN_task(void) { // Transmit heartbeat const uint32_t msg_diff = msTicks - state.last_msg_ms; if (msg_diff >= config.heartbeat_period && PAC55XX_CAN->SR.TBS != 0) diff --git a/firmware/src/can/can_func.c b/firmware/src/can/can_func.c index db6e9f9e..c4689fb1 100644 --- a/firmware/src/can/can_func.c +++ b/firmware/src/can/can_func.c @@ -169,7 +169,7 @@ void can_baud(CAN_BAUD_TYPE baud) } } -void can_process_standard(void) +PAC5XXX_RAMFUNC void can_process_standard(void) { uint32_t buffer = PAC55XX_CAN->RXBUF; // read RX buffer, RX buffer bit order same as TX buffer @@ -212,7 +212,7 @@ void can_process_standard(void) // * You should have received a copy of the GNU General Public License // * along with this program. If not, see . -void can_process_extended(void) +PAC5XXX_RAMFUNC void can_process_extended(void) { uint32_t buffer = PAC55XX_CAN->RXBUF; // read RX buffer, RX buffer bit order same as TX buffer @@ -244,7 +244,7 @@ void can_process_extended(void) } } -void can_transmit_standard(uint8_t dataLen, uint16_t id, const uint8_t * data) +PAC5XXX_RAMFUNC void can_transmit_standard(uint8_t dataLen, uint16_t id, const uint8_t * data) { while (PAC55XX_CAN->SR.TBS == 0) {}; // wait for TX buffer free PAC55XX_CAN->TXBUF = (dataLen << 0) | // DLC - Data Length Code @@ -288,7 +288,7 @@ void can_transmit_standard(uint8_t dataLen, uint16_t id, const uint8_t * data) // * You should have received a copy of the GNU General Public License // * along with this program. If not, see . -void can_transmit_extended(uint8_t dataLen, uint32_t id, const uint8_t * data) +PAC5XXX_RAMFUNC void can_transmit_extended(uint8_t dataLen, uint32_t id, const uint8_t * data) { while (PAC55XX_CAN->SR.TBS == 0) {}; // wait for TX buffer free PAC55XX_CAN->TXBUF = (dataLen << 0) | // DLC - Data Length Code @@ -412,13 +412,13 @@ CAN_BAUD_TYPE CAN_IntToBaudType(uint16_t baud) return ret; } -void ids_from_arbitration(uint32_t arb_id, uint32_t* ep_id, uint32_t* seq_id) +inline void ids_from_arbitration(uint32_t arb_id, uint32_t* ep_id, uint32_t* seq_id) { *ep_id = arb_id & CAN_EP_MASK; *seq_id = (arb_id & CAN_SEQ_MASK) >> CAN_EP_SIZE; } -void arbitration_from_ids(uint32_t* arb_id, uint32_t ep_id, uint32_t seq_id, uint32_t node_id) +inline void arbitration_from_ids(uint32_t* arb_id, uint32_t ep_id, uint32_t seq_id, uint32_t node_id) { *arb_id = (ep_id & CAN_EP_MASK) | ((seq_id << CAN_EP_SIZE) & CAN_SEQ_MASK) | ((node_id << (CAN_EP_SIZE + CAN_SEQ_SIZE)) & CAN_DEV_MASK); } diff --git a/firmware/src/controller/controller.c b/firmware/src/controller/controller.c index 9ef2da85..5fc647a4 100644 --- a/firmware/src/controller/controller.c +++ b/firmware/src/controller/controller.c @@ -29,7 +29,7 @@ #include "src/watchdog/watchdog.h" void CLControlStep(void); -PAC5XXX_RAMFUNC static inline bool Controller_LimitVelocity(float min_limit, float max_limit, float vel_estimate, +static inline bool Controller_LimitVelocity(float min_limit, float max_limit, float vel_estimate, float vel_gain, float *I); static struct FloatTriplet zeroDC = {0.5f, 0.5f, 0.5f}; @@ -155,7 +155,7 @@ void Controller_ControlLoop(void) } } - void CLControlStep(void) +PAC5XXX_RAMFUNC void CLControlStep(void) { if (state.mode >= CTRL_TRAJECTORY) { @@ -278,7 +278,7 @@ void Controller_ControlLoop(void) float mod_q = Vq / Vbus_voltage; float mod_d = Vd / Vbus_voltage; state.Ibus_est = state.Iq_est * mod_q + state.Id_est * mod_d; - state.power_est = state.Iq_est * Vbus_voltage; + state.power_est = state.Ibus_est * Vbus_voltage; // dq modulation limiter const float dq_mod_scale_factor = PWM_LIMIT * fast_inv_sqrt((mod_q * mod_q) + (mod_d * mod_d)); diff --git a/firmware/src/motor/calibration.h b/firmware/src/motor/calibration.h index 84a56aff..29ce3f8a 100644 --- a/firmware/src/motor/calibration.h +++ b/firmware/src/motor/calibration.h @@ -30,7 +30,7 @@ #define CAL_V_GAIN (0.0005f) #define CAL_V_INDUCTANCE (2.0f) #elif defined BOARD_REV_M5 -#define CAL_V_GAIN (0.001f) +#define CAL_V_GAIN (0.002f) #define CAL_V_INDUCTANCE (5.0f) #endif diff --git a/firmware/src/scheduler/scheduler.c b/firmware/src/scheduler/scheduler.c index dcff1dd2..0ee4c1aa 100644 --- a/firmware/src/scheduler/scheduler.c +++ b/firmware/src/scheduler/scheduler.c @@ -35,10 +35,10 @@ SchedulerState state = {0}; void WaitForControlLoopInterrupt(void) { - state.busy_cycles = DWT->CYCCNT - state.busy_loop_start; + while (!state.adc_interrupt) { - state.busy = false; + if (state.can_interrupt) { // Handle CAN @@ -62,6 +62,8 @@ void WaitForControlLoopInterrupt(void) } else { + state.busy = false; + state.busy_cycles = DWT->CYCCNT - state.busy_loop_start; // Go back to sleep __DSB(); __ISB(); @@ -91,7 +93,7 @@ void ADC_IRQHandler(void) // the control deadline is not missed, // i.e. the previous control loop is complete prior // to the ADC triggering the next - if (true == gate_driver_is_enabled() && true == state.busy) + if ((gate_driver_is_enabled() == true) && (state.busy == true)) { state.errors |= SCHEDULER_ERRORS_CONTROL_BLOCK_REENTERED; // We do not change the control state here, to @@ -113,9 +115,9 @@ void CAN_IRQHandler(void) state.can_interrupt = true; } -void SysTick_Handler(void) /* SysTick interrupt Handler. */ +void SysTick_Handler(void) { - msTicks = msTicks + 1; /* See startup file startup_LPC17xx.s for SysTick vector */ + msTicks = msTicks + 1; CAN_task(); } @@ -132,12 +134,12 @@ void Wdt_IRQHandler(void) PAC55XX_WWDT->WWDTFLAG.IF = 1; } -uint32_t Scheduler_GetTotalCycles(void) +PAC5XXX_RAMFUNC uint32_t Scheduler_GetTotalCycles(void) { return state.total_cycles; } -uint32_t Scheduler_GetBusyCycles(void) +PAC5XXX_RAMFUNC uint32_t Scheduler_GetBusyCycles(void) { return state.busy_cycles; }