Added MXV observer

confgenerator.h

@@ -8,7 +8,7 @@
 #include <stdbool.h>
 
 // Constants
-#define MCCONF_SIGNATURE		2490415037
+#define MCCONF_SIGNATURE		1065524471
 #define APPCONF_SIGNATURE		2099347128
 
 // Functions

datatypes.h

@@ -128,6 +128,9 @@ typedef enum {
 	FOC_OBSERVER_MXLEMMING,
 	FOC_OBSERVER_ORTEGA_LAMBDA_COMP,
 	FOC_OBSERVER_MXLEMMING_LAMBDA_COMP,
+	FOC_OBSERVER_MXV,
+	FOC_OBSERVER_MXV_LAMBDA_COMP,
+	FOC_OBSERVER_MXV_LAMBDA_COMP_LIN,
 } mc_foc_observer_type;
 
 typedef enum {

motor/foc_math.c

@@ -101,6 +101,9 @@ void foc_observer_update(float v_alpha, float v_beta, float i_alpha, float i_bet
 
 	case FOC_OBSERVER_MXLEMMING:
 	case FOC_OBSERVER_MXLEMMING_LAMBDA_COMP:
+	case FOC_OBSERVER_MXV:
+	case FOC_OBSERVER_MXV_LAMBDA_COMP:
+	case FOC_OBSERVER_MXV_LAMBDA_COMP_LIN:
 		// LICENCE NOTE:
 		// This function deviates slightly from the BSD 3 clause licence.
 		// The work here is entirely original to the MESC FOC project, and not based
@@ -114,24 +117,48 @@ void foc_observer_update(float v_alpha, float v_beta, float i_alpha, float i_bet
 		state->x1 += (v_alpha - R_ia) * dt - L * (i_alpha - state->i_alpha_last);
 		state->x2 += (v_beta - R_ib) * dt - L * (i_beta - state->i_beta_last);
 
-		if (conf_now->foc_observer_type == FOC_OBSERVER_MXLEMMING_LAMBDA_COMP) {
-			// This is essentially the flux linkage observer from
-			// https://cas.mines-paristech.fr/~praly/Telechargement/Conferences/2017_IFAC_Bernard-Praly.pdf
-			// with a slight modification. We use the same gain here as it is related to the Ortega-gain,
-			// but we scale it down as it is not nearly as critical because the flux linkage is mostly DC.
-			// When the motor starts to saturate we still want to be able to keep up though, so the gain is
-			// still high enough to react with some "reasonable" speed.
-			float err = SQ(state->lambda_est) - (SQ(state->x1) + SQ(state->x2));
-			state->lambda_est += 0.1 * gamma_half * state->lambda_est * -err * dt;
-
-			// Clamp the observed flux linkage (not sure if this is needed)
-			utils_truncate_number(&(state->lambda_est), lambda * 0.3, lambda * 2.5);
-
-			utils_truncate_number_abs(&(state->x1), state->lambda_est);
-			utils_truncate_number_abs(&(state->x2), state->lambda_est);
+		if (conf_now->foc_observer_type == FOC_OBSERVER_MXLEMMING_LAMBDA_COMP ||
+				conf_now->foc_observer_type == FOC_OBSERVER_MXV_LAMBDA_COMP ||
+				conf_now->foc_observer_type == FOC_OBSERVER_MXV_LAMBDA_COMP_LIN) {
+
+			if (conf_now->foc_observer_type == FOC_OBSERVER_MXV_LAMBDA_COMP_LIN) {
+				float mag = NORM2_f(state->x1, state->x2);
+				UTILS_LP_FAST(state->lambda_est, mag, 0.1 * gamma_half * dt * SQ(state->lambda_est));
+				utils_truncate_number(&(state->lambda_est), lambda * 0.3, lambda * 2.5);
+
+				if (mag > state->lambda_est) {
+					state->x1 = (state->x1 / mag) * state->lambda_est;
+					state->x2 = (state->x2 / mag) * state->lambda_est;
+				}
+			} else if (conf_now->foc_observer_type == FOC_OBSERVER_MXV_LAMBDA_COMP) {
+				float err = SQ(state->lambda_est) - (SQ(state->x1) + SQ(state->x2));
+				state->lambda_est += 0.1 * gamma_half * state->lambda_est * -err * dt;
+				utils_truncate_number(&(state->lambda_est), lambda * 0.3, lambda * 2.5);
+
+				float mag = NORM2_f(state->x1, state->x2);
+				if (mag > state->lambda_est) {
+					state->x1 = (state->x1 / mag) * state->lambda_est;
+					state->x2 = (state->x2 / mag) * state->lambda_est;
+				}
+			} else {
+				float err = SQ(state->lambda_est) - (SQ(state->x1) + SQ(state->x2));
+				state->lambda_est += 0.1 * gamma_half * state->lambda_est * -err * dt;
+				utils_truncate_number(&(state->lambda_est), lambda * 0.3, lambda * 2.5);
+
+				utils_truncate_number_abs(&(state->x1), state->lambda_est);
+				utils_truncate_number_abs(&(state->x2), state->lambda_est);
+			}
 		} else {
-			utils_truncate_number_abs(&(state->x1), lambda);
-			utils_truncate_number_abs(&(state->x2), lambda);
+			if (conf_now->foc_observer_type == FOC_OBSERVER_MXV) {
+				float mag = NORM2_f(state->x1, state->x2);
+				if (mag > lambda) {
+					state->x1 = (state->x1 / mag) * lambda;
+					state->x2 = (state->x2 / mag) * lambda;
+				}
+			} else {
+				utils_truncate_number_abs(&(state->x1), lambda);
+				utils_truncate_number_abs(&(state->x2), lambda);
+			}
 		}
 
 		// Set these to 0 to allow using the same atan2-code as for Ortega
@@ -182,8 +209,8 @@ void foc_observer_update(float v_alpha, float v_beta, float i_alpha, float i_bet
 	}
 
 	// Can we clamp the flux in dq with q flux = 0 and d flux is lambda
-	// Then the state->x1 and state->x2 (which are the alpha and betan fluxes) are set as lambda*sin and lambda*cos
-	// The d flux each time would have a residual after tranform from ab to dq. This can be used as an input to the flux estimator
+	// Then the state->x1 and state->x2 (which are the alpha and beta fluxes) are set as lambda*sin and lambda*cos
+	// The d flux each time would have a residual after transform from ab to dq. This can be used as an input to the flux estimator
 }
 
 void foc_pll_run(float phase, float dt, float *phase_var,