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
tinymovr · Nov 24, 2022 · fe0abba · fe0abba
1 parent db4cdce
commit fe0abba
Show file tree

Hide file tree

Showing 7 changed files with 61 additions and 58 deletions.
diff --git a/firmware/src/adc/adc.c b/firmware/src/adc/adc.c
@@ -18,11 +18,9 @@
 #include <math.h>
 #include <string.h>
 #include <src/motor/motor.h>
+#include <src/gatedriver/gatedriver.h>
 #include <src/adc/adc.h>
 
-// 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
@@ -18,7 +18,7 @@
 #ifndef ADC_ADC_H_
 #define ADC_ADC_H_
 
-#include "src/common.h"
+#include <src/common.h>
 
 #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
@@ -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
@@ -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 <http://www.gnu.org/licenses/>.
 
-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 <http://www.gnu.org/licenses/>.
 
-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
@@ -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
@@ -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
@@ -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;
 }