Merge pull request #1548 from bitcraze/rik/kalman_defs_overwrite_param

Kalman: Fix persistent parameters being overwritten at init

Author: gemenerik

src/modules/interface/kalman_core/kalman_core.h

@@ -135,7 +135,19 @@ typedef struct {
   float attitudeReversion;
 } kalmanCoreParams_t;
 
-/*  - Load default parameters */
+/**
+ * @brief Initialize Kalman core parameters with default values
+ *
+ * This function exists primarily for Python bindings to initialize their own
+ * kalmanCoreParams_t structs. In the firmware, default parameters are initialized
+ * via a static initializer in estimator_kalman.c to avoid overwriting persistent
+ * parameters loaded from storage.
+ *
+ * Default values are defined in kalman_core_params_defaults.h to maintain a
+ * single source of truth.
+ *
+ * @param params Pointer to the parameter struct to initialize
+ */
 void kalmanCoreDefaultParams(kalmanCoreParams_t *params);
 
 /*  - Initialize Kalman State */

src/modules/interface/kalman_core/kalman_core_params_defaults.h

@@ -0,0 +1,82 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2025 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+/**
+ * @file kalman_core_params_defaults.h
+ * @brief Single source of truth for Kalman core parameter default values
+ *
+ * This header defines default parameter values as a macro to avoid duplication.
+ * The macro is used in two places:
+ * 1. estimator_kalman.c - Static initializer for firmware (avoids overwriting persistent params)
+ * 2. kalman_core.c - kalmanCoreDefaultParams() function for Python bindings
+ */
+
+// Process noise defaults depend on configuration
+#ifdef CONFIG_ESTIMATOR_KALMAN_GENERAL_PURPOSE
+#define KALMAN_CORE_PROC_NOISE_DEFAULTS \
+  .procNoiseAcc_xy = 0.5f, \
+  .procNoiseAcc_z = 0.5f
+#else
+#define KALMAN_CORE_PROC_NOISE_DEFAULTS \
+  .procNoiseAcc_xy = 0.5f, \
+  .procNoiseAcc_z = 1.0f
+#endif
+
+/**
+ * @brief Macro containing default values for kalmanCoreParams_t
+ *
+ * Use as a designated initializer, e.g.:
+ *   kalmanCoreParams_t params = { KALMAN_CORE_DEFAULT_PARAMS_INIT };
+ */
+#define KALMAN_CORE_DEFAULT_PARAMS_INIT \
+  /* Initial variances, uncertain of position, but know we're stationary and roughly flat */ \
+  .stdDevInitialPosition_xy = 100, \
+  .stdDevInitialPosition_z = 1, \
+  .stdDevInitialVelocity = 0.01, \
+  .stdDevInitialAttitude_rollpitch = 0.01, \
+  .stdDevInitialAttitude_yaw = 0.01, \
+  \
+  KALMAN_CORE_PROC_NOISE_DEFAULTS, \
+  .procNoiseVel = 0, \
+  .procNoisePos = 0, \
+  .procNoiseAtt = 0, \
+  .measNoiseBaro = 2.0f,           /* meters */ \
+  .measNoiseGyro_rollpitch = 0.1f, /* radians per second */ \
+  .measNoiseGyro_yaw = 0.1f,       /* radians per second */ \
+  \
+  .initialX = 0.0, \
+  .initialY = 0.0, \
+  .initialZ = 0.0, \
+  /* Initial yaw of the Crazyflie in radians. */ \
+  /* 0 --- facing positive X */ \
+  /* PI / 2 --- facing positive Y */ \
+  /* PI --- facing negative X */ \
+  /* 3 * PI / 2 --- facing negative Y */ \
+  .initialYaw = 0.0, \
+  \
+  /* Roll/pitch/yaw zero reversion is on by default. Will be overridden by estimatorKalmanInit() if requested by the deck. */ \
+  .attitudeReversion = 0.001f

src/modules/src/estimator/estimator_kalman.c

@@ -59,6 +59,7 @@
  */
 
 #include "kalman_core.h"
+#include "kalman_core_params_defaults.h"
 #include "kalman_supervisor.h"
 
 #include "FreeRTOS.h"
@@ -158,7 +159,9 @@ static OutlierFilterLhState_t sweepOutlierFilterState;
 // Indicates that the internal state is corrupt and should be reset
 bool resetEstimation = false;
 
-static kalmanCoreParams_t coreParams;
+static kalmanCoreParams_t coreParams = {
+  KALMAN_CORE_DEFAULT_PARAMS_INIT
+};
 
 // Data used to enable the task and stabilizer loop to run with minimal locking
 static state_t taskEstimatorState; // The estimator state produced by the task, copied to the stabilizer when needed.
@@ -191,7 +194,6 @@ STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(kalmanTask, KALMAN_TASK_STACKSIZE);
 
 // Called one time during system startup
 void estimatorKalmanTaskInit() {
-  kalmanCoreDefaultParams(&coreParams);
   // It would be logical to set the params->attitudeReversion here, based on deck requirements, but the decks are
   // not initialized yet at this point so it is done in estimatorKalmanInit().
 

src/modules/src/kalman_core/kalman_core.c

@@ -58,6 +58,7 @@
  */
 
 #include "kalman_core.h"
+#include "kalman_core_params_defaults.h"
 #include "cfassert.h"
 #include "autoconf.h"
 
@@ -116,42 +117,12 @@ static void assertStateNotNaN(const kalmanCoreData_t* this)
 // Small number epsilon, to prevent dividing by zero
 #define EPS (1e-6f)
 
+__attribute__((used))
 void kalmanCoreDefaultParams(kalmanCoreParams_t* params)
 {
-  // Initial variances, uncertain of position, but know we're stationary and roughly flat
-  params->stdDevInitialPosition_xy = 100;
-  params->stdDevInitialPosition_z = 1;
-  params->stdDevInitialVelocity = 0.01;
-  params->stdDevInitialAttitude_rollpitch = 0.01;
-  params->stdDevInitialAttitude_yaw = 0.01;
-
-  #ifdef CONFIG_ESTIMATOR_KALMAN_GENERAL_PURPOSE
-  params->procNoiseAcc_xy = 0.5f;
-  params->procNoiseAcc_z = 0.5f;
-  #else
-  params->procNoiseAcc_xy = 0.5f;
-  params->procNoiseAcc_z = 1.0f;
-  #endif
-  params->procNoiseVel = 0;
-  params->procNoisePos = 0;
-  params->procNoiseAtt = 0;
-  params->measNoiseBaro = 2.0f;           // meters
-  params->measNoiseGyro_rollpitch = 0.1f; // radians per second
-  params->measNoiseGyro_yaw = 0.1f;       // radians per second
-
-  params->initialX = 0.0;
-  params->initialY = 0.0;
-  params->initialZ = 0.0;
-
-  // Initial yaw of the Crazyflie in radians.
-  // 0 --- facing positive X
-  // PI / 2 --- facing positive Y
-  // PI --- facing negative X
-  // 3 * PI / 2 --- facing negative Y
-  params->initialYaw = 0.0;
-
-  // Roll/pitch/yaw zero reversion is on by default. Will be overridden by estimator_kalman.c if requested by the deck.
-  params->attitudeReversion = 0.001f;
+  *params = (kalmanCoreParams_t){
+    KALMAN_CORE_DEFAULT_PARAMS_INIT
+  };
 }
 
 void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params, const uint32_t nowMs)