From 0c60967b3b2d9f72a26dbed5fb100dd45446da4a Mon Sep 17 00:00:00 2001
From: NoahWe <74419424+NoahWe@users.noreply.github.com>
Date: Wed, 2 Jul 2025 16:21:47 +0200
Subject: [PATCH] Small fix for airspeed consistency checks (#3494)

* fix warning snprintf for AP and add check for norm in circfit

* make norm cutoff configurable

* Iteration counter overrule fix + different name for forloops and iterations

* insert missing comma

---------

Co-authored-by: Christophe De Wagter <dewagter@gmail.com>
---
 sw/airborne/math/pprz_circfit_float.c            | 16 ++++++++++++----
 sw/airborne/math/pprz_circfit_float.h            |  3 ++-
 .../modules/checks/airspeed_consistency.c        |  1 +
 3 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/sw/airborne/math/pprz_circfit_float.c b/sw/airborne/math/pprz_circfit_float.c
index dfbf2eaf05..96b7880504 100644
--- a/sw/airborne/math/pprz_circfit_float.c
+++ b/sw/airborne/math/pprz_circfit_float.c
@@ -27,6 +27,10 @@
 #define PPRZ_CIRCFIT_ITER_MAX 250
 #endif
 
+#ifndef PPRZ_CIRCFIT_NORM_CUTOFF
+#define PPRZ_CIRCFIT_NORM_CUTOFF 1e-6f
+#endif
+
 enum CircFitStatus_t pprz_circfit_wei_float(struct circle_t *c, const float *x, const float *y, uint16_t n, struct circle_t *g) {
 
   // Check if initial guess is provided
@@ -44,7 +48,7 @@ enum CircFitStatus_t pprz_circfit_wei_float(struct circle_t *c, const float *x,
   float x_prev = 0;
   float y_prev = 0;
   float r_prev = -1;
-  uint16_t i = 0;
+  uint16_t iteration = 0;
     
   while (fabsf(c->r - r_prev) > PPRZ_CIRCFIT_EPSILON || fabsf(c->x - x_prev) > PPRZ_CIRCFIT_EPSILON || fabsf(c->y - y_prev) > PPRZ_CIRCFIT_EPSILON) {
     
@@ -62,6 +66,10 @@ enum CircFitStatus_t pprz_circfit_wei_float(struct circle_t *c, const float *x,
     c->r = sum_norm / n;
     
     for (int i = 0; i < n; i++) {
+      if (norm[i] < PPRZ_CIRCFIT_NORM_CUTOFF)
+      {
+        return CIRC_FIT_NORM_ERROR; // Norm error, too small distance
+      }
       c->x += x[i] + c->r * (x_prev - x[i]) / norm[i];
       c->y += y[i] + c->r * (y_prev - y[i]) / norm[i];
     }
@@ -69,12 +77,12 @@ enum CircFitStatus_t pprz_circfit_wei_float(struct circle_t *c, const float *x,
     c->x /= n;
     c->y /= n;
 
-    if (i >= PPRZ_CIRCFIT_ITER_MAX) {
+    iteration++;
+
+    if (iteration >= PPRZ_CIRCFIT_ITER_MAX) {
       return CIRC_FIT_ITERATION_LIMIT; // Reached iteration limit
     }
 
-    i++;
-    
   }
 
   return CIRC_FIT_OK; // Circle fit successful
diff --git a/sw/airborne/math/pprz_circfit_float.h b/sw/airborne/math/pprz_circfit_float.h
index f0ddfaff6b..fef5ce4b3d 100644
--- a/sw/airborne/math/pprz_circfit_float.h
+++ b/sw/airborne/math/pprz_circfit_float.h
@@ -24,7 +24,8 @@
 enum CircFitStatus_t {
   CIRC_FIT_OK = 0,                  // Circle fit successful
   CIRC_FIT_ERROR = -1,              // Circle fit failed
-  CIRC_FIT_ITERATION_LIMIT = -2     // Circle fit reached iteration limit
+  CIRC_FIT_ITERATION_LIMIT = -2,     // Circle fit reached iteration limit
+  CIRC_FIT_NORM_ERROR = -3,          // Circle fit norm error
 };
 
 struct circle_t {
diff --git a/sw/airborne/modules/checks/airspeed_consistency.c b/sw/airborne/modules/checks/airspeed_consistency.c
index 6b70756314..66ff821f11 100644
--- a/sw/airborne/modules/checks/airspeed_consistency.c
+++ b/sw/airborne/modules/checks/airspeed_consistency.c
@@ -24,6 +24,7 @@
  * Simple consistency check for airspeed measurements while flying circles
  */
 
+#include <stdio.h>
 #include "modules/checks/airspeed_consistency.h"
 #include "navigation.h"
 #include "state.h"