Merge pull request zephyrproject-rtos#35 from uLipe/bugfix/mahony_pi_…

…windup fusion: mahony: add PI controller integral limit
CS-CW · Jun 16, 2022 · a54986a · a54986a
2 parents fd5a62f + 226185a
commit a54986a
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 4 deletions.
diff --git a/include/zsl/orientation/fusion/mahony.h b/include/zsl/orientation/fusion/mahony.h
@@ -48,6 +48,11 @@ struct zsl_fus_mahn_cfg {
 	 */
 	zsl_real_t ki;
 
+	/**
+	 * @brief Integral limit for the integrator to avoid windup. Must be greater than 0
+	 */
+	zsl_real_t integral_limit;
+
 	/**
 	 * @brief Integral feedback vector, which is updated every iteration.
 	 *        Its initial value must be (0, 0, 0).

diff --git a/samples/orientation/apitest/src/main.c b/samples/orientation/apitest/src/main.c
@@ -29,6 +29,7 @@ static zsl_real_t _mahn_intfb[3] = { 0.0, 0.0, 0.0 };
 static struct zsl_fus_mahn_cfg mahn_cfg = {
 	.kp = 0.235,
 	.ki = 0.02,
+	.integral_limit = 10000.0f,
 	.intfb = {
 		.sz = 3,
 		.data = _mahn_intfb,

diff --git a/src/orientation/fusion/mahony.c b/src/orientation/fusion/mahony.c
@@ -14,7 +14,9 @@ static uint32_t zsl_fus_mahn_freq;
 
 static int zsl_fus_mahony_imu(struct zsl_vec *g, struct zsl_vec *a,
 			      zsl_real_t *Kp, zsl_real_t *Ki,
-			      struct zsl_vec *integralFB, zsl_real_t *incl,
+			      struct zsl_vec *integralFB, 
+				  zsl_real_t integral_limit,
+				  zsl_real_t *incl,
 			      struct zsl_quat *q)
 {
 	int rc = 0;
@@ -68,6 +70,25 @@ static int zsl_fus_mahony_imu(struct zsl_vec *g, struct zsl_vec *a,
 		integralFB->data[1] += e.data[1] * (1.0 / zsl_fus_mahn_freq);
 		integralFB->data[2] += e.data[2] * (1.0 / zsl_fus_mahn_freq);
 
+		/* Limit integral values */
+		if(integralFB->data[0] > integral_limit) {
+			integralFB->data[0] = integral_limit;
+		} else if (integralFB->data[0] < -integral_limit) {
+			integralFB->data[0] = -integral_limit;
+		}
+
+		if(integralFB->data[1] > integral_limit) {
+			integralFB->data[1] = integral_limit;
+		} else if (integralFB->data[1] < -integral_limit) {
+			integralFB->data[1] = -integral_limit;
+		}
+
+		if(integralFB->data[2] > integral_limit) {
+			integralFB->data[2] = integral_limit;
+		} else if (integralFB->data[2] < -integral_limit) {
+			integralFB->data[2] = -integral_limit;
+		}
+
 		/* Apply integral feedback multiplied by Ki. */
 		g->data[0] += *Ki * integralFB->data[0];
 		g->data[1] += *Ki * integralFB->data[1];
@@ -92,7 +113,8 @@ static int zsl_fus_mahony_imu(struct zsl_vec *g, struct zsl_vec *a,
 
 static int zsl_fus_mahony(struct zsl_vec *g, struct zsl_vec *a,
 			  struct zsl_vec *m, zsl_real_t *Kp, zsl_real_t *Ki,
-			  struct zsl_vec *integralFB, zsl_real_t *incl, struct zsl_quat *q)
+			  struct zsl_vec *integralFB,
+			  zsl_real_t integral_limit, zsl_real_t *incl, struct zsl_quat *q)
 {
 	int rc = 0;
 
@@ -112,7 +134,7 @@ static int zsl_fus_mahony(struct zsl_vec *g, struct zsl_vec *a,
 
 	/* Use IMU algorithm if the magnetometer measurement is invalid. */
 	if ((m == NULL) || (ZSL_ABS(zsl_vec_norm(m)) < 1E-6)) {
-		return zsl_fus_mahony_imu(g, a, Kp, Ki, integralFB, incl, q);
+		return zsl_fus_mahony_imu(g, a, Kp, Ki, integralFB, integral_limit, incl, q);
 	}
 
 	/* Continue with the calculations only if the data from the accelerometer
@@ -202,6 +224,25 @@ static int zsl_fus_mahony(struct zsl_vec *g, struct zsl_vec *a,
 		integralFB->data[1] += e.data[1] * (1.0 / zsl_fus_mahn_freq);
 		integralFB->data[2] += e.data[2] * (1.0 / zsl_fus_mahn_freq);
 
+		/* Limit integral values */
+		if(integralFB->data[0] > integral_limit) {
+			integralFB->data[0] = integral_limit;
+		} else if (integralFB->data[0] < -integral_limit) {
+			integralFB->data[0] = -integral_limit;
+		}
+
+		if(integralFB->data[1] > integral_limit) {
+			integralFB->data[1] = integral_limit;
+		} else if (integralFB->data[1] < -integral_limit) {
+			integralFB->data[1] = -integral_limit;
+		}
+
+		if(integralFB->data[2] > integral_limit) {
+			integralFB->data[2] = integral_limit;
+		} else if (integralFB->data[2] < -integral_limit) {
+			integralFB->data[2] = -integral_limit;
+		}
+
 		/* Apply integral feedback multiplied by Ki. */
 		g->data[0] += *Ki * integralFB->data[0];
 		g->data[1] += *Ki * integralFB->data[1];
@@ -256,7 +297,7 @@ int zsl_fus_mahn_feed(struct zsl_vec *a, struct zsl_vec *m, struct zsl_vec *g,
 	}
 
 	return zsl_fus_mahony(g, a, m, &(mcfg->kp), &(mcfg->ki), &(mcfg->intfb),
-			      incl, q);
+			      mcfg->integral_limit, incl, q);
 }
 
 void zsl_fus_mahn_error(int error)