Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Small fix for airspeed consistency checks (#3494)
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details

Browse Source 
* 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>
This commit is contained in:
NoahWe
2025-07-02 16:21:47 +02:00
committed by GitHub
parent b4ed21ae1e
commit 0c60967b3b
3 changed files with 15 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_circfit_float.c
+12 -4
View File
@@ -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
sw/airborne/math/pprz_circfit_float.h
+2 -1
View File
@@ -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 {
sw/airborne/modules/checks/airspeed_consistency.c
+1
View File
@@ -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"