Field Weakening and MTPA support

confgenerator.c

@@ -71,6 +71,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
 	buffer_append_float32_auto(buffer, conf->foc_pll_kp, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_pll_ki, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_motor_l, &ind);
+	buffer_append_float32_auto(buffer, conf->foc_motor_ld_lq_diff, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_motor_r, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_motor_flux_linkage, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_observer_gain, &ind);
@@ -106,6 +107,11 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
 	buffer_append_uint16(buffer, conf->foc_hfi_start_samples, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_hfi_obs_ovr_sec, &ind);
 	buffer[ind++] = conf->foc_hfi_samples;
+	buffer[ind++] = conf->foc_field_weakening_enable;
+	buffer_append_float32_auto(buffer, conf->foc_field_weakening_kp, &ind);
+	buffer_append_float32_auto(buffer, conf->foc_field_weakening_ki, &ind);
+	buffer_append_float32_auto(buffer, conf->foc_field_weakening_mod_threshold, &ind);
+	buffer_append_float32_auto(buffer, conf->foc_field_weakening_d_current_factor, &ind);
 	buffer_append_int16(buffer, conf->gpd_buffer_notify_left, &ind);
 	buffer_append_int16(buffer, conf->gpd_buffer_interpol, &ind);
 	buffer_append_float32_auto(buffer, conf->gpd_current_filter_const, &ind);
@@ -346,6 +352,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
 	conf->foc_pll_kp = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_pll_ki = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_motor_l = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_motor_ld_lq_diff = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_motor_r = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_motor_flux_linkage = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_observer_gain = buffer_get_float32_auto(buffer, &ind);
@@ -381,6 +388,11 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
 	conf->foc_hfi_start_samples = buffer_get_uint16(buffer, &ind);
 	conf->foc_hfi_obs_ovr_sec = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_hfi_samples = buffer[ind++];
+	conf->foc_field_weakening_enable = buffer[ind++];
+	conf->foc_field_weakening_kp = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_field_weakening_ki = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_field_weakening_mod_threshold = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_field_weakening_d_current_factor = buffer_get_float32_auto(buffer, &ind);
 	conf->gpd_buffer_notify_left = buffer_get_int16(buffer, &ind);
 	conf->gpd_buffer_interpol = buffer_get_int16(buffer, &ind);
 	conf->gpd_current_filter_const = buffer_get_float32_auto(buffer, &ind);
@@ -617,6 +629,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
 	conf->foc_pll_kp = MCCONF_FOC_PLL_KP;
 	conf->foc_pll_ki = MCCONF_FOC_PLL_KI;
 	conf->foc_motor_l = MCCONF_FOC_MOTOR_L;
+	conf->foc_motor_ld_lq_diff = MCCONF_FOC_MOTOR_LD_LQ_DIFF;
 	conf->foc_motor_r = MCCONF_FOC_MOTOR_R;
 	conf->foc_motor_flux_linkage = MCCONF_FOC_MOTOR_FLUX_LINKAGE;
 	conf->foc_observer_gain = MCCONF_FOC_OBSERVER_GAIN;
@@ -652,6 +665,11 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
 	conf->foc_hfi_start_samples = MCCONF_FOC_HFI_START_SAMPLES;
 	conf->foc_hfi_obs_ovr_sec = MCCONF_FOC_HFI_OBS_OVR_SEC;
 	conf->foc_hfi_samples = MCCONF_FOC_HFI_SAMPLES;
+	conf->foc_field_weakening_enable = MCCONF_FOC_FIELD_WEAKENING_ENABLE;
+	conf->foc_field_weakening_kp = MCCONF_FOC_FIELD_WEAKENING_KP;
+	conf->foc_field_weakening_ki = MCCONF_FOC_FIELD_WEAKENING_KI;
+	conf->foc_field_weakening_mod_threshold = MCCONF_FOC_FIELD_WEAKENING_MOD_THRESHOLD;
+	conf->foc_field_weakening_d_current_factor = MCCONF_FOC_FIELD_WEAKENING_D_CURRENT_FACTOR;
 	conf->gpd_buffer_notify_left = MCCONF_GPD_BUFFER_NOTIFY_LEFT;
 	conf->gpd_buffer_interpol = MCCONF_GPD_BUFFER_INTERPOL;
 	conf->gpd_current_filter_const = MCCONF_GPD_CURRENT_FILTER_CONST;

confgenerator.h

@@ -8,7 +8,7 @@
 #include <stdbool.h>
 
 // Constants
-#define MCCONF_SIGNATURE		3632471335
+#define MCCONF_SIGNATURE		3945830766
 #define APPCONF_SIGNATURE		2964134872
 
 // Functions

datatypes.h

@@ -137,6 +137,7 @@ typedef enum {
 	CONTROL_MODE_OPENLOOP_PHASE,
 	CONTROL_MODE_OPENLOOP_DUTY,
 	CONTROL_MODE_OPENLOOP_DUTY_PHASE,
+	CONTROL_MODE_RAMP_DOWN_ID,
 	CONTROL_MODE_NONE
 } mc_control_mode;
 
@@ -276,6 +277,9 @@ typedef struct {
 	float foc_encoder_cos_gain;
 	float foc_encoder_sincos_filter_constant;
 	float foc_motor_l;
+	float foc_motor_ld;
+	float foc_motor_lq;
+	float foc_motor_ld_lq_diff;
 	float foc_motor_r;
 	float foc_motor_flux_linkage;
 	float foc_observer_gain;
@@ -298,6 +302,12 @@ typedef struct {
 	bool foc_temp_comp;
 	float foc_temp_comp_base_temp;
 	float foc_current_filter_const;
+	// FW config
+	bool foc_field_weakening_enable;
+	float foc_field_weakening_kp;
+	float foc_field_weakening_ki;
+	float foc_field_weakening_mod_threshold;
+	float foc_field_weakening_d_current_factor;
 	mc_foc_cc_decoupling_mode foc_cc_decoupling;
 	mc_foc_observer_type foc_observer_type;
 	float foc_hfi_voltage_start;

mc_interface.c

@@ -56,6 +56,7 @@ static int m_ignore_iterations;
 static volatile unsigned int m_cycles_running;
 static volatile bool m_lock_enabled;
 static volatile bool m_lock_override_once;
+static volatile bool m_disable_encoder_faults;
 static volatile float m_motor_current_sum;
 static volatile float m_input_current_sum;
 static volatile float m_motor_current_iterations;
@@ -124,6 +125,7 @@ void mc_interface_init(mc_configuration *configuration) {
 	m_cycles_running = 0;
 	m_lock_enabled = false;
 	m_lock_override_once = false;
+	m_disable_encoder_faults = false;
 	m_motor_current_sum = 0.0;
 	m_input_current_sum = 0.0;
 	m_motor_current_iterations = 0.0;
@@ -400,6 +402,14 @@ mc_fault_code mc_interface_get_fault(void) {
 	return m_fault_now;
 }
 
+void mc_disable_encoder_faults(void) {
+	m_disable_encoder_faults = true;
+}
+
+void mc_enable_encoder_faults(void) {
+	m_disable_encoder_faults = false;
+}
+
 const char* mc_interface_fault_to_string(mc_fault_code fault) {
 	switch (fault) {
 	case FAULT_CODE_NONE: return "FAULT_CODE_NONE"; break;
@@ -2012,35 +2022,36 @@ static THD_FUNCTION(timer_thread, arg) {
 				break;
 		}
 
+		if (m_disable_encoder_faults == false) {
+			// Trigger encoder error rate fault, using 1% errors as threshold.
+			// Relevant only in FOC mode with encoder enabled
+			if(m_conf.motor_type == MOTOR_TYPE_FOC &&
+				m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
+				mcpwm_foc_is_using_encoder() &&
+				encoder_spi_get_error_rate() > 0.05) {
+				mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI);
+			}
 
-		// Trigger encoder error rate fault, using 1% errors as threshold.
-		// Relevant only in FOC mode with encoder enabled
-		if(m_conf.motor_type == MOTOR_TYPE_FOC &&
-			m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
-			mcpwm_foc_is_using_encoder() &&
-			encoder_spi_get_error_rate() > 0.05) {
-			mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI);
-		}
-
-		if(m_conf.motor_type == MOTOR_TYPE_FOC &&
-			m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
-			m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
+			if(m_conf.motor_type == MOTOR_TYPE_FOC &&
+				m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
+				m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
 
-			if (encoder_sincos_get_signal_below_min_error_rate() > 0.05)
-				mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE);
-			if (encoder_sincos_get_signal_above_max_error_rate() > 0.05)
-				mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE);
-		}
+				if (encoder_sincos_get_signal_below_min_error_rate() > 0.05)
+					mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE);
+				if (encoder_sincos_get_signal_above_max_error_rate() > 0.05)
+					mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE);
+			}
 
-		if(m_conf.motor_type == MOTOR_TYPE_FOC &&
-			m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
-			m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
-			if (encoder_resolver_loss_of_tracking_error_rate() > 0.05)
-				mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT);
-			if (encoder_resolver_degradation_of_signal_error_rate() > 0.05)
-				mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS);
-			if (encoder_resolver_loss_of_signal_error_rate() > 0.04)
-				mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS);
+			if(m_conf.motor_type == MOTOR_TYPE_FOC &&
+				m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
+				m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
+				if (encoder_resolver_loss_of_tracking_error_rate() > 0.05)
+					mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT);
+				if (encoder_resolver_degradation_of_signal_error_rate() > 0.05)
+					mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS);
+				if (encoder_resolver_loss_of_signal_error_rate() > 0.04)
+					mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS);
+			}
 		}
 		// TODO: Implement for BLDC and GPDRIVE
 		if(m_conf.motor_type == MOTOR_TYPE_FOC) {

mc_interface.h

@@ -31,6 +31,8 @@ void mc_interface_set_pwm_callback(void (*p_func)(void));
 void mc_interface_lock(void);
 void mc_interface_unlock(void);
 void mc_interface_lock_override_once(void);
+void mc_disable_encoder_faults(void);
+void mc_enable_encoder_faults(void);
 mc_fault_code mc_interface_get_fault(void);
 const char* mc_interface_fault_to_string(mc_fault_code fault);
 mc_state mc_interface_get_state(void);

mcconf/mcconf_default.h

@@ -251,6 +251,15 @@
 #ifndef MCCONF_FOC_MOTOR_L
 #define MCCONF_FOC_MOTOR_L				0.000007
 #endif
+#ifndef MCCONF_FOC_MOTOR_LD
+#define MCCONF_FOC_MOTOR_LD				0.00002
+#endif
+#ifndef MCCONF_FOC_MOTOR_LQ
+#define MCCONF_FOC_MOTOR_LQ				0.00002
+#endif
+#ifndef MCCONF_FOC_MOTOR_LD_LQ_DIFF
+#define MCCONF_FOC_MOTOR_LD_LQ_DIFF		0.0
+#endif
 #ifndef MCCONF_FOC_MOTOR_R
 #define MCCONF_FOC_MOTOR_R				0.015
 #endif
@@ -329,6 +338,24 @@
 #ifndef MCCONF_FOC_CURRENT_FILTER_CONST
 #define MCCONF_FOC_CURRENT_FILTER_CONST	0.1		// Filter constant for the filtered currents
 #endif
+#ifndef MCCONF_FOC_MTPA_ENABLE
+#define MCCONF_FOC_MTPA_ENABLE				false	// MTPA Enable
+#endif
+#ifndef MCCONF_FOC_FIELD_WEAKENING_ENABLE
+#define MCCONF_FOC_FIELD_WEAKENING_ENABLE	false	// Field Weakening enable
+#endif
+#ifndef MCCONF_FOC_FIELD_WEAKENING_KP
+#define MCCONF_FOC_FIELD_WEAKENING_KP		0.02	// Field Weakening proportional gain
+#endif
+#ifndef MCCONF_FOC_FIELD_WEAKENING_KI
+#define MCCONF_FOC_FIELD_WEAKENING_KI		0.02	// Field Weakening integral gain
+#endif
+#ifndef MCCONF_FOC_FIELD_WEAKENING_MOD_THRESHOLD
+#define MCCONF_FOC_FIELD_WEAKENING_MOD_THRESHOLD		0.81	// Modulation above which FW will be used
+#endif
+#ifndef MCCONF_FOC_FIELD_WEAKENING_D_CURRENT_FACTOR
+#define MCCONF_FOC_FIELD_WEAKENING_D_CURRENT_FACTOR		0.3		// Max FW D current as a % of max phase current
+#endif
 #ifndef MCCONF_FOC_CC_DECOUPLING
 #define MCCONF_FOC_CC_DECOUPLING		FOC_CC_DECOUPLING_BEMF // Current controller decoupling
 #endif