Added saturation compensation modes

conf_general.h

@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			6
 #define FW_VERSION_MINOR			00
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		49
+#define FW_TEST_VERSION_NUMBER		50
 
 #include "datatypes.h"
 

confgenerator.c

@@ -104,6 +104,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
 	buffer_append_float32_auto(buffer, conf->foc_sl_erpm, &ind);
 	buffer[ind++] = conf->foc_sample_v0_v7;
 	buffer[ind++] = conf->foc_sample_high_current;
+	buffer[ind++] = conf->foc_sat_comp_mode;
 	buffer_append_float16(buffer, conf->foc_sat_comp, 1000, &ind);
 	buffer[ind++] = conf->foc_temp_comp;
 	buffer_append_float16(buffer, conf->foc_temp_comp_base_temp, 100, &ind);
@@ -493,6 +494,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
 	conf->foc_sl_erpm = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_sample_v0_v7 = buffer[ind++];
 	conf->foc_sample_high_current = buffer[ind++];
+	conf->foc_sat_comp_mode = buffer[ind++];
 	conf->foc_sat_comp = buffer_get_float16(buffer, 1000, &ind);
 	conf->foc_temp_comp = buffer[ind++];
 	conf->foc_temp_comp_base_temp = buffer_get_float16(buffer, 100, &ind);
@@ -878,6 +880,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
 	conf->foc_sl_erpm = MCCONF_FOC_SL_ERPM;
 	conf->foc_sample_v0_v7 = MCCONF_FOC_SAMPLE_V0_V7;
 	conf->foc_sample_high_current = MCCONF_FOC_SAMPLE_HIGH_CURRENT;
+	conf->foc_sat_comp_mode = MCCONF_FOC_SAT_COMP_MODE;
 	conf->foc_sat_comp = MCCONF_FOC_SAT_COMP;
 	conf->foc_temp_comp = MCCONF_FOC_TEMP_COMP;
 	conf->foc_temp_comp_base_temp = MCCONF_FOC_TEMP_COMP_BASE_TEMP;

confgenerator.h

@@ -8,7 +8,7 @@
 #include <stdbool.h>
 
 // Constants
-#define MCCONF_SIGNATURE		4210917936
+#define MCCONF_SIGNATURE		3188187738
 #define APPCONF_SIGNATURE		992988844
 
 // Functions

datatypes.h

@@ -325,6 +325,13 @@ typedef enum {
 	SPEED_SRC_OBSERVER,
 } SPEED_SRC;
 
+typedef enum {
+	SAT_COMP_DISABLED = 0,
+	SAT_COMP_FACTOR,
+	SAT_COMP_LAMBDA,
+	SAT_COMP_LAMBDA_AND_FACTOR
+} SAT_COMP_MODE;
+
 typedef struct {
 	// Limits
 	float l_current_max;
@@ -422,6 +429,7 @@ typedef struct {
 	float foc_sl_erpm;
 	bool foc_sample_v0_v7;
 	bool foc_sample_high_current;
+	SAT_COMP_MODE foc_sat_comp_mode;
 	float foc_sat_comp;
 	bool foc_temp_comp;
 	float foc_temp_comp_base_temp;

motor/foc_math.c

@@ -32,9 +32,32 @@ void foc_observer_update(float v_alpha, float v_beta, float i_alpha, float i_bet
 	float lambda = conf_now->foc_motor_flux_linkage;
 
 	// Saturation compensation
-	const float comp_fact = conf_now->foc_sat_comp * (motor->m_motor_state.i_abs_filter / conf_now->l_current_max);
-	L -= L * comp_fact;
-	lambda -= lambda * comp_fact;
+	switch(conf_now->foc_sat_comp_mode) {
+	case SAT_COMP_LAMBDA:
+		// Here we assume that the inductance drops by the same amount as the flux linkage. I have
+		// no idea if this is a valid or even a reasonable assumption.
+		if (conf_now->foc_observer_type >= FOC_OBSERVER_ORTEGA_LAMBDA_COMP) {
+			L = L * (state->lambda_est / lambda);
+		}
+		break;
+
+	case SAT_COMP_FACTOR: {
+		const float comp_fact = conf_now->foc_sat_comp * (motor->m_motor_state.i_abs_filter / conf_now->l_current_max);
+		L -= L * comp_fact;
+		lambda -= lambda * comp_fact;
+	} break;
+
+	case SAT_COMP_LAMBDA_AND_FACTOR: {
+		if (conf_now->foc_observer_type >= FOC_OBSERVER_ORTEGA_LAMBDA_COMP) {
+			L = L * (state->lambda_est / lambda);
+		}
+		const float comp_fact = conf_now->foc_sat_comp * (motor->m_motor_state.i_abs_filter / conf_now->l_current_max);
+		L -= L * comp_fact;
+	} break;
+
+	default:
+		break;
+	}
 
 	// Temperature compensation
 	if (conf_now->foc_temp_comp) {

motor/mcconf_default.h

@@ -367,8 +367,11 @@
 #ifndef MCCONF_FOC_SAMPLE_HIGH_CURRENT
 #define MCCONF_FOC_SAMPLE_HIGH_CURRENT	false	// High current sampling mode (requires three shunts)
 #endif
+#ifndef MCCONF_FOC_SAT_COMP_MODE
+#define MCCONF_FOC_SAT_COMP_MODE		SAT_COMP_LAMBDA		// Stator saturation compensation mode
+#endif
 #ifndef MCCONF_FOC_SAT_COMP
-#define MCCONF_FOC_SAT_COMP				0.0		// Stator saturation compensation
+#define MCCONF_FOC_SAT_COMP				0.0		// Stator saturation compensation factor
 #endif
 #ifndef MCCONF_FOC_TEMP_COMP
 #define MCCONF_FOC_TEMP_COMP			false	// Motor temperature compensation