From 35bccefe82e86407aeeda3bef1476683543c27c7 Mon Sep 17 00:00:00 2001 From: Bjorn Date: Fri, 24 Jul 2026 15:40:46 -0700 Subject: [PATCH 01/18] Start Mahony attitude filter with gyro propagation --- mahony/mahony.c | 186 +++++++++++++ mahony/mahony.h | 112 ++++++++ math_sdr/math_sdr.h | 4 +- test/mahony/.gitignore | 6 + test/mahony/Makefile | 106 +++++++ test/mahony/main.h | 8 + test/mahony/test_mahony.c | 562 ++++++++++++++++++++++++++++++++++++++ 7 files changed, 983 insertions(+), 1 deletion(-) create mode 100644 mahony/mahony.c create mode 100644 mahony/mahony.h create mode 100644 test/mahony/.gitignore create mode 100644 test/mahony/Makefile create mode 100644 test/mahony/main.h create mode 100644 test/mahony/test_mahony.c diff --git a/mahony/mahony.c b/mahony/mahony.c new file mode 100644 index 0000000..cc6c9b3 --- /dev/null +++ b/mahony/mahony.c @@ -0,0 +1,186 @@ +/******************************************************************************* + * + * FILE: + * mahony.c + * + * DESCRIPTION: + * Mahony attitude filter implementation. + * + ******************************************************************************/ + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include +#include + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "mahony.h" + +/*------------------------------------------------------------------------------ + Private Functions + ------------------------------------------------------------------------------*/ + +/** + * @brief Determines whether every quaternion component is finite. + */ +static bool mahony_quat_is_finite + ( + QUAT quaternion + ) +{ +return + ( + isfinite(quaternion.w) && + isfinite(quaternion.x) && + isfinite(quaternion.y) && + isfinite(quaternion.z) + ); + +} /* mahony_quat_is_finite */ + + +/** + * @brief Determines whether every vector component is finite. + */ +static bool mahony_vector_is_finite + ( + VECTOR_3F vector + ) +{ +return + ( + isfinite(vector.x) && + isfinite(vector.y) && + isfinite(vector.z) + ); + +} /* mahony_vector_is_finite */ + + +/*------------------------------------------------------------------------------ + Public Functions + ------------------------------------------------------------------------------*/ + +bool mahony_init + ( + MAHONY_FILTER *filter, + QUAT initial_attitude, + float proportional_gain, + float integral_gain + ) +{ +if ( filter == NULL ) + { + return false; + } + +if ( !mahony_quat_is_finite(initial_attitude) ) + { + return false; + } + +if ( !isfinite(proportional_gain) || + !isfinite(integral_gain) ) + { + return false; + } + +if ( proportional_gain < 0.0f || + integral_gain < 0.0f ) + { + return false; + } + +filter->attitude = quat_normalize(initial_attitude); + +filter->integral_error.x = 0.0f; +filter->integral_error.y = 0.0f; +filter->integral_error.z = 0.0f; + +filter->proportional_gain = proportional_gain; +filter->integral_gain = integral_gain; + +return true; + +} /* mahony_init */ + + +bool mahony_update_gyro + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + float delta_time_s + ) +{ +QUAT angular_velocity; +QUAT attitude_derivative; +QUAT attitude_delta; + +if ( filter == NULL ) + { + return false; + } + +if ( !mahony_quat_is_finite(filter->attitude) ) + { + return false; + } + +if ( !mahony_vector_is_finite(gyro_body_rad_s) ) + { + return false; + } + +if ( !isfinite(delta_time_s) || + delta_time_s <= 0.0f ) + { + return false; + } + +/* + * The attitude quaternion is a body-to-world rotation and angular velocity is + * expressed in the body frame: + * + * q_dot = 0.5 * q * omega_body + */ +angular_velocity.w = 0.0f; +angular_velocity.x = gyro_body_rad_s.x; +angular_velocity.y = gyro_body_rad_s.y; +angular_velocity.z = gyro_body_rad_s.z; + +attitude_derivative = quat_mult + ( + filter->attitude, + angular_velocity + ); + +attitude_derivative = quat_scale + ( + attitude_derivative, + 0.5f + ); + +attitude_delta = quat_scale + ( + attitude_derivative, + delta_time_s + ); + +filter->attitude = quat_add + ( + filter->attitude, + attitude_delta + ); + +filter->attitude = quat_normalize(filter->attitude); + +return true; + +} /* mahony_update_gyro */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file diff --git a/mahony/mahony.h b/mahony/mahony.h new file mode 100644 index 0000000..d56cac5 --- /dev/null +++ b/mahony/mahony.h @@ -0,0 +1,112 @@ +/******************************************************************************* + * + * FILE: + * mahony.h + * + * DESCRIPTION: + * Mahony attitude filter interface. + * + ******************************************************************************/ + +#ifndef MAHONY_H +#define MAHONY_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "math_sdr.h" + +/*------------------------------------------------------------------------------ + Typedefs + ------------------------------------------------------------------------------*/ + +/** + * @brief Three-dimensional floating-point vector. + */ +typedef struct _VECTOR_3F + { + float x; + float y; + float z; + } VECTOR_3F; + +/** + * @brief State and gains for a Mahony attitude filter. + */ +typedef struct _MAHONY_FILTER + { + /** + * Body-to-world attitude quaternion. + */ + QUAT attitude; + + /** + * Accumulated attitude error used for integral gyro-bias correction. + */ + VECTOR_3F integral_error; + + float proportional_gain; + float integral_gain; + + } MAHONY_FILTER; + +/*------------------------------------------------------------------------------ + Function Prototypes + ------------------------------------------------------------------------------*/ + +/** + * @brief Initializes a Mahony attitude filter. + * + * @param filter Filter instance to initialize. + * @param initial_attitude Initial body-to-world attitude quaternion. + * @param proportional_gain Proportional correction gain. + * @param integral_gain Integral correction gain. + * + * @return true when initialization succeeds; otherwise false. + */ +bool mahony_init + ( + MAHONY_FILTER *filter, + QUAT initial_attitude, + float proportional_gain, + float integral_gain + ); + +/** + * @brief Propagates attitude using body-frame gyroscope measurements. + * + * The attitude quaternion represents the body-to-world rotation. The angular + * velocity vector must be expressed in the body frame in radians per second. + * + * @param filter Initialized filter instance. + * @param gyro_body_rad_s Body-frame angular velocity in radians per second. + * @param delta_time_s Elapsed time in seconds. + * + * @return true when the attitude was updated; otherwise false. + */ +bool mahony_update_gyro + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + float delta_time_s + ); + +#ifdef __cplusplus +} +#endif + +#endif /* MAHONY_H */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 37b594a..a884f2a 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -41,8 +41,10 @@ extern "C" { /*------------------------------------------------------------------------------ Includes ------------------------------------------------------------------------------*/ -#include #include +#include +#include +#include /*------------------------------------------------------------------------------ diff --git a/test/mahony/.gitignore b/test/mahony/.gitignore new file mode 100644 index 0000000..9bf4711 --- /dev/null +++ b/test/mahony/.gitignore @@ -0,0 +1,6 @@ +build/ +coverage/ +results.txt +*.gcda +*.gcno +*.gcov diff --git a/test/mahony/Makefile b/test/mahony/Makefile new file mode 100644 index 0000000..42a4902 --- /dev/null +++ b/test/mahony/Makefile @@ -0,0 +1,106 @@ +################################################################ +# +# math_sdr unit tests (based on gcc) +# +################################################################ + +################################################################ +# target +################################################################ +TARGET = mahony + +################################################################ +# build variables +################################################################ +DEBUG ?= 0 +OPT = -Og + +################################################################ +# paths +################################################################ +BUILD_DIR = build +ROOT_DIR = ../.. + +################################################################ +# source +################################################################ +TEST_SOURCES = \ +test_mahony.c + +COV_C_SOURCES = \ +$(ROOT_DIR)/mahony/mahony.c \ +$(ROOT_DIR)/math_sdr/math_sdr.c + +FRAMEWORK_SOURCES = \ +$(ROOT_DIR)/test/framework/src/test_assert.c \ +$(ROOT_DIR)/test/framework/src/test_runner.c + +C_SOURCES = $(TEST_SOURCES) $(COV_C_SOURCES) $(FRAMEWORK_SOURCES) + +################################################################ +# compiler +################################################################ +CC = gcc + +################################################################ +# C flags +################################################################ +C_INCLUDES = \ +-I. \ +-I$(ROOT_DIR)/mahony \ +-I$(ROOT_DIR)/math_sdr \ +-I$(ROOT_DIR)/test/framework/src + +C_DEFS = \ +-DUNIT_TEST + +CFLAGS = $(C_INCLUDES) $(C_DEFS) $(OPT) -Wall -g + +CFLAGS += -Wno-unused-function +CFLAGS += -Wno-unused-variable +CFLAGS += -ftest-coverage +CFLAGS += -fprofile-arcs +ifeq ($(DEBUG), 1) +CFLAGS += -DDEBUG +else +CFLAGS += -DRELBLD +endif + +################################################################ +# build +################################################################ +all: clean $(BUILD_DIR)/$(TARGET) + +OBJECTS = $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o))) +vpath %.c $(sort $(dir $(C_SOURCES))) + +$(BUILD_DIR)/%.o: %.c $(BUILD_DIR) + $(CC) -c $(CFLAGS) $< -o $@ + +$(BUILD_DIR)/$(TARGET): $(OBJECTS) + $(CC) $(OBJECTS) -o $@ -lgcov -lm + +$(BUILD_DIR): + mkdir $@ + +################################################################ +# test +################################################################ +test: + @echo THIS TEST MUST BE EXECUTED IN A BASH TERMINAL. CMD/PS do not work. + -rm -fR $(BUILD_DIR) + $(MAKE) all + $(BUILD_DIR)/$(TARGET) + tail -n 7 "results.txt" + mkdir -p coverage + gcovr $(BUILD_DIR) \ + --filter "$(ROOT_DIR)/mahony/mahony.c" \ + --filter "$(ROOT_DIR)/math_sdr/math_sdr.c" \ + --html-details coverage/coverage.html \ + --json coverage/coverage.json + +################################################################ +# clean +################################################################ +clean: + -rm -fR $(BUILD_DIR) diff --git a/test/mahony/main.h b/test/mahony/main.h new file mode 100644 index 0000000..55e71c4 --- /dev/null +++ b/test/mahony/main.h @@ -0,0 +1,8 @@ +#ifndef TEST_MAHONY_MAIN_H +#define TEST_MAHONY_MAIN_H + +#include +#include +#include + +#endif /* TEST_MAHONY_MAIN_H */ \ No newline at end of file diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c new file mode 100644 index 0000000..10f9bde --- /dev/null +++ b/test/mahony/test_mahony.c @@ -0,0 +1,562 @@ +/******************************************************************************* + * + * FILE: + * test_mahony.c + * + * DESCRIPTION: + * Unit tests for the Mahony attitude filter. + * + ******************************************************************************/ + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include +#include +#include + +#define TEST_PI 3.14159265358979323846f +#define TEST_TOLERANCE 0.001f + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "mahony.h" +#include "sdrtf_pub.h" + +/*------------------------------------------------------------------------------ + Test Helpers + ------------------------------------------------------------------------------*/ + +static void assert_quat_components + ( + const char *description, + QUAT actual, + QUAT expected + ) +{ +TEST_begin_nested_case(description); + +TEST_ASSERT_EQ_FLOAT("Quaternion w component", actual.w, expected.w); +TEST_ASSERT_EQ_FLOAT("Quaternion x component", actual.x, expected.x); +TEST_ASSERT_EQ_FLOAT("Quaternion y component", actual.y, expected.y); +TEST_ASSERT_EQ_FLOAT("Quaternion z component", actual.z, expected.z); + +TEST_end_nested_case(); + +} /* assert_quat_components */ + + +/*------------------------------------------------------------------------------ + Initialization Tests + ------------------------------------------------------------------------------*/ + +void test_mahony_init_identity_attitude + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.0f + ) + ); + +assert_quat_components + ( + "Identity attitude remains identity", + filter.attitude, + identity + ); + +} /* test_mahony_init_identity_attitude */ + + +void test_mahony_init_normalizes_attitude + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT initial_attitude = + { + .w = 2.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +QUAT expected = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +assert_quat_components + ( + "Initial attitude is normalized", + filter.attitude, + expected + ); + +} /* test_mahony_init_normalizes_attitude */ + + +void test_mahony_init_zero_quaternion_uses_identity + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT zero_quaternion = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +QUAT expected = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + zero_quaternion, + 1.0f, + 0.0f + ) + ); + +assert_quat_components + ( + "Zero quaternion falls back to identity", + filter.attitude, + expected + ); + +} /* test_mahony_init_zero_quaternion_uses_identity */ + + +void test_mahony_init_clears_integral_error + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.1f + ) + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Integral error x initializes to zero", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Integral error y initializes to zero", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Integral error z initializes to zero", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_init_clears_integral_error */ + + +void test_mahony_init_rejects_null_filter + ( + void + ) +{ +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_FALSE + ( + "Null filter is rejected", + mahony_init + ( + NULL, + identity, + 1.0f, + 0.0f + ) + ); + +} /* test_mahony_init_rejects_null_filter */ + + +void test_mahony_init_rejects_negative_gain + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_FALSE + ( + "Negative proportional gain is rejected", + mahony_init + ( + &filter, + identity, + -1.0f, + 0.0f + ) + ); + +TEST_ASSERT_FALSE + ( + "Negative integral gain is rejected", + mahony_init + ( + &filter, + identity, + 1.0f, + -0.1f + ) + ); + +} /* test_mahony_init_rejects_negative_gain */ + + +/*------------------------------------------------------------------------------ + Gyroscope Propagation Tests + ------------------------------------------------------------------------------*/ + +void test_mahony_update_gyro_zero_rate + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F zero_rate = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "Zero-rate update succeeds", + mahony_update_gyro + ( + &filter, + zero_rate, + 0.01f + ) + ); + +assert_quat_components + ( + "Zero angular velocity preserves identity", + filter.attitude, + identity + ); + +} /* test_mahony_update_gyro_zero_rate */ + + +void test_mahony_update_gyro_positive_yaw + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.5f * TEST_PI + }; + +QUAT body_x = + { + .w = 0.0f, + .x = 1.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Positive-yaw propagation succeeds", + mahony_update_gyro + ( + &filter, + gyro_body_rad_s, + 0.001f + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Positive 90-degree yaw quaternion w component", + filter.attitude.w, + cosf(0.25f * TEST_PI) + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Positive 90-degree yaw quaternion z component", + filter.attitude.z, + sinf(0.25f * TEST_PI) + ); + +QUAT world_vector = quat_rotate_body_to_world + ( + filter.attitude, + body_x + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Positive yaw rotates body positive X to world positive Y", + world_vector.y, + 1.0f + ); + +} /* test_mahony_update_gyro_positive_yaw */ + + +void test_mahony_update_gyro_rejects_invalid_delta_time + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F zero_rate = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +TEST_ASSERT_FALSE + ( + "Zero delta time is rejected", + mahony_update_gyro + ( + &filter, + zero_rate, + 0.0f + ) + ); + +TEST_ASSERT_FALSE + ( + "Negative delta time is rejected", + mahony_update_gyro + ( + &filter, + zero_rate, + -0.01f + ) + ); + +} /* test_mahony_update_gyro_rejects_invalid_delta_time */ + +/*------------------------------------------------------------------------------ + Main + ------------------------------------------------------------------------------*/ + +int main + ( + void + ) +{ +unit_test tests[] = + { + { + "mahony_init_identity_attitude", + test_mahony_init_identity_attitude + }, + { + "mahony_init_normalizes_attitude", + test_mahony_init_normalizes_attitude + }, + { + "mahony_init_zero_quaternion_uses_identity", + test_mahony_init_zero_quaternion_uses_identity + }, + { + "mahony_init_clears_integral_error", + test_mahony_init_clears_integral_error + }, + { + "mahony_init_rejects_null_filter", + test_mahony_init_rejects_null_filter + }, + { + "mahony_init_rejects_negative_gain", + test_mahony_init_rejects_negative_gain + }, + { + "mahony_update_gyro_zero_rate", + test_mahony_update_gyro_zero_rate + }, + { + "mahony_update_gyro_positive_yaw", + test_mahony_update_gyro_positive_yaw + }, + { + "mahony_update_gyro_rejects_invalid_delta_time", + test_mahony_update_gyro_rejects_invalid_delta_time + } + }; + +TEST_INITIALIZE_TEST("mahony.c", tests); + +} /* main */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file From 41be529b540ebb22a6e81452d9f1ac3673289742 Mon Sep 17 00:00:00 2001 From: Bjorn Date: Fri, 24 Jul 2026 17:09:15 -0700 Subject: [PATCH 02/18] Add proportional accelerometer correction to Mahony filter --- mahony/mahony.c | 201 +++++++++++ mahony/mahony.h | 25 ++ test/mahony/test_mahony.c | 735 +++++++++++++++++++++++++++++++++++++- 3 files changed, 953 insertions(+), 8 deletions(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index cc6c9b3..823536e 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -59,6 +59,104 @@ return } /* mahony_vector_is_finite */ +static float vector_magnitude + ( + VECTOR_3F vector + ) +{ +return sqrtf + ( + vector.x * vector.x + + vector.y * vector.y + + vector.z * vector.z + ); + +} /* vector_magnitude */ + + +static bool vector_normalize + ( + VECTOR_3F *vector + ) +{ +float magnitude; + +if ( vector == NULL ) + { + return false; + } + +if ( !mahony_vector_is_finite(*vector) ) + { + return false; + } + +magnitude = vector_magnitude(*vector); + +if ( magnitude <= 0.0f || !isfinite(magnitude) ) + { + return false; + } + +vector->x /= magnitude; +vector->y /= magnitude; +vector->z /= magnitude; + +return true; + +} /* vector_normalize */ + + +static VECTOR_3F vector_cross + ( + VECTOR_3F a, + VECTOR_3F b + ) +{ +VECTOR_3F result; + +result.x = a.y * b.z - a.z * b.y; +result.y = a.z * b.x - a.x * b.z; +result.z = a.x * b.y - a.y * b.x; + +return result; + +} /* vector_cross */ + + +static VECTOR_3F vector_add + ( + VECTOR_3F a, + VECTOR_3F b + ) +{ +VECTOR_3F result; + +result.x = a.x + b.x; +result.y = a.y + b.y; +result.z = a.z + b.z; + +return result; + +} /* vector_add */ + + +static VECTOR_3F vector_scale + ( + VECTOR_3F vector, + float scalar + ) +{ +VECTOR_3F result; + +result.x = vector.x * scalar; +result.y = vector.y * scalar; +result.z = vector.z * scalar; + +return result; + +} /* vector_scale */ + /*------------------------------------------------------------------------------ Public Functions @@ -107,6 +205,13 @@ return true; } /* mahony_init */ +/* + * Verify the filter, gyro, and timestep are valid. + * Convert the body-frame angular velocity into a pure quaternion. + * Use the quaternion differential equation to calculate how quickly the + * body-to-world attitude is changing. Multiply that derivative by the + * timestep, add it to the current attitude, and normalize the result. + */ bool mahony_update_gyro ( @@ -181,6 +286,102 @@ return true; } /* mahony_update_gyro */ +bool mahony_update_imu + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + VECTOR_3F accel_body, + float delta_time_s, + bool use_accel + ) +{ +QUAT gravity_world; +QUAT gravity_body_quat; + +VECTOR_3F gravity_estimated_body; +VECTOR_3F attitude_error; +VECTOR_3F proportional_correction; +VECTOR_3F gyro_corrected; + +if ( filter == NULL ) + { + return false; + } + +if ( !mahony_vector_is_finite(gyro_body_rad_s) ) + { + return false; + } + +if ( !isfinite(delta_time_s) || + delta_time_s <= 0.0f ) + { + return false; + } + +gyro_corrected = gyro_body_rad_s; + +/* + * Accelerometer feedback is optional. If the measurement is invalid or has + * zero magnitude, continue with gyroscope-only propagation. + */ +if ( use_accel ) + { + if ( vector_normalize(&accel_body) ) + { + /* + * The attitude quaternion represents the body-to-world rotation. + * Rotate the fixed world gravity direction into the body frame to + * predict where gravity should appear according to the estimate. + */ + gravity_world.w = 0.0f; + gravity_world.x = 0.0f; + gravity_world.y = 0.0f; + gravity_world.z = 1.0f; + + gravity_body_quat = quat_rotate_world_to_body + ( + filter->attitude, + gravity_world + ); + + gravity_estimated_body.x = gravity_body_quat.x; + gravity_estimated_body.y = gravity_body_quat.y; + gravity_estimated_body.z = gravity_body_quat.z; + + /* + * measured x estimated produces a correction that drives the + * estimated gravity direction toward the measured direction. + */ + attitude_error = vector_cross + ( + accel_body, + gravity_estimated_body + ); + + proportional_correction = vector_scale + ( + attitude_error, + filter->proportional_gain + ); + + gyro_corrected = vector_add + ( + gyro_corrected, + proportional_correction + ); + } + } + +return mahony_update_gyro + ( + filter, + gyro_corrected, + delta_time_s + ); + +} /* mahony_update_imu */ + /******************************************************************************* * END OF FILE ******************************************************************************/ \ No newline at end of file diff --git a/mahony/mahony.h b/mahony/mahony.h index d56cac5..311e2a1 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -101,6 +101,31 @@ bool mahony_update_gyro float delta_time_s ); +/** + * @brief Updates attitude using gyroscope propagation and accelerometer + * proportional feedback. + * + * The gyroscope must be expressed in the body frame in radians per second. + * The accelerometer must be expressed in the body frame. Its magnitude is + * removed internally because the filter uses only its measured direction. + * + * @param filter Initialized filter instance. + * @param gyro_body_rad_s Body-frame angular velocity in radians per second. + * @param accel_body Body-frame accelerometer measurement. + * @param delta_time_s Elapsed time in seconds. + * @param use_accel Whether accelerometer feedback should be applied. + * + * @return true when the attitude was updated; otherwise false. + */ +bool mahony_update_imu + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + VECTOR_3F accel_body, + float delta_time_s, + bool use_accel + ); + #ifdef __cplusplus } #endif diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 10f9bde..5d0bd61 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -51,6 +51,16 @@ TEST_end_nested_case(); Initialization Tests ------------------------------------------------------------------------------*/ + /** + * @brief Verifies that the filter accepts an identity initial attitude. + * + * This test initializes the filter with a unit identity quaternion and checks + * that initialization succeeds without changing the attitude. + * + * This is important because identity represents the simplest valid attitude + * and is the expected starting state when the body and world frames are + * aligned. + */ void test_mahony_init_identity_attitude ( void @@ -87,7 +97,16 @@ assert_quat_components } /* test_mahony_init_identity_attitude */ - +/** + * @brief Verifies that the initial attitude quaternion is normalized. + * + * This test initializes the filter with a valid but non-unit quaternion and + * checks that the stored attitude is converted to unit length. + * + * This is important because only unit quaternions represent pure rotations. + * Quaternion propagation and vector rotation can produce invalid results if + * the attitude quaternion is not normalized. + */ void test_mahony_init_normalizes_attitude ( void @@ -132,7 +151,16 @@ assert_quat_components } /* test_mahony_init_normalizes_attitude */ - +/** + * @brief Verifies that a zero quaternion falls back to identity. + * + * This test initializes the filter with a quaternion whose components are all + * zero and checks that quaternion normalization produces the identity + * attitude. + * + * This is important because a zero quaternion does not represent a valid + * rotation and cannot be normalized through ordinary division. + */ void test_mahony_init_zero_quaternion_uses_identity ( void @@ -177,7 +205,15 @@ assert_quat_components } /* test_mahony_init_zero_quaternion_uses_identity */ - +/** + * @brief Verifies that the integral correction state starts at zero. + * + * This test initializes the filter and checks that all components of the + * accumulated integral error are cleared. + * + * This is important because stale integral error would introduce a false gyro + * correction as soon as the filter begins operating. + */ void test_mahony_init_clears_integral_error ( void @@ -228,7 +264,15 @@ TEST_ASSERT_EQ_FLOAT } /* test_mahony_init_clears_integral_error */ - +/** + * @brief Verifies that initialization rejects a null filter pointer. + * + * This test calls mahony_init() without a valid filter instance and checks + * that the function reports failure. + * + * This is important because dereferencing a null filter pointer would cause + * undefined behavior or a runtime fault on the flight computer. + */ void test_mahony_init_rejects_null_filter ( void @@ -256,7 +300,16 @@ TEST_ASSERT_FALSE } /* test_mahony_init_rejects_null_filter */ - +/** + * @brief Verifies that negative feedback gains are rejected. + * + * This test attempts to initialize the filter with negative proportional and + * integral gains and checks that initialization fails. + * + * This is important because negative gains would reverse the intended + * feedback direction and could cause attitude errors to grow rather than + * converge. + */ void test_mahony_init_rejects_negative_gain ( void @@ -303,6 +356,15 @@ TEST_ASSERT_FALSE Gyroscope Propagation Tests ------------------------------------------------------------------------------*/ + /** + * @brief Verifies that zero angular velocity preserves the current attitude. + * + * This test begins at the identity attitude, supplies a zero gyroscope vector, + * and checks that gyro propagation does not rotate the attitude. + * + * This is important because a stationary body should not develop artificial + * rotation when the measured angular rate is zero. + */ void test_mahony_update_gyro_zero_rate ( void @@ -357,7 +419,19 @@ assert_quat_components } /* test_mahony_update_gyro_zero_rate */ - +/** + * @brief Verifies positive yaw propagation from body-frame angular velocity. + * + * This test applies a positive 90-degree-per-second body Z-axis angular rate + * for one second and checks that the resulting quaternion represents an + * approximately positive 90-degree yaw. + * + * It also rotates the body positive X axis into the world frame to verify that + * it points toward world positive Y. + * + * This is important because it confirms the quaternion multiplication order, + * rotation sign, gyro units, and body-to-world attitude convention. + */ void test_mahony_update_gyro_positive_yaw ( void @@ -445,7 +519,16 @@ TEST_ASSERT_EQ_FLOAT } /* test_mahony_update_gyro_positive_yaw */ - +/** + * @brief Verifies that gyro propagation rejects invalid timesteps. + * + * This test supplies zero and negative elapsed times and checks that the + * update reports failure. + * + * This is important because attitude propagation requires a positive elapsed + * time. Invalid timing values could reverse propagation or hide timing errors + * in the sensor update path. + */ void test_mahony_update_gyro_rejects_invalid_delta_time ( void @@ -504,6 +587,618 @@ TEST_ASSERT_FALSE } /* test_mahony_update_gyro_rejects_invalid_delta_time */ +/** + * @brief Verifies that aligned gravity produces no attitude correction. + * + * This test begins with an identity attitude, zero angular velocity, and an + * accelerometer measurement aligned with the expected body-frame gravity + * direction. + * + * The measured and estimated gravity vectors should have a zero cross product, + * so the attitude should remain unchanged. + * + * This is important because a correctly aligned filter must not introduce + * artificial rotation when there is no attitude error. + */ +void test_mahony_update_imu_aligned_gravity + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Aligned IMU update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "Aligned gravity preserves identity attitude", + filter.attitude, + identity + ); + +} /* test_mahony_update_imu_aligned_gravity */ + +/** + * @brief Verifies that accelerometer feedback reduces a roll error. + * + * This test initializes the attitude with a positive roll offset while + * supplying zero angular velocity and a stationary gravity measurement. + * Repeated Mahony updates should move the estimated body Z axis closer to the + * world Z axis. + * + * This is important because it verifies that proportional accelerometer + * feedback corrects roll drift and that the cross-product sign is consistent + * with the body-to-world quaternion convention. + */ +void test_mahony_update_imu_roll_error_converges + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +const float initial_roll_rad = deg_to_rad(10.0f); + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + initial_roll_rad + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 2000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Roll-error correction update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +QUAT corrected_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Roll error decreases toward level", + fabsf(corrected_world_z.y) < fabsf(initial_world_z.y) + ); + +TEST_ASSERT_TRUE + ( + "Corrected body Z approaches world Z", + corrected_world_z.z > initial_world_z.z + ); + +} /* test_mahony_update_imu_roll_error_converges */ + +/** + * @brief Verifies that accelerometer feedback reduces a pitch error. + * + * This test initializes the attitude with a positive pitch offset while + * supplying zero angular velocity and a stationary gravity measurement. + * Repeated Mahony updates should move the estimated body Z axis closer to the + * world Z axis. + * + * This is important because it confirms that accelerometer correction works + * on both observable tilt axes rather than only for roll. + */ +void test_mahony_update_imu_pitch_error_converges + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +const float initial_pitch_rad = deg_to_rad(10.0f); + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + initial_pitch_rad, + 0.0f + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 2000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Pitch-error correction update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +QUAT corrected_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Pitch error decreases toward level", + fabsf(corrected_world_z.x) < fabsf(initial_world_z.x) + ); + +TEST_ASSERT_TRUE + ( + "Corrected body Z approaches world Z", + corrected_world_z.z > initial_world_z.z + ); + +} /* test_mahony_update_imu_pitch_error_converges */ + +/** + * @brief Verifies that accelerometer feedback does not correct yaw. + * + * This test initializes the attitude with a yaw offset while roll and pitch + * remain level. Because yaw rotation does not change the gravity direction, + * repeated accelerometer corrections should leave the yaw attitude unchanged. + * + * This is important because gravity provides roll and pitch information but + * contains no heading information. Yaw correction requires another reference, + * such as a calibrated magnetometer. + */ +void test_mahony_update_imu_yaw_error_does_not_converge + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +const float initial_yaw_rad = deg_to_rad(20.0f); + +QUAT initial_attitude = eul_to_quat + ( + initial_yaw_rad, + 0.0f, + 0.0f + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_x = + { + .w = 0.0f, + .x = 1.0f, + .y = 0.0f, + .z = 0.0f + }; + +QUAT initial_world_x = quat_rotate_body_to_world + ( + initial_attitude, + body_x + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 2000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Yaw-only correction update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +QUAT final_world_x = quat_rotate_body_to_world + ( + filter.attitude, + body_x + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer does not correct yaw X component", + final_world_x.x, + initial_world_x.x + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer does not correct yaw Y component", + final_world_x.y, + initial_world_x.y + ); + +} /* test_mahony_update_imu_yaw_error_does_not_converge */ + +/** + * @brief Verifies that a zero accelerometer vector falls back to gyro-only + * propagation. + * + * This test runs two identical filters. One uses mahony_update_gyro(), while + * the other uses mahony_update_imu() with accelerometer feedback requested but + * a zero accelerometer vector. + * + * Both filters should produce the same attitude because the zero vector cannot + * be normalized and must therefore be excluded from feedback. + * + * This is important because an invalid accelerometer sample should not stop + * attitude propagation or introduce undefined normalization behavior. + */ +void test_mahony_update_imu_zero_accel_uses_gyro_only + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = 0.0f, + .z = deg_to_rad(45.0f) + }; + +VECTOR_3F zero_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "Zero-accelerometer IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + zero_accel, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "Zero accelerometer matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_zero_accel_uses_gyro_only */ + +/** + * @brief Verifies that disabling accelerometer feedback matches gyro-only + * propagation. + * + * This test runs two identical filters while providing a deliberately + * misaligned accelerometer measurement. One filter uses gyro-only propagation, + * and the other calls mahony_update_imu() with use_accel set to false. + * + * Both filters should produce the same attitude because the accelerometer + * measurement must be completely ignored. + * + * This is important because flight-state logic must be able to disable + * accelerometer correction during thrust, vibration, saturation, or other + * high-dynamic conditions. + */ +void test_mahony_update_imu_disabled_accel_uses_gyro_only + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = deg_to_rad(30.0f), + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F misaligned_accel = + { + .x = GRAVITY, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "Disabled-accelerometer IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + misaligned_accel, + 0.001f, + false + ) + ); + } + +assert_quat_components + ( + "Disabled accelerometer matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_disabled_accel_uses_gyro_only */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -550,7 +1245,31 @@ unit_test tests[] = { "mahony_update_gyro_rejects_invalid_delta_time", test_mahony_update_gyro_rejects_invalid_delta_time - } + }, + { + "mahony_update_imu_aligned_gravity", + test_mahony_update_imu_aligned_gravity + }, + { + "mahony_update_imu_roll_error_converges", + test_mahony_update_imu_roll_error_converges + }, + { + "mahony_update_imu_pitch_error_converges", + test_mahony_update_imu_pitch_error_converges + }, + { + "mahony_update_imu_yaw_error_does_not_converge", + test_mahony_update_imu_yaw_error_does_not_converge + }, + { + "mahony_update_imu_zero_accel_uses_gyro_only", + test_mahony_update_imu_zero_accel_uses_gyro_only + }, + { + "mahony_update_imu_disabled_accel_uses_gyro_only", + test_mahony_update_imu_disabled_accel_uses_gyro_only + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From c21ff3437d7bac04a401000a37409a2daeb74b6e Mon Sep 17 00:00:00 2001 From: Bjorn Date: Sat, 25 Jul 2026 08:13:43 -0700 Subject: [PATCH 03/18] Add Mahony attitude diagnostic output tests --- test/mahony/test_mahony.c | 416 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 416 insertions(+) diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 5d0bd61..847020c 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -46,6 +46,177 @@ TEST_end_nested_case(); } /* assert_quat_components */ +/** + * @brief Limits a floating-point value to a specified range. + */ +static float clamp_float + ( + float value, + float minimum, + float maximum + ) +{ +if ( value < minimum ) + { + return minimum; + } + +if ( value > maximum ) + { + return maximum; + } + +return value; + +} /* clamp_float */ + + +/** + * @brief Converts a body-to-world quaternion into ZYX Euler angles. + * + * The returned values represent roll about X, pitch about Y, and yaw about Z. + * Angles are returned in degrees for readable diagnostic output. + */ +static VECTOR_3F quat_to_euler_deg + ( + QUAT attitude + ) +{ +VECTOR_3F angles_deg; + +float sin_roll_cos_pitch; +float cos_roll_cos_pitch; +float sin_pitch; +float sin_yaw_cos_pitch; +float cos_yaw_cos_pitch; + +attitude = quat_normalize(attitude); + +sin_roll_cos_pitch = + 2.0f * + ( + attitude.w * attitude.x + + attitude.y * attitude.z + ); + +cos_roll_cos_pitch = + 1.0f - + 2.0f * + ( + attitude.x * attitude.x + + attitude.y * attitude.y + ); + +sin_pitch = + 2.0f * + ( + attitude.w * attitude.y - + attitude.z * attitude.x + ); + +sin_pitch = clamp_float + ( + sin_pitch, + -1.0f, + 1.0f + ); + +sin_yaw_cos_pitch = + 2.0f * + ( + attitude.w * attitude.z + + attitude.x * attitude.y + ); + +cos_yaw_cos_pitch = + 1.0f - + 2.0f * + ( + attitude.y * attitude.y + + attitude.z * attitude.z + ); + +angles_deg.x = rad_to_deg + ( + atan2f + ( + sin_roll_cos_pitch, + cos_roll_cos_pitch + ) + ); + +angles_deg.y = rad_to_deg + ( + asinf(sin_pitch) + ); + +angles_deg.z = rad_to_deg + ( + atan2f + ( + sin_yaw_cos_pitch, + cos_yaw_cos_pitch + ) + ); + +return angles_deg; + +} /* quat_to_euler_deg */ + + +/** + * @brief Prints one attitude sample as Euler angles and quaternion components. + */ +static void print_attitude_sample + ( + int32_t interval, + float time_s, + QUAT attitude + ) +{ +VECTOR_3F angles_deg = quat_to_euler_deg(attitude); + +printf + ( + "%3ld | %6.2f | %9.3f %9.3f %9.3f | " + "%9.5f %9.5f %9.5f %9.5f\n", + (long)interval, + time_s, + angles_deg.x, + angles_deg.y, + angles_deg.z, + attitude.w, + attitude.x, + attitude.y, + attitude.z + ); + +} /* print_attitude_sample */ + + +/** + * @brief Prints the diagnostic attitude-table heading. + */ +static void print_attitude_heading + ( + const char *title + ) +{ +printf("\n%s\n", title); +printf + ( + "Int | Time s | Roll deg Pitch deg Yaw deg | " + " w x y z\n" + ); + +printf + ( + "----+--------+-------------------------------+" + "----------------------------------------\n" + ); + +} /* print_attitude_heading */ + /*------------------------------------------------------------------------------ Initialization Tests @@ -1199,6 +1370,243 @@ assert_quat_components } /* test_mahony_update_imu_disabled_accel_uses_gyro_only */ +/** + * @brief Prints attitude propagation over ten gyro-only update intervals. + * + * This test simulates a rocket rotating simultaneously about all three body + * axes. It prints the estimated roll, pitch, yaw, and body-to-world quaternion + * after each update. + * + * This is useful for visually confirming that angular velocity accumulates + * smoothly, quaternion components change continuously, and normalization keeps + * the quaternion valid throughout propagation. + */ +void test_mahony_print_gyro_propagation + ( + void + ) +{ +int32_t interval; + +const int32_t interval_count = 10; +const float delta_time_s = 0.1f; + +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +/* + * Simulated body-frame rocket rotation: + * + * Roll rate = 10 degrees per second + * Pitch rate = 5 degrees per second + * Yaw rate = 20 degrees per second + */ +VECTOR_3F gyro_body_rad_s = + { + .x = deg_to_rad(10.0f), + .y = deg_to_rad(5.0f), + .z = deg_to_rad(20.0f) + }; + +TEST_ASSERT_TRUE + ( + "Gyro diagnostic filter initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +print_attitude_heading + ( + "GYROSCOPE-ONLY ATTITUDE PROPAGATION" + ); + +print_attitude_sample + ( + 0, + 0.0f, + filter.attitude + ); + +for ( interval = 1; interval <= interval_count; interval++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro diagnostic propagation succeeds", + mahony_update_gyro + ( + &filter, + gyro_body_rad_s, + delta_time_s + ) + ); + + print_attitude_sample + ( + interval, + interval * delta_time_s, + filter.attitude + ); + } + +/* + * A propagated attitude must remain a unit quaternion. + */ +float quaternion_norm = sqrtf + ( + filter.attitude.w * filter.attitude.w + + filter.attitude.x * filter.attitude.x + + filter.attitude.y * filter.attitude.y + + filter.attitude.z * filter.attitude.z + ); + +TEST_ASSERT_TRUE + ( + "Gyro-propagated quaternion remains normalized", + fabsf(quaternion_norm - 1.0f) < 0.001f + ); + +} /* test_mahony_print_gyro_propagation */ + +/** + * @brief Prints proportional accelerometer correction over ten intervals. + * + * This test simulates a low-dynamic or coasting flight period. The estimated + * attitude begins with roll and pitch errors, the gyro reports no rotation, + * and the accelerometer supplies a stable gravity direction. + * + * The printed output should show roll and pitch moving toward zero while yaw + * remains approximately unchanged. This is useful for visualizing how Mahony + * proportional feedback corrects observable tilt error without pretending + * that gravity provides heading information. + */ +void test_mahony_print_accelerometer_correction + ( + void + ) +{ +int32_t interval; + +const int32_t interval_count = 10; +const float delta_time_s = 0.1f; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + deg_to_rad(20.0f), + deg_to_rad(-10.0f), + deg_to_rad(15.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +/* + * This represents a low-dynamic condition where the measured acceleration + * direction is a usable gravity reference. + */ +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Accelerometer diagnostic filter initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.5f, + 0.0f + ) + ); + +print_attitude_heading + ( + "MAHONY PROPORTIONAL ACCELEROMETER CORRECTION" + ); + +print_attitude_sample + ( + 0, + 0.0f, + filter.attitude + ); + +for ( interval = 1; interval <= interval_count; interval++ ) + { + TEST_ASSERT_TRUE + ( + "Accelerometer diagnostic update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + delta_time_s, + true + ) + ); + + print_attitude_sample + ( + interval, + interval * delta_time_s, + filter.attitude + ); + } + +QUAT final_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +/* + * A level attitude has the body Z axis aligned with world Z. Therefore, its + * world Z component should increase as the tilt error is corrected. + */ +TEST_ASSERT_TRUE + ( + "Accelerometer correction improves body Z alignment", + final_world_z.z > initial_world_z.z + ); + +} /* test_mahony_print_accelerometer_correction */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -1270,6 +1678,14 @@ unit_test tests[] = "mahony_update_imu_disabled_accel_uses_gyro_only", test_mahony_update_imu_disabled_accel_uses_gyro_only }, + { + "mahony_print_gyro_propagation", + test_mahony_print_gyro_propagation + }, + { + "mahony_print_accelerometer_correction", + test_mahony_print_accelerometer_correction + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From d47061aaefb806d119b3739f2b6c765b508c652e Mon Sep 17 00:00:00 2001 From: Bjorn Date: Sat, 25 Jul 2026 15:42:51 -0700 Subject: [PATCH 04/18] Add accelerometer validity gating to Mahony filter --- mahony/mahony.c | 31 ++- mahony/mahony.h | 5 + test/mahony/test_mahony.c | 412 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 447 insertions(+), 1 deletion(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index 823536e..9adf520 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -19,6 +19,18 @@ ------------------------------------------------------------------------------*/ #include "mahony.h" +/*------------------------------------------------------------------------------ + Private Macros + ------------------------------------------------------------------------------*/ + +/* + * Accelerometer feedback is only used when the measured magnitude is reasonably + * close to one g. These are initial software validation thresholds and should + * be tuned later using stationary, vibration, and flight data. + */ +#define MAHONY_ACCEL_MIN_MAGNITUDE (0.85f * GRAVITY) +#define MAHONY_ACCEL_MAX_MAGNITUDE (1.15f * GRAVITY) + /*------------------------------------------------------------------------------ Private Functions ------------------------------------------------------------------------------*/ @@ -303,6 +315,11 @@ VECTOR_3F attitude_error; VECTOR_3F proportional_correction; VECTOR_3F gyro_corrected; +float accel_magnitude; + +bool accel_valid; +bool apply_accel; + if ( filter == NULL ) { return false; @@ -319,13 +336,25 @@ if ( !isfinite(delta_time_s) || return false; } +accel_magnitude = vector_magnitude(accel_body); + +accel_valid = + mahony_vector_is_finite(accel_body) && + isfinite(accel_magnitude) && + accel_magnitude >= MAHONY_ACCEL_MIN_MAGNITUDE && + accel_magnitude <= MAHONY_ACCEL_MAX_MAGNITUDE; + +apply_accel = + use_accel && + accel_valid; + gyro_corrected = gyro_body_rad_s; /* * Accelerometer feedback is optional. If the measurement is invalid or has * zero magnitude, continue with gyroscope-only propagation. */ -if ( use_accel ) +if ( apply_accel ) { if ( vector_normalize(&accel_body) ) { diff --git a/mahony/mahony.h b/mahony/mahony.h index 311e2a1..38e1c48 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -109,6 +109,10 @@ bool mahony_update_gyro * The accelerometer must be expressed in the body frame. Its magnitude is * removed internally because the filter uses only its measured direction. * + * Accelerometer feedback is applied only when the caller enables it and the + * measured acceleration magnitude falls within the configured validity range. + * Invalid accelerometer samples are ignored while gyro propagation continues. + * * @param filter Initialized filter instance. * @param gyro_body_rad_s Body-frame angular velocity in radians per second. * @param accel_body Body-frame accelerometer measurement. @@ -117,6 +121,7 @@ bool mahony_update_gyro * * @return true when the attitude was updated; otherwise false. */ + bool mahony_update_imu ( MAHONY_FILTER *filter, diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 847020c..faabae6 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -1607,6 +1607,402 @@ TEST_ASSERT_TRUE } /* test_mahony_print_accelerometer_correction */ +/** + * @brief Verifies that an accelerometer magnitude below the valid range is + * rejected. + * + * This test compares ordinary gyro propagation against an IMU update using an + * accelerometer vector below the configured minimum magnitude. + * + * This is important because a weak or invalid accelerometer measurement should + * not influence attitude, while gyro propagation must continue normally. + */ +void test_mahony_update_imu_rejects_low_accel_magnitude + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = deg_to_rad(20.0f), + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F low_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.50f * GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "Low-acceleration IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + low_accel, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "Low acceleration matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_rejects_low_accel_magnitude */ + +/** + * @brief Verifies that a non-finite accelerometer sample is rejected. + * + * This test supplies a NaN accelerometer component and compares the result + * against gyro-only propagation. + * + * This is important because corrupted sensor data must not propagate NaN + * values into the attitude quaternion. + */ +void test_mahony_update_imu_rejects_nonfinite_accel + ( + void + ) +{ +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = 0.0f, + .z = deg_to_rad(10.0f) + }; + +VECTOR_3F invalid_accel = + { + .x = NAN, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.01f + ) + ); + +TEST_ASSERT_TRUE + ( + "Invalid-accelerometer IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + invalid_accel, + 0.01f, + true + ) + ); + +assert_quat_components + ( + "Non-finite acceleration matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_rejects_nonfinite_accel */ + +/** + * @brief Verifies that a valid one-g accelerometer sample still corrects tilt. + * + * This test starts with a roll error and supplies a valid one-g acceleration + * vector. The corrected body Z axis should move closer to world Z. + * + * This is important because validity gating must reject poor samples without + * blocking legitimate gravity correction. + */ +void test_mahony_update_imu_accepts_valid_accel_magnitude + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Valid-acceleration IMU update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + true + ) + ); + } + +QUAT corrected_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Valid acceleration improves body Z alignment", + corrected_world_z.z > initial_world_z.z + ); + +} /* test_mahony_update_imu_accepts_valid_accel_magnitude */ + +/** + * @brief Verifies that an accelerometer magnitude above the valid range is + * rejected. + * + * This test simulates a high-acceleration condition such as powered ascent. + * The IMU update should ignore the accelerometer and match gyro-only + * propagation. + * + * This is important because thrust acceleration must not be mistaken for the + * gravity direction. + */ +void test_mahony_update_imu_rejects_high_accel_magnitude + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = deg_to_rad(15.0f), + .z = 0.0f + }; + +VECTOR_3F high_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 2.0f * GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "High-acceleration IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + high_accel, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "High acceleration matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_rejects_high_accel_magnitude */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -1686,6 +2082,22 @@ unit_test tests[] = "mahony_print_accelerometer_correction", test_mahony_print_accelerometer_correction }, + { + "mahony_update_imu_rejects_low_accel_magnitude", + test_mahony_update_imu_rejects_low_accel_magnitude + }, + { + "mahony_update_imu_rejects_high_accel_magnitude", + test_mahony_update_imu_rejects_high_accel_magnitude + }, + { + "mahony_update_imu_rejects_nonfinite_accel", + test_mahony_update_imu_rejects_nonfinite_accel + }, + { + "mahony_update_imu_accepts_valid_accel_magnitude", + test_mahony_update_imu_accepts_valid_accel_magnitude + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From a7709557cbe37feb2abdc540e321e6f158e3e288 Mon Sep 17 00:00:00 2001 From: Bjorn Date: Sat, 25 Jul 2026 16:01:19 -0700 Subject: [PATCH 05/18] Add integral correction and anti-windup to Mahony filter --- mahony/mahony.c | 96 ++++++- test/mahony/test_mahony.c | 558 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 644 insertions(+), 10 deletions(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index 9adf520..7b4037b 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -31,6 +31,13 @@ #define MAHONY_ACCEL_MIN_MAGNITUDE (0.85f * GRAVITY) #define MAHONY_ACCEL_MAX_MAGNITUDE (1.15f * GRAVITY) +/* + * Limits each integral correction component to prevent windup. The value is + * expressed as an angular-rate correction in radians per second and should be + * tuned using sensor characterization and flight data. + */ +#define MAHONY_INTEGRAL_LIMIT_RAD_S 0.25f + /*------------------------------------------------------------------------------ Private Functions ------------------------------------------------------------------------------*/ @@ -53,7 +60,6 @@ return } /* mahony_quat_is_finite */ - /** * @brief Determines whether every vector component is finite. */ @@ -85,7 +91,6 @@ return sqrtf } /* vector_magnitude */ - static bool vector_normalize ( VECTOR_3F *vector @@ -118,6 +123,26 @@ return true; } /* vector_normalize */ +static float clamp_float + ( + float value, + float minimum, + float maximum + ) +{ +if ( value < minimum ) + { + return minimum; + } + +if ( value > maximum ) + { + return maximum; + } + +return value; + +} /* clamp_float */ static VECTOR_3F vector_cross ( @@ -383,23 +408,74 @@ if ( apply_accel ) * estimated gravity direction toward the measured direction. */ attitude_error = vector_cross + ( + accel_body, + gravity_estimated_body + ); + + /* + * Accumulate persistent attitude error as a gyro-rate correction. Integral + * feedback is updated only while accelerometer feedback is both permitted and + * valid, preventing high-dynamic or corrupted measurements from winding up + * the correction state. + */ + if ( filter->integral_gain > 0.0f ) + { + filter->integral_error.x += + filter->integral_gain * + attitude_error.x * + delta_time_s; + + filter->integral_error.y += + filter->integral_gain * + attitude_error.y * + delta_time_s; + + filter->integral_error.z += + filter->integral_gain * + attitude_error.z * + delta_time_s; + + filter->integral_error.x = clamp_float ( - accel_body, - gravity_estimated_body + filter->integral_error.x, + -MAHONY_INTEGRAL_LIMIT_RAD_S, + MAHONY_INTEGRAL_LIMIT_RAD_S ); - proportional_correction = vector_scale + filter->integral_error.y = clamp_float ( - attitude_error, - filter->proportional_gain + filter->integral_error.y, + -MAHONY_INTEGRAL_LIMIT_RAD_S, + MAHONY_INTEGRAL_LIMIT_RAD_S ); - gyro_corrected = vector_add + filter->integral_error.z = clamp_float ( - gyro_corrected, - proportional_correction + filter->integral_error.z, + -MAHONY_INTEGRAL_LIMIT_RAD_S, + MAHONY_INTEGRAL_LIMIT_RAD_S ); } + + proportional_correction = vector_scale + ( + attitude_error, + filter->proportional_gain + ); + + gyro_corrected = vector_add + ( + gyro_corrected, + proportional_correction + ); + + gyro_corrected = vector_add + ( + gyro_corrected, + filter->integral_error + ); + } } return mahony_update_gyro diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index faabae6..684d914 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -2003,6 +2003,540 @@ assert_quat_components } /* test_mahony_update_imu_rejects_high_accel_magnitude */ +/** + * @brief Verifies that zero integral gain prevents integral accumulation. + * + * A valid tilt error is supplied repeatedly, but Ki is zero. The stored + * integral correction must remain zero. + */ +void test_mahony_integral_zero_gain_does_not_accumulate + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Zero-Ki update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + true + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Zero Ki preserves integral X", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Zero Ki preserves integral Y", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Zero Ki preserves integral Z", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_integral_zero_gain_does_not_accumulate */ + +/** + * @brief Verifies that valid accelerometer feedback accumulates integral error. + * + * The filter begins with a roll error, zero gyro rate, and valid gravity. + * A nonzero Ki should accumulate a correction about the roll axis. + */ +void test_mahony_integral_valid_error_accumulates + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 0.0f, + 0.5f + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.1f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Roll error accumulates integral correction", + fabsf(filter.integral_error.x) > 0.0f + ); + +TEST_ASSERT_TRUE + ( + "Pure roll error produces negligible integral Y", + fabsf(filter.integral_error.y) < TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Pure roll error produces negligible integral Z", + fabsf(filter.integral_error.z) < TEST_TOLERANCE + ); + +} /* test_mahony_integral_valid_error_accumulates */ + +/** + * @brief Verifies that disabling accelerometer feedback prevents windup. + * + * Even with valid gravity and a tilt error, use_accel=false must prevent the + * integral state from accumulating. + */ +void test_mahony_integral_disabled_accel_does_not_accumulate + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.5f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Disabled-accelerometer update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + false + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Disabled accelerometer preserves integral X", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Disabled accelerometer preserves integral Y", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Disabled accelerometer preserves integral Z", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_integral_disabled_accel_does_not_accumulate */ + +/** + * @brief Verifies that invalid acceleration does not accumulate integral error. + * + * A two-g acceleration exceeds the configured validity range and must therefore + * be excluded from integral feedback. + */ +void test_mahony_integral_invalid_accel_does_not_accumulate + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F invalid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 2.0f * GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.5f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Invalid-accelerometer update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + invalid_accel, + 0.001f, + true + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Invalid acceleration preserves integral X", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Invalid acceleration preserves integral Y", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Invalid acceleration preserves integral Z", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_integral_invalid_accel_does_not_accumulate */ + +/** + * @brief Verifies that integral correction is limited by anti-windup. + * + * A large integral gain and large roll error would otherwise produce an + * excessive stored angular-rate correction. + */ +void test_mahony_integral_is_limited + ( + void + ) +{ +const float expected_limit_rad_s = 0.25f; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(90.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 0.0f, + 100.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "High-integral-gain update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 1.0f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral X does not exceed anti-windup limit", + fabsf(filter.integral_error.x) <= + expected_limit_rad_s + TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Integral Y does not exceed anti-windup limit", + fabsf(filter.integral_error.y) <= + expected_limit_rad_s + TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Integral Z does not exceed anti-windup limit", + fabsf(filter.integral_error.z) <= + expected_limit_rad_s + TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Large roll error reaches integral limit", + fabsf + ( + fabsf(filter.integral_error.x) - + expected_limit_rad_s + ) < + TEST_TOLERANCE + ); + +} /* test_mahony_integral_is_limited */ + +/** + * @brief Verifies that accumulated integral error affects attitude propagation. + * + * Two filters begin with the same roll error. One has Ki=0 and one has Ki>0. + * With Kp=0 and zero measured gyro, only the filter with integral feedback + * should change its attitude. + */ +void test_mahony_integral_correction_affects_attitude + ( + void + ) +{ +MAHONY_FILTER no_integral_filter; +MAHONY_FILTER integral_filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "No-integral filter initialization succeeds", + mahony_init + ( + &no_integral_filter, + initial_attitude, + 0.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral filter initialization succeeds", + mahony_init + ( + &integral_filter, + initial_attitude, + 0.0f, + 1.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "No-integral update succeeds", + mahony_update_imu + ( + &no_integral_filter, + zero_gyro, + valid_accel, + 0.1f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral update succeeds", + mahony_update_imu + ( + &integral_filter, + zero_gyro, + valid_accel, + 0.1f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral correction changes propagated attitude", + fabsf + ( + integral_filter.attitude.x - + no_integral_filter.attitude.x + ) > + 0.000001f + ); + +} /* test_mahony_integral_correction_affects_attitude */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -2098,6 +2632,30 @@ unit_test tests[] = "mahony_update_imu_accepts_valid_accel_magnitude", test_mahony_update_imu_accepts_valid_accel_magnitude }, + { + "mahony_integral_zero_gain_does_not_accumulate", + test_mahony_integral_zero_gain_does_not_accumulate + }, + { + "mahony_integral_valid_error_accumulates", + test_mahony_integral_valid_error_accumulates + }, + { + "mahony_integral_disabled_accel_does_not_accumulate", + test_mahony_integral_disabled_accel_does_not_accumulate + }, + { + "mahony_integral_invalid_accel_does_not_accumulate", + test_mahony_integral_invalid_accel_does_not_accumulate + }, + { + "mahony_integral_is_limited", + test_mahony_integral_is_limited + }, + { + "mahony_integral_correction_affects_attitude", + test_mahony_integral_correction_affects_attitude + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From eaa16eddd6e346b5fc81c180b7cafea24a255b4d Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 26 Jul 2026 15:07:52 -0700 Subject: [PATCH 06/18] Integrate Mahony filter into sensor state estimation --- sensor/sensor.c | 187 ++++++++++++++++++++++++++++++++---------------- 1 file changed, 125 insertions(+), 62 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 79ba662..10c08c1 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -45,6 +45,20 @@ #include "usb.h" #include "sensor.h" #include "math_sdr.h" +#include "mahony.h" + +/*------------------------------------------------------------------------------ + Private Macros +------------------------------------------------------------------------------*/ + +/* + * Initial Mahony gains for firmware integration. + * + * Proportional correction is enabled conservatively. Integral correction + * remains disabled until the gains are tuned using stationary and flight data. + */ +#define SENSOR_MAHONY_KP 1.0f +#define SENSOR_MAHONY_KI 0.0f /*------------------------------------------------------------------------------ @@ -61,15 +75,20 @@ float velo_x_prev = 0.0f; float velo_y_prev = 0.0f; float velo_z_prev = 0.0f; -/* State estimation */ -QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; - /*------------------------------------------------------------------------------ Static Variables ------------------------------------------------------------------------------*/ static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ +/* + * Persistent attitude-filter state. This instance retains the quaternion and + * integral correction between consecutive IMU updates. + */ +static MAHONY_FILTER mahony_filter; + +/* Timestamp of the previous Mahony update in microseconds. */ +static uint64_t mahony_tick = 0; /*------------------------------------------------------------------------------ Internal function prototypes @@ -323,19 +342,42 @@ else * * *******************************************************************************/ void sensor_init - ( - PRESET_DATA* preset_data - ) + ( + PRESET_DATA* preset_data + ) { -imu_velo_tick = get_us_tick(); - -sensor_reset_velo(); +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; float ax = preset_data->imu_offset.accel_x; float ay = preset_data->imu_offset.accel_y; float az = preset_data->imu_offset.accel_z; -attitude = quat_grav_attitude(ax, ay, az, attitude); +QUAT initial_attitude = quat_grav_attitude + ( + ax, + ay, + az, + identity + ); + +imu_velo_tick = get_us_tick(); +mahony_tick = imu_velo_tick; + +sensor_reset_velo(); + +(void)mahony_init + ( + &mahony_filter, + initial_attitude, + SENSOR_MAHONY_KP, + SENSOR_MAHONY_KI + ); } /* sensor_init */ @@ -412,63 +454,84 @@ mount_orientation = orientation; * Perform sensor fusion on imu converted data to get body rate * * * *******************************************************************************/ -static uint32_t last_tick = 0; + void sensor_body_state - ( - const IMU_CONVERTED* imu_converted, - STATE_ESTIMATION* state_estimate - ) + ( + const IMU_CONVERTED* imu_converted, + STATE_ESTIMATION* state_estimate + ) { -/* Determine delta T */ -uint32_t now_tick = HAL_GetTick(); -float dt = (now_tick - last_tick) / 1000.0f; -if ( dt <= 0.0f || dt > 1.0f ) - { - dt = 0.01f; - } -last_tick = now_tick; - -/* Copy IMU data for readability */ -// float ax = imu_converted->accel_x; -// float ay = imu_converted->accel_y; -// float az = imu_converted->accel_z; - -/* Raw gyro data in deg/s */ -float gx = imu_converted->gyro_x; -float gy = imu_converted->gyro_y; -float gz = imu_converted->gyro_z; - -/* Convert gyro to pure quaternion */ -QUAT q_gyro; /* Must be in radians */ -q_gyro.w = 0.0f; -q_gyro.x = deg_to_rad(gx); -q_gyro.y = deg_to_rad(gy); -q_gyro.z = deg_to_rad(gz); - -/* q_rate = 0.5 * attitude * q_gyro */ -QUAT q_rate = quat_mult(attitude, q_gyro); -q_rate = quat_scale(q_rate, 0.5f); - -/* Dead reckoing orientation by integrating gyro */ -/* attitude += dt * q_rate */ -QUAT rate_dt = quat_scale(q_rate, dt); -attitude = quat_add(attitude, rate_dt); - -/* Sensor fuson with gravity if not in flight */ -if ( get_fc_state() <= FC_STATE_LAUNCH_DETECT ) - { - // QUAT q_acc = quat_grav_attitude(ax, ay, az, attitude); - // gravity_comp_filter(&attitude, q_acc); - } +uint64_t current_tick; +uint64_t imu_tdelta; -/* Scale back to unit quaternion to avoid drift */ -attitude = quat_normalize(attitude); +float delta_time_s; -/* Store results */ -state_estimate->attitude = attitude; -state_estimate->roll_rate = gx; /* Rate in deg/s */ +bool use_accel; -} +VECTOR_3F gyro_body_rad_s; +VECTOR_3F accel_body_m_s2; + +/* + * Calculate elapsed time between attitude updates using the microsecond timer. + */ +current_tick = get_us_tick(); +imu_tdelta = current_tick - mahony_tick; + +delta_time_s = + (float)imu_tdelta / + (float)MICROSEC_PER_SEC; + +if ( mahony_tick == 0 || + delta_time_s <= 0.0f || + delta_time_s > 1.0f ) + { + delta_time_s = 0.01f; + } + +mahony_tick = current_tick; + +/* + * Converted gyro data is in degrees per second. Mahony requires radians per + * second. + */ +gyro_body_rad_s.x = deg_to_rad(imu_converted->gyro_x); +gyro_body_rad_s.y = deg_to_rad(imu_converted->gyro_y); +gyro_body_rad_s.z = deg_to_rad(imu_converted->gyro_z); + +/* + * Converted accelerometer data is already in meters per second squared. + */ +accel_body_m_s2.x = imu_converted->accel_x; +accel_body_m_s2.y = imu_converted->accel_y; +accel_body_m_s2.z = imu_converted->accel_z; + +/* + * Permit accelerometer correction only before powered flight. The Mahony + * filter still performs its own magnitude and finite-value checks. + */ +use_accel = + get_fc_state() <= FC_STATE_LAUNCH_DETECT; + +(void)mahony_update_imu + ( + &mahony_filter, + gyro_body_rad_s, + accel_body_m_s2, + delta_time_s, + use_accel + ); + +/* + * Store the filter's body-to-world quaternion as the system attitude estimate. + */ +state_estimate->attitude = mahony_filter.attitude; + +/* + * Preserve the existing public roll-rate units of degrees per second. + */ +state_estimate->roll_rate = imu_converted->gyro_x; + +} /* sensor_body_state */ /******************************************************************************* From 3e82cb8beac73df8d7ab6b3488bab7a12587afce Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 26 Jul 2026 19:01:47 -0700 Subject: [PATCH 07/18] Document Mahony gain tuning rationale --- sensor/sensor.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 10c08c1..ab25898 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -56,6 +56,15 @@ * * Proportional correction is enabled conservatively. Integral correction * remains disabled until the gains are tuned using stationary and flight data. + * + * + * Proportional gain (KP) corrects gyro drift using the accelerometer. It is + * deliberately low to reduce overcorrection during flight. + * + * Integral gain (KI) learns persistent gyro bias over time. It remains + * disabled because an untuned integral term can accumulate incorrect + * corrections during vibration, launch acceleration, or invalid accelerometer + * data. */ #define SENSOR_MAHONY_KP 1.0f #define SENSOR_MAHONY_KI 0.0f @@ -82,7 +91,7 @@ float velo_z_prev = 0.0f; static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ /* - * Persistent attitude-filter state. This instance retains the quaternion and + * Persistent attitude filter state. This instance retains the quaternion and * integral correction between consecutive IMU updates. */ static MAHONY_FILTER mahony_filter; From 4fe00973e3bb34d33771155f56557deea65f9031 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Mon, 3 Aug 2026 16:27:38 -0700 Subject: [PATCH 08/18] Convert Mahony attitude to world-to-body convention --- mahony/mahony.c | 22 +++--- mahony/mahony.h | 11 +-- math_sdr/math_sdr.c | 20 ++--- math_sdr/math_sdr.h | 4 +- sensor/sensor.c | 6 +- sensor/sensor.h | 2 +- test/mahony/test_mahony.c | 145 ++++++++++++++++++++++------------ test/math_sdr/test_math_sdr.c | 4 +- 8 files changed, 128 insertions(+), 86 deletions(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index 7b4037b..d064cfe 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -246,7 +246,7 @@ return true; * Verify the filter, gyro, and timestep are valid. * Convert the body-frame angular velocity into a pure quaternion. * Use the quaternion differential equation to calculate how quickly the - * body-to-world attitude is changing. Multiply that derivative by the + * world-to-body attitude is changing. Multiply that derivative by the * timestep, add it to the current attitude, and normalize the result. */ @@ -283,10 +283,10 @@ if ( !isfinite(delta_time_s) || } /* - * The attitude quaternion is a body-to-world rotation and angular velocity is - * expressed in the body frame: + * The attitude quaternion represents the world-to-body rotation, while angular + * velocity is expressed in the body frame: * - * q_dot = 0.5 * q * omega_body + * q_dot = -0.5 * omega_body * q */ angular_velocity.w = 0.0f; angular_velocity.x = gyro_body_rad_s.x; @@ -295,14 +295,14 @@ angular_velocity.z = gyro_body_rad_s.z; attitude_derivative = quat_mult ( - filter->attitude, - angular_velocity + angular_velocity, + filter->attitude ); attitude_derivative = quat_scale ( attitude_derivative, - 0.5f + -0.5f ); attitude_delta = quat_scale @@ -384,10 +384,10 @@ if ( apply_accel ) if ( vector_normalize(&accel_body) ) { /* - * The attitude quaternion represents the body-to-world rotation. - * Rotate the fixed world gravity direction into the body frame to - * predict where gravity should appear according to the estimate. - */ + * The attitude quaternion represents the world-to-body rotation. Rotate the + * fixed world gravity direction directly into the body frame to predict where + * gravity should appear according to the current attitude estimate. + */ gravity_world.w = 0.0f; gravity_world.x = 0.0f; gravity_world.y = 0.0f; diff --git a/mahony/mahony.h b/mahony/mahony.h index 38e1c48..1970ee1 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -46,8 +46,8 @@ typedef struct _VECTOR_3F typedef struct _MAHONY_FILTER { /** - * Body-to-world attitude quaternion. - */ + * World-to-body attitude quaternion. + */ QUAT attitude; /** @@ -68,7 +68,7 @@ typedef struct _MAHONY_FILTER * @brief Initializes a Mahony attitude filter. * * @param filter Filter instance to initialize. - * @param initial_attitude Initial body-to-world attitude quaternion. + * @param initial_attitude Initial world-to-body attitude quaternion. * @param proportional_gain Proportional correction gain. * @param integral_gain Integral correction gain. * @@ -83,9 +83,10 @@ bool mahony_init ); /** - * @brief Propagates attitude using body-frame gyroscope measurements. + * @brief Propagates a world-to-body attitude using body-frame gyroscope + * measurements. * - * The attitude quaternion represents the body-to-world rotation. The angular + * The attitude quaternion represents the world-to-body rotation. The angular * velocity vector must be expressed in the body frame in radians per second. * * @param filter Initialized filter instance. diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index cbdfe2a..cb742db 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -265,11 +265,11 @@ return result; /** * @brief Rotates a vector from the world frame into the body frame. * - * The attitude quaternion represents the body-to-world rotation: + * The attitude quaternion represents the world-to-body rotation: * - * vector_body = conjugate(q) * vector_world * q + * vector_body = q * vector_world * conjugate(q) * - * @param attitude Body-to-world attitude quaternion. + * @param attitude World-to-body attitude quaternion. * @param vector_world Pure quaternion containing the world-frame vector. * * @return Pure quaternion containing the body-frame vector. @@ -284,8 +284,8 @@ QUAT attitude_conj = quat_conj(attitude); return quat_mult ( - quat_mult(attitude_conj, vector_world), - attitude + quat_mult(attitude, vector_world), + attitude_conj ); } /* quat_rotate_world_to_body */ @@ -294,11 +294,11 @@ return quat_mult /** * @brief Rotates a vector from the body frame into the world frame. * - * The attitude quaternion represents the body-to-world rotation: + * The attitude quaternion represents the world-to-body rotation: * - * vector_world = q * vector_body * conjugate(q) + * vector_world = conjugate(q) * vector_body * q * - * @param attitude Body-to-world attitude quaternion. + * @param attitude World-to-body attitude quaternion. * @param vector_body Pure quaternion containing the body-frame vector. * * @return Pure quaternion containing the world-frame vector. @@ -313,8 +313,8 @@ QUAT attitude_conj = quat_conj(attitude); return quat_mult ( - quat_mult(attitude, vector_body), - attitude_conj + quat_mult(attitude_conj, vector_body), + attitude ); } /* quat_rotate_body_to_world */ diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index a884f2a..0249ba1 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -163,7 +163,7 @@ QUAT quat_conj /** * @brief Rotates a vector from the world frame into the body frame. * - * @param attitude Body-to-world attitude quaternion. + * @param attitude World-to-body attitude quaternion. * @param vector_world Pure quaternion containing the world-frame vector. * * @return Pure quaternion containing the body-frame vector. @@ -177,7 +177,7 @@ QUAT quat_rotate_world_to_body /** * @brief Rotates a vector from the body frame into the world frame. * - * @param attitude Body-to-world attitude quaternion. + * @param attitude World-to-body attitude quaternion. * @param vector_body Pure quaternion containing the body-frame vector. * * @return Pure quaternion containing the world-frame vector. diff --git a/sensor/sensor.c b/sensor/sensor.c index ab25898..2d39383 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -531,7 +531,7 @@ use_accel = ); /* - * Store the filter's body-to-world quaternion as the system attitude estimate. + * Store the filter's world-to-body quaternion as the system attitude estimate. */ state_estimate->attitude = mahony_filter.attitude; @@ -634,8 +634,8 @@ const QUAT gravity_world = }; /* - * The attitude quaternion is body-to-world, so rotate world gravity - * into the body frame before subtracting it from the accelerometer. + * The attitude quaternion is world-to-body, so rotate world gravity directly + * into the body frame before subtracting it from measured acceleration. */ QUAT gravity_body = quat_rotate_world_to_body ( diff --git a/sensor/sensor.h b/sensor/sensor.h index 9b42fa7..4058285 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -97,7 +97,7 @@ typedef enum /* State estimation from processed sensors */ typedef struct _STATE_ESTIMATION { /* - * Body-to-world attitude quaternion. + * World-to-body attitude quaternion. * * Body-frame vector to world frame: * v_world = q * v_body * conjugate(q) diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 684d914..5288eed 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -72,10 +72,10 @@ return value; /** - * @brief Converts a body-to-world quaternion into ZYX Euler angles. + * @brief Converts a world-to-body quaternion into ZYX Euler angles. * - * The returned values represent roll about X, pitch about Y, and yaw about Z. - * Angles are returned in degrees for readable diagnostic output. + * The world-to-body attitude is conjugated into its equivalent body-to-world + * quaternion before extracting roll, pitch, and yaw for diagnostic display. */ static VECTOR_3F quat_to_euler_deg ( @@ -84,6 +84,14 @@ static VECTOR_3F quat_to_euler_deg { VECTOR_3F angles_deg; +/* + * The Euler extraction formulas below operate on a body-to-world quaternion. + */ +attitude = quat_conj + ( + quat_normalize(attitude) + ); + float sin_roll_cos_pitch; float cos_roll_cos_pitch; float sin_pitch; @@ -601,7 +609,7 @@ assert_quat_components * it points toward world positive Y. * * This is important because it confirms the quaternion multiplication order, - * rotation sign, gyro units, and body-to-world attitude convention. + * rotation sign, gyro units, and world-to-body attitude convention. */ void test_mahony_update_gyro_positive_yaw ( @@ -670,9 +678,9 @@ TEST_ASSERT_EQ_FLOAT TEST_ASSERT_EQ_FLOAT ( - "Positive 90-degree yaw quaternion z component", + "Positive 90-degree yaw world-to-body quaternion z component", filter.attitude.z, - sinf(0.25f * TEST_PI) + -sinf(0.25f * TEST_PI) ); QUAT world_vector = quat_rotate_body_to_world @@ -849,7 +857,7 @@ assert_quat_components * * This is important because it verifies that proportional accelerometer * feedback corrects roll drift and that the cross-product sign is consistent - * with the body-to-world quaternion convention. + * with the world-to-body quaternion convention. */ void test_mahony_update_imu_roll_error_converges ( @@ -862,11 +870,14 @@ MAHONY_FILTER filter; const float initial_roll_rad = deg_to_rad(10.0f); -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - initial_roll_rad + eul_to_quat + ( + 0.0f, + 0.0f, + initial_roll_rad + ) ); VECTOR_3F zero_gyro = @@ -967,11 +978,14 @@ MAHONY_FILTER filter; const float initial_pitch_rad = deg_to_rad(10.0f); -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - initial_pitch_rad, - 0.0f + eul_to_quat + ( + 0.0f, + initial_pitch_rad, + 0.0f + ) ); VECTOR_3F zero_gyro = @@ -1072,11 +1086,14 @@ MAHONY_FILTER filter; const float initial_yaw_rad = deg_to_rad(20.0f); -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - initial_yaw_rad, - 0.0f, - 0.0f + eul_to_quat + ( + initial_yaw_rad, + 0.0f, + 0.0f + ) ); VECTOR_3F zero_gyro = @@ -1374,7 +1391,7 @@ assert_quat_components * @brief Prints attitude propagation over ten gyro-only update intervals. * * This test simulates a rocket rotating simultaneously about all three body - * axes. It prints the estimated roll, pitch, yaw, and body-to-world quaternion + * axes. It prints the estimated roll, pitch, yaw, and world-to-body quaternion * after each update. * * This is useful for visually confirming that angular velocity accumulates @@ -1503,11 +1520,14 @@ const float delta_time_s = 0.1f; MAHONY_FILTER filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - deg_to_rad(20.0f), - deg_to_rad(-10.0f), - deg_to_rad(15.0f) + eul_to_quat + ( + deg_to_rad(20.0f), + deg_to_rad(-10.0f), + deg_to_rad(15.0f) + ) ); VECTOR_3F zero_gyro = @@ -1823,11 +1843,14 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) + eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ) ); VECTOR_3F zero_gyro = @@ -2018,11 +2041,14 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) + eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ) ); VECTOR_3F zero_gyro = @@ -2103,11 +2129,14 @@ void test_mahony_integral_valid_error_accumulates { MAHONY_FILTER filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) + eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ) ); VECTOR_3F zero_gyro = @@ -2184,11 +2213,14 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) + eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ) ); VECTOR_3F zero_gyro = @@ -2271,11 +2303,14 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) + eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ) ); VECTOR_3F zero_gyro = @@ -2358,11 +2393,14 @@ const float expected_limit_rad_s = 0.25f; MAHONY_FILTER filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - deg_to_rad(90.0f) + eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(90.0f) + ) ); VECTOR_3F zero_gyro = @@ -2453,11 +2491,14 @@ void test_mahony_integral_correction_affects_attitude MAHONY_FILTER no_integral_filter; MAHONY_FILTER integral_filter; -QUAT initial_attitude = eul_to_quat +QUAT initial_attitude = quat_conj ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) + eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ) ); VECTOR_3F zero_gyro = diff --git a/test/math_sdr/test_math_sdr.c b/test/math_sdr/test_math_sdr.c index 75e58e6..ec6e694 100644 --- a/test/math_sdr/test_math_sdr.c +++ b/test/math_sdr/test_math_sdr.c @@ -124,7 +124,7 @@ QUAT attitude = { .w = cosf(half_angle), .x = 0.0f, - .y = sinf(half_angle), + .y = -sinf(half_angle), .z = 0.0f }; @@ -169,7 +169,7 @@ QUAT attitude = .w = cosf(half_angle), .x = 0.0f, .y = 0.0f, - .z = sinf(half_angle) + .z = -sinf(half_angle) }; QUAT vector_body = From f911c2d3d0db4f59c1a779b629002635d0458891 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Mon, 3 Aug 2026 16:54:22 -0700 Subject: [PATCH 09/18] Update Mahony tests for renamed framework --- test/mahony/Makefile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/mahony/Makefile b/test/mahony/Makefile index 42a4902..e0d53b8 100644 --- a/test/mahony/Makefile +++ b/test/mahony/Makefile @@ -32,8 +32,8 @@ $(ROOT_DIR)/mahony/mahony.c \ $(ROOT_DIR)/math_sdr/math_sdr.c FRAMEWORK_SOURCES = \ -$(ROOT_DIR)/test/framework/src/test_assert.c \ -$(ROOT_DIR)/test/framework/src/test_runner.c +$(ROOT_DIR)/_test/framework/src/test_assert.c \ +$(ROOT_DIR)/_test/framework/src/test_runner.c C_SOURCES = $(TEST_SOURCES) $(COV_C_SOURCES) $(FRAMEWORK_SOURCES) @@ -49,7 +49,7 @@ C_INCLUDES = \ -I. \ -I$(ROOT_DIR)/mahony \ -I$(ROOT_DIR)/math_sdr \ --I$(ROOT_DIR)/test/framework/src +-I$(ROOT_DIR)/_test/framework/src C_DEFS = \ -DUNIT_TEST From 13ce68cb0b0c2b6957015ca2c36e03c893787cb8 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 6 Aug 2026 19:08:26 -0700 Subject: [PATCH 10/18] Revert "Convert Mahony attitude to world-to-body convention" This reverts commit 4fe00973e3bb34d33771155f56557deea65f9031. --- _test/math_sdr/test_math_sdr.c | 4 +- mahony/mahony.c | 22 ++--- mahony/mahony.h | 11 ++- math_sdr/math_sdr.c | 20 ++--- math_sdr/math_sdr.h | 4 +- sensor/sensor.c | 6 +- sensor/sensor.h | 2 +- test/mahony/test_mahony.c | 145 ++++++++++++--------------------- 8 files changed, 86 insertions(+), 128 deletions(-) diff --git a/_test/math_sdr/test_math_sdr.c b/_test/math_sdr/test_math_sdr.c index ec6e694..75e58e6 100644 --- a/_test/math_sdr/test_math_sdr.c +++ b/_test/math_sdr/test_math_sdr.c @@ -124,7 +124,7 @@ QUAT attitude = { .w = cosf(half_angle), .x = 0.0f, - .y = -sinf(half_angle), + .y = sinf(half_angle), .z = 0.0f }; @@ -169,7 +169,7 @@ QUAT attitude = .w = cosf(half_angle), .x = 0.0f, .y = 0.0f, - .z = -sinf(half_angle) + .z = sinf(half_angle) }; QUAT vector_body = diff --git a/mahony/mahony.c b/mahony/mahony.c index d064cfe..7b4037b 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -246,7 +246,7 @@ return true; * Verify the filter, gyro, and timestep are valid. * Convert the body-frame angular velocity into a pure quaternion. * Use the quaternion differential equation to calculate how quickly the - * world-to-body attitude is changing. Multiply that derivative by the + * body-to-world attitude is changing. Multiply that derivative by the * timestep, add it to the current attitude, and normalize the result. */ @@ -283,10 +283,10 @@ if ( !isfinite(delta_time_s) || } /* - * The attitude quaternion represents the world-to-body rotation, while angular - * velocity is expressed in the body frame: + * The attitude quaternion is a body-to-world rotation and angular velocity is + * expressed in the body frame: * - * q_dot = -0.5 * omega_body * q + * q_dot = 0.5 * q * omega_body */ angular_velocity.w = 0.0f; angular_velocity.x = gyro_body_rad_s.x; @@ -295,14 +295,14 @@ angular_velocity.z = gyro_body_rad_s.z; attitude_derivative = quat_mult ( - angular_velocity, - filter->attitude + filter->attitude, + angular_velocity ); attitude_derivative = quat_scale ( attitude_derivative, - -0.5f + 0.5f ); attitude_delta = quat_scale @@ -384,10 +384,10 @@ if ( apply_accel ) if ( vector_normalize(&accel_body) ) { /* - * The attitude quaternion represents the world-to-body rotation. Rotate the - * fixed world gravity direction directly into the body frame to predict where - * gravity should appear according to the current attitude estimate. - */ + * The attitude quaternion represents the body-to-world rotation. + * Rotate the fixed world gravity direction into the body frame to + * predict where gravity should appear according to the estimate. + */ gravity_world.w = 0.0f; gravity_world.x = 0.0f; gravity_world.y = 0.0f; diff --git a/mahony/mahony.h b/mahony/mahony.h index 1970ee1..38e1c48 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -46,8 +46,8 @@ typedef struct _VECTOR_3F typedef struct _MAHONY_FILTER { /** - * World-to-body attitude quaternion. - */ + * Body-to-world attitude quaternion. + */ QUAT attitude; /** @@ -68,7 +68,7 @@ typedef struct _MAHONY_FILTER * @brief Initializes a Mahony attitude filter. * * @param filter Filter instance to initialize. - * @param initial_attitude Initial world-to-body attitude quaternion. + * @param initial_attitude Initial body-to-world attitude quaternion. * @param proportional_gain Proportional correction gain. * @param integral_gain Integral correction gain. * @@ -83,10 +83,9 @@ bool mahony_init ); /** - * @brief Propagates a world-to-body attitude using body-frame gyroscope - * measurements. + * @brief Propagates attitude using body-frame gyroscope measurements. * - * The attitude quaternion represents the world-to-body rotation. The angular + * The attitude quaternion represents the body-to-world rotation. The angular * velocity vector must be expressed in the body frame in radians per second. * * @param filter Initialized filter instance. diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index cb742db..cbdfe2a 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -265,11 +265,11 @@ return result; /** * @brief Rotates a vector from the world frame into the body frame. * - * The attitude quaternion represents the world-to-body rotation: + * The attitude quaternion represents the body-to-world rotation: * - * vector_body = q * vector_world * conjugate(q) + * vector_body = conjugate(q) * vector_world * q * - * @param attitude World-to-body attitude quaternion. + * @param attitude Body-to-world attitude quaternion. * @param vector_world Pure quaternion containing the world-frame vector. * * @return Pure quaternion containing the body-frame vector. @@ -284,8 +284,8 @@ QUAT attitude_conj = quat_conj(attitude); return quat_mult ( - quat_mult(attitude, vector_world), - attitude_conj + quat_mult(attitude_conj, vector_world), + attitude ); } /* quat_rotate_world_to_body */ @@ -294,11 +294,11 @@ return quat_mult /** * @brief Rotates a vector from the body frame into the world frame. * - * The attitude quaternion represents the world-to-body rotation: + * The attitude quaternion represents the body-to-world rotation: * - * vector_world = conjugate(q) * vector_body * q + * vector_world = q * vector_body * conjugate(q) * - * @param attitude World-to-body attitude quaternion. + * @param attitude Body-to-world attitude quaternion. * @param vector_body Pure quaternion containing the body-frame vector. * * @return Pure quaternion containing the world-frame vector. @@ -313,8 +313,8 @@ QUAT attitude_conj = quat_conj(attitude); return quat_mult ( - quat_mult(attitude_conj, vector_body), - attitude + quat_mult(attitude, vector_body), + attitude_conj ); } /* quat_rotate_body_to_world */ diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 0249ba1..a884f2a 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -163,7 +163,7 @@ QUAT quat_conj /** * @brief Rotates a vector from the world frame into the body frame. * - * @param attitude World-to-body attitude quaternion. + * @param attitude Body-to-world attitude quaternion. * @param vector_world Pure quaternion containing the world-frame vector. * * @return Pure quaternion containing the body-frame vector. @@ -177,7 +177,7 @@ QUAT quat_rotate_world_to_body /** * @brief Rotates a vector from the body frame into the world frame. * - * @param attitude World-to-body attitude quaternion. + * @param attitude Body-to-world attitude quaternion. * @param vector_body Pure quaternion containing the body-frame vector. * * @return Pure quaternion containing the world-frame vector. diff --git a/sensor/sensor.c b/sensor/sensor.c index b19c88c..5dfe976 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -531,7 +531,7 @@ use_accel = ); /* - * Store the filter's world-to-body quaternion as the system attitude estimate. + * Store the filter's body-to-world quaternion as the system attitude estimate. */ state_estimate->attitude = mahony_filter.attitude; @@ -637,8 +637,8 @@ const QUAT gravity_world = }; /* - * The attitude quaternion is world-to-body, so rotate world gravity directly - * into the body frame before subtracting it from measured acceleration. + * The attitude quaternion is body-to-world, so rotate world gravity + * into the body frame before subtracting it from the accelerometer. */ QUAT gravity_body = quat_rotate_world_to_body ( diff --git a/sensor/sensor.h b/sensor/sensor.h index 4058285..9b42fa7 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -97,7 +97,7 @@ typedef enum /* State estimation from processed sensors */ typedef struct _STATE_ESTIMATION { /* - * World-to-body attitude quaternion. + * Body-to-world attitude quaternion. * * Body-frame vector to world frame: * v_world = q * v_body * conjugate(q) diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 5288eed..684d914 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -72,10 +72,10 @@ return value; /** - * @brief Converts a world-to-body quaternion into ZYX Euler angles. + * @brief Converts a body-to-world quaternion into ZYX Euler angles. * - * The world-to-body attitude is conjugated into its equivalent body-to-world - * quaternion before extracting roll, pitch, and yaw for diagnostic display. + * The returned values represent roll about X, pitch about Y, and yaw about Z. + * Angles are returned in degrees for readable diagnostic output. */ static VECTOR_3F quat_to_euler_deg ( @@ -84,14 +84,6 @@ static VECTOR_3F quat_to_euler_deg { VECTOR_3F angles_deg; -/* - * The Euler extraction formulas below operate on a body-to-world quaternion. - */ -attitude = quat_conj - ( - quat_normalize(attitude) - ); - float sin_roll_cos_pitch; float cos_roll_cos_pitch; float sin_pitch; @@ -609,7 +601,7 @@ assert_quat_components * it points toward world positive Y. * * This is important because it confirms the quaternion multiplication order, - * rotation sign, gyro units, and world-to-body attitude convention. + * rotation sign, gyro units, and body-to-world attitude convention. */ void test_mahony_update_gyro_positive_yaw ( @@ -678,9 +670,9 @@ TEST_ASSERT_EQ_FLOAT TEST_ASSERT_EQ_FLOAT ( - "Positive 90-degree yaw world-to-body quaternion z component", + "Positive 90-degree yaw quaternion z component", filter.attitude.z, - -sinf(0.25f * TEST_PI) + sinf(0.25f * TEST_PI) ); QUAT world_vector = quat_rotate_body_to_world @@ -857,7 +849,7 @@ assert_quat_components * * This is important because it verifies that proportional accelerometer * feedback corrects roll drift and that the cross-product sign is consistent - * with the world-to-body quaternion convention. + * with the body-to-world quaternion convention. */ void test_mahony_update_imu_roll_error_converges ( @@ -870,14 +862,11 @@ MAHONY_FILTER filter; const float initial_roll_rad = deg_to_rad(10.0f); -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - initial_roll_rad - ) + 0.0f, + 0.0f, + initial_roll_rad ); VECTOR_3F zero_gyro = @@ -978,14 +967,11 @@ MAHONY_FILTER filter; const float initial_pitch_rad = deg_to_rad(10.0f); -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - initial_pitch_rad, - 0.0f - ) + 0.0f, + initial_pitch_rad, + 0.0f ); VECTOR_3F zero_gyro = @@ -1086,14 +1072,11 @@ MAHONY_FILTER filter; const float initial_yaw_rad = deg_to_rad(20.0f); -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - initial_yaw_rad, - 0.0f, - 0.0f - ) + initial_yaw_rad, + 0.0f, + 0.0f ); VECTOR_3F zero_gyro = @@ -1391,7 +1374,7 @@ assert_quat_components * @brief Prints attitude propagation over ten gyro-only update intervals. * * This test simulates a rocket rotating simultaneously about all three body - * axes. It prints the estimated roll, pitch, yaw, and world-to-body quaternion + * axes. It prints the estimated roll, pitch, yaw, and body-to-world quaternion * after each update. * * This is useful for visually confirming that angular velocity accumulates @@ -1520,14 +1503,11 @@ const float delta_time_s = 0.1f; MAHONY_FILTER filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - deg_to_rad(20.0f), - deg_to_rad(-10.0f), - deg_to_rad(15.0f) - ) + deg_to_rad(20.0f), + deg_to_rad(-10.0f), + deg_to_rad(15.0f) ); VECTOR_3F zero_gyro = @@ -1843,14 +1823,11 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) - ) + 0.0f, + 0.0f, + deg_to_rad(10.0f) ); VECTOR_3F zero_gyro = @@ -2041,14 +2018,11 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) - ) + 0.0f, + 0.0f, + deg_to_rad(10.0f) ); VECTOR_3F zero_gyro = @@ -2129,14 +2103,11 @@ void test_mahony_integral_valid_error_accumulates { MAHONY_FILTER filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) - ) + 0.0f, + 0.0f, + deg_to_rad(10.0f) ); VECTOR_3F zero_gyro = @@ -2213,14 +2184,11 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) - ) + 0.0f, + 0.0f, + deg_to_rad(10.0f) ); VECTOR_3F zero_gyro = @@ -2303,14 +2271,11 @@ int32_t index; MAHONY_FILTER filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) - ) + 0.0f, + 0.0f, + deg_to_rad(10.0f) ); VECTOR_3F zero_gyro = @@ -2393,14 +2358,11 @@ const float expected_limit_rad_s = 0.25f; MAHONY_FILTER filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - deg_to_rad(90.0f) - ) + 0.0f, + 0.0f, + deg_to_rad(90.0f) ); VECTOR_3F zero_gyro = @@ -2491,14 +2453,11 @@ void test_mahony_integral_correction_affects_attitude MAHONY_FILTER no_integral_filter; MAHONY_FILTER integral_filter; -QUAT initial_attitude = quat_conj +QUAT initial_attitude = eul_to_quat ( - eul_to_quat - ( - 0.0f, - 0.0f, - deg_to_rad(10.0f) - ) + 0.0f, + 0.0f, + deg_to_rad(10.0f) ); VECTOR_3F zero_gyro = From ee214a3a133ad9fcb624ec67b7c67dccad5bc303 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 9 Aug 2026 13:57:41 -0700 Subject: [PATCH 11/18] Add Mahony status enum --- mahony/mahony.c | 46 ++- mahony/mahony.h | 32 +- test/mahony/test_mahony.c | 688 +++++++++++++++++++++++--------------- 3 files changed, 458 insertions(+), 308 deletions(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index 7b4037b..1a7c359 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -196,10 +196,9 @@ return result; /*------------------------------------------------------------------------------ - Public Functions - ------------------------------------------------------------------------------*/ - -bool mahony_init + * Public Functions + *----------------------------------------------------------------------------*/ +MAHONY_STATUS mahony_init ( MAHONY_FILTER *filter, QUAT initial_attitude, @@ -209,37 +208,34 @@ bool mahony_init { if ( filter == NULL ) { - return false; + return MAHONY_NULL_POINTER; } if ( !mahony_quat_is_finite(initial_attitude) ) { - return false; + return MAHONY_INVALID_QUATERNION; } if ( !isfinite(proportional_gain) || !isfinite(integral_gain) ) { - return false; + return MAHONY_NONFINITE_GAIN; } if ( proportional_gain < 0.0f || integral_gain < 0.0f ) { - return false; + return MAHONY_NEGATIVE_GAIN; } filter->attitude = quat_normalize(initial_attitude); - filter->integral_error.x = 0.0f; filter->integral_error.y = 0.0f; filter->integral_error.z = 0.0f; - filter->proportional_gain = proportional_gain; filter->integral_gain = integral_gain; -return true; - +return MAHONY_OK; } /* mahony_init */ /* @@ -250,7 +246,7 @@ return true; * timestep, add it to the current attitude, and normalize the result. */ -bool mahony_update_gyro +MAHONY_STATUS mahony_update_gyro ( MAHONY_FILTER *filter, VECTOR_3F gyro_body_rad_s, @@ -263,30 +259,30 @@ QUAT attitude_delta; if ( filter == NULL ) { - return false; + return MAHONY_NULL_POINTER; } if ( !mahony_quat_is_finite(filter->attitude) ) { - return false; + return MAHONY_INVALID_QUATERNION; } if ( !mahony_vector_is_finite(gyro_body_rad_s) ) { - return false; + return MAHONY_INVALID_GYRO; } if ( !isfinite(delta_time_s) || delta_time_s <= 0.0f ) { - return false; + return MAHONY_INVALID_DELTA_TIME; } /* - * The attitude quaternion is a body-to-world rotation and angular velocity is - * expressed in the body frame: + * The attitude quaternion is a body-to-world rotation and angular velocity + * is expressed in the body frame: * - * q_dot = 0.5 * q * omega_body + * q_dot = 0.5 * q * omega_body */ angular_velocity.w = 0.0f; angular_velocity.x = gyro_body_rad_s.x; @@ -319,11 +315,11 @@ filter->attitude = quat_add filter->attitude = quat_normalize(filter->attitude); -return true; +return MAHONY_OK; } /* mahony_update_gyro */ -bool mahony_update_imu +MAHONY_STATUS mahony_update_imu ( MAHONY_FILTER *filter, VECTOR_3F gyro_body_rad_s, @@ -347,18 +343,18 @@ bool apply_accel; if ( filter == NULL ) { - return false; + return MAHONY_NULL_POINTER; } if ( !mahony_vector_is_finite(gyro_body_rad_s) ) { - return false; + return MAHONY_INVALID_GYRO; } if ( !isfinite(delta_time_s) || delta_time_s <= 0.0f ) { - return false; + return MAHONY_INVALID_DELTA_TIME; } accel_magnitude = vector_magnitude(accel_body); diff --git a/mahony/mahony.h b/mahony/mahony.h index 38e1c48..043c7cd 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -40,6 +40,23 @@ typedef struct _VECTOR_3F float z; } VECTOR_3F; +/** + * @brief Mahony attitude filter status codes. + */ +typedef enum + { + MAHONY_OK = 0, //success (SDR convention) + MAHONY_NULL_POINTER, //filter == NULL + MAHONY_INVALID_QUATERNION, //attitude contains NaN/Inf + MAHONY_NONFINITE_GAIN, //Kp or Ki is NaN/Inf + MAHONY_NEGATIVE_GAIN, //Kp or Ki is below zero + MAHONY_INVALID_GYRO, //gyro contains NaN/Inf + MAHONY_INVALID_DELTA_TIME //dt is NaN/Inf, zero, or negative + // no MAHONY_INVALID_ACCEL because Invalid acceleration is.. + // deliberately treated as "don't use accel correction; continue gyro-only,".. + // not as a failed Mahony update. + } MAHONY_STATUS; + /** * @brief State and gains for a Mahony attitude filter. */ @@ -72,9 +89,10 @@ typedef struct _MAHONY_FILTER * @param proportional_gain Proportional correction gain. * @param integral_gain Integral correction gain. * - * @return true when initialization succeeds; otherwise false. + * @return MAHONY_OK when initialization succeeds; otherwise a Mahony status + * code describing the failure. */ -bool mahony_init +MAHONY_STATUS mahony_init ( MAHONY_FILTER *filter, QUAT initial_attitude, @@ -92,9 +110,10 @@ bool mahony_init * @param gyro_body_rad_s Body-frame angular velocity in radians per second. * @param delta_time_s Elapsed time in seconds. * - * @return true when the attitude was updated; otherwise false. + * @return MAHONY_OK when initialization succeeds; otherwise a Mahony status + code describing the failure. */ -bool mahony_update_gyro +MAHONY_STATUS mahony_update_gyro ( MAHONY_FILTER *filter, VECTOR_3F gyro_body_rad_s, @@ -119,10 +138,11 @@ bool mahony_update_gyro * @param delta_time_s Elapsed time in seconds. * @param use_accel Whether accelerometer feedback should be applied. * - * @return true when the attitude was updated; otherwise false. + * @return MAHONY_OK when the attitude was updated; otherwise a Mahony status + code describing the failure. */ -bool mahony_update_imu +MAHONY_STATUS mahony_update_imu ( MAHONY_FILTER *filter, VECTOR_3F gyro_body_rad_s, diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 684d914..94efb9d 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -247,7 +247,7 @@ QUAT identity = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -256,7 +256,8 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); assert_quat_components @@ -301,7 +302,7 @@ QUAT expected = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -310,7 +311,8 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.0f - ) + ), + MAHONY_OK ); assert_quat_components @@ -355,7 +357,7 @@ QUAT expected = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -364,7 +366,8 @@ TEST_ASSERT_TRUE zero_quaternion, 1.0f, 0.0f - ) + ), + MAHONY_OK ); assert_quat_components @@ -400,7 +403,7 @@ QUAT identity = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -409,7 +412,8 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.1f - ) + ), + MAHONY_OK ); TEST_ASSERT_EQ_FLOAT @@ -457,7 +461,7 @@ QUAT identity = .z = 0.0f }; -TEST_ASSERT_FALSE +TEST_ASSERT_EQ_UINT ( "Null filter is rejected", mahony_init @@ -466,7 +470,8 @@ TEST_ASSERT_FALSE identity, 1.0f, 0.0f - ) + ), + MAHONY_NULL_POINTER ); } /* test_mahony_init_rejects_null_filter */ @@ -496,7 +501,7 @@ QUAT identity = .z = 0.0f }; -TEST_ASSERT_FALSE +TEST_ASSERT_EQ_UINT ( "Negative proportional gain is rejected", mahony_init @@ -505,10 +510,11 @@ TEST_ASSERT_FALSE identity, -1.0f, 0.0f - ) + ), + MAHONY_NEGATIVE_GAIN ); -TEST_ASSERT_FALSE +TEST_ASSERT_EQ_UINT ( "Negative integral gain is rejected", mahony_init @@ -517,7 +523,8 @@ TEST_ASSERT_FALSE identity, 1.0f, -0.1f - ) + ), + MAHONY_NEGATIVE_GAIN ); } /* test_mahony_init_rejects_negative_gain */ @@ -558,7 +565,7 @@ VECTOR_3F zero_rate = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -567,10 +574,11 @@ TEST_ASSERT_TRUE identity, 0.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Zero-rate update succeeds", mahony_update_gyro @@ -578,7 +586,8 @@ TEST_ASSERT_TRUE &filter, zero_rate, 0.01f - ) + ), + MAHONY_OK ); assert_quat_components @@ -610,6 +619,8 @@ void test_mahony_update_gyro_positive_yaw { int32_t index; +MAHONY_STATUS status = MAHONY_OK; + MAHONY_FILTER filter; QUAT identity = @@ -635,7 +646,7 @@ QUAT body_x = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -644,23 +655,27 @@ TEST_ASSERT_TRUE identity, 0.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE + status |= mahony_update_gyro ( - "Positive-yaw propagation succeeds", - mahony_update_gyro - ( - &filter, - gyro_body_rad_s, - 0.001f - ) + &filter, + gyro_body_rad_s, + 0.001f ); } +TEST_ASSERT_EQ_UINT + ( + "Positive-yaw propagation succeeds", + status, + MAHONY_OK + ); + TEST_ASSERT_EQ_FLOAT ( "Positive 90-degree yaw quaternion w component", @@ -722,7 +737,7 @@ VECTOR_3F zero_rate = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -731,10 +746,11 @@ TEST_ASSERT_TRUE identity, 0.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_FALSE +TEST_ASSERT_EQ_UINT ( "Zero delta time is rejected", mahony_update_gyro @@ -742,10 +758,11 @@ TEST_ASSERT_FALSE &filter, zero_rate, 0.0f - ) + ), + MAHONY_INVALID_DELTA_TIME ); -TEST_ASSERT_FALSE +TEST_ASSERT_EQ_UINT ( "Negative delta time is rejected", mahony_update_gyro @@ -753,7 +770,8 @@ TEST_ASSERT_FALSE &filter, zero_rate, -0.01f - ) + ), + MAHONY_INVALID_DELTA_TIME ); } /* test_mahony_update_gyro_rejects_invalid_delta_time */ @@ -778,6 +796,8 @@ void test_mahony_update_imu_aligned_gravity { int32_t index; +MAHONY_STATUS status = MAHONY_OK; + MAHONY_FILTER filter; QUAT identity = @@ -802,7 +822,7 @@ VECTOR_3F accel_body = .z = GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -811,25 +831,29 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE - ( - "Aligned IMU update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - accel_body, - 0.001f, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Aligned IMU updates succeed", + status, + MAHONY_OK + ); + assert_quat_components ( "Aligned gravity preserves identity attitude", @@ -858,6 +882,8 @@ void test_mahony_update_imu_roll_error_converges { int32_t index; +MAHONY_STATUS status = MAHONY_OK; + MAHONY_FILTER filter; const float initial_roll_rad = deg_to_rad(10.0f); @@ -897,7 +923,7 @@ QUAT initial_world_z = quat_rotate_body_to_world body_z ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -906,25 +932,29 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 2000; index++ ) { - TEST_ASSERT_TRUE - ( - "Roll-error correction update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - accel_body, - 0.001f, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Roll-error correction updates succeed", + status, + MAHONY_OK + ); + QUAT corrected_world_z = quat_rotate_body_to_world ( filter.attitude, @@ -963,6 +993,8 @@ void test_mahony_update_imu_pitch_error_converges { int32_t index; +MAHONY_STATUS status = MAHONY_OK; + MAHONY_FILTER filter; const float initial_pitch_rad = deg_to_rad(10.0f); @@ -1002,7 +1034,7 @@ QUAT initial_world_z = quat_rotate_body_to_world body_z ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -1011,25 +1043,29 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 2000; index++ ) { - TEST_ASSERT_TRUE - ( - "Pitch-error correction update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - accel_body, - 0.001f, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Pitch-error correction updates succeed", + status, + MAHONY_OK + ); + QUAT corrected_world_z = quat_rotate_body_to_world ( filter.attitude, @@ -1068,6 +1104,8 @@ void test_mahony_update_imu_yaw_error_does_not_converge { int32_t index; +MAHONY_STATUS status = MAHONY_OK; + MAHONY_FILTER filter; const float initial_yaw_rad = deg_to_rad(20.0f); @@ -1107,7 +1145,7 @@ QUAT initial_world_x = quat_rotate_body_to_world body_x ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -1116,25 +1154,29 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 2000; index++ ) { - TEST_ASSERT_TRUE - ( - "Yaw-only correction update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - accel_body, - 0.001f, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Yaw-only correction updates succeed", + status, + MAHONY_OK + ); + QUAT final_world_x = quat_rotate_body_to_world ( filter.attitude, @@ -1178,6 +1220,9 @@ void test_mahony_update_imu_zero_accel_uses_gyro_only { int32_t index; +MAHONY_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + MAHONY_FILTER gyro_filter; MAHONY_FILTER imu_filter; @@ -1203,7 +1248,7 @@ VECTOR_3F zero_accel = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Gyro filter initialization succeeds", mahony_init @@ -1212,10 +1257,11 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "IMU filter initialization succeeds", mahony_init @@ -1224,36 +1270,43 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE + gyro_status |= mahony_update_gyro ( - "Gyro-only update succeeds", - mahony_update_gyro - ( - &gyro_filter, - gyro_body_rad_s, - 0.001f - ) + &gyro_filter, + gyro_body_rad_s, + 0.001f ); - TEST_ASSERT_TRUE - ( - "Zero-accelerometer IMU update succeeds", - mahony_update_imu - ( - &imu_filter, - gyro_body_rad_s, - zero_accel, - 0.001f, - true - ) + imu_status |= mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + zero_accel, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Gyro-only updates succeed", + gyro_status, + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Zero-accelerometer IMU updates succeed", + imu_status, + MAHONY_OK + ); + assert_quat_components ( "Zero accelerometer matches gyro-only propagation", @@ -1285,6 +1338,9 @@ void test_mahony_update_imu_disabled_accel_uses_gyro_only { int32_t index; +MAHONY_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + MAHONY_FILTER gyro_filter; MAHONY_FILTER imu_filter; @@ -1310,7 +1366,7 @@ VECTOR_3F misaligned_accel = .z = 0.0f }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Gyro filter initialization succeeds", mahony_init @@ -1319,10 +1375,11 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "IMU filter initialization succeeds", mahony_init @@ -1331,36 +1388,43 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE + gyro_status |= mahony_update_gyro ( - "Gyro-only update succeeds", - mahony_update_gyro - ( - &gyro_filter, - gyro_body_rad_s, - 0.001f - ) + &gyro_filter, + gyro_body_rad_s, + 0.001f ); - TEST_ASSERT_TRUE - ( - "Disabled-accelerometer IMU update succeeds", - mahony_update_imu - ( - &imu_filter, - gyro_body_rad_s, - misaligned_accel, - 0.001f, - false - ) + imu_status |= mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + misaligned_accel, + 0.001f, + false ); } +TEST_ASSERT_EQ_UINT + ( + "Gyro-only updates succeed", + gyro_status, + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Disabled-accelerometer IMU updates succeed", + imu_status, + MAHONY_OK + ); + assert_quat_components ( "Disabled accelerometer matches gyro-only propagation", @@ -1388,6 +1452,8 @@ void test_mahony_print_gyro_propagation { int32_t interval; +MAHONY_STATUS status = MAHONY_OK; + const int32_t interval_count = 10; const float delta_time_s = 0.1f; @@ -1415,7 +1481,7 @@ VECTOR_3F gyro_body_rad_s = .z = deg_to_rad(20.0f) }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Gyro diagnostic filter initialization succeeds", mahony_init @@ -1424,7 +1490,8 @@ TEST_ASSERT_TRUE identity, 0.0f, 0.0f - ) + ), + MAHONY_OK ); print_attitude_heading @@ -1441,15 +1508,11 @@ print_attitude_sample for ( interval = 1; interval <= interval_count; interval++ ) { - TEST_ASSERT_TRUE + status |= mahony_update_gyro ( - "Gyro diagnostic propagation succeeds", - mahony_update_gyro - ( - &filter, - gyro_body_rad_s, - delta_time_s - ) + &filter, + gyro_body_rad_s, + delta_time_s ); print_attitude_sample @@ -1460,6 +1523,13 @@ for ( interval = 1; interval <= interval_count; interval++ ) ); } +TEST_ASSERT_EQ_UINT + ( + "Gyro diagnostic propagation succeeds", + status, + MAHONY_OK + ); + /* * A propagated attitude must remain a unit quaternion. */ @@ -1498,6 +1568,8 @@ void test_mahony_print_accelerometer_correction { int32_t interval; +MAHONY_STATUS status = MAHONY_OK; + const int32_t interval_count = 10; const float delta_time_s = 0.1f; @@ -1542,7 +1614,7 @@ QUAT initial_world_z = quat_rotate_body_to_world body_z ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Accelerometer diagnostic filter initialization succeeds", mahony_init @@ -1551,7 +1623,8 @@ TEST_ASSERT_TRUE initial_attitude, 1.5f, 0.0f - ) + ), + MAHONY_OK ); print_attitude_heading @@ -1568,17 +1641,13 @@ print_attitude_sample for ( interval = 1; interval <= interval_count; interval++ ) { - TEST_ASSERT_TRUE - ( - "Accelerometer diagnostic update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - accel_body, - delta_time_s, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + delta_time_s, + true ); print_attitude_sample @@ -1589,6 +1658,13 @@ for ( interval = 1; interval <= interval_count; interval++ ) ); } +TEST_ASSERT_EQ_UINT + ( + "Accelerometer diagnostic updates succeed", + status, + MAHONY_OK + ); + QUAT final_world_z = quat_rotate_body_to_world ( filter.attitude, @@ -1624,6 +1700,9 @@ void test_mahony_update_imu_rejects_low_accel_magnitude { int32_t index; +MAHONY_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + MAHONY_FILTER gyro_filter; MAHONY_FILTER imu_filter; @@ -1649,7 +1728,7 @@ VECTOR_3F low_accel = .z = 0.50f * GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Gyro filter initialization succeeds", mahony_init @@ -1658,10 +1737,11 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "IMU filter initialization succeeds", mahony_init @@ -1670,36 +1750,43 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE + gyro_status |= mahony_update_gyro ( - "Gyro-only update succeeds", - mahony_update_gyro - ( - &gyro_filter, - gyro_body_rad_s, - 0.001f - ) + &gyro_filter, + gyro_body_rad_s, + 0.001f ); - TEST_ASSERT_TRUE - ( - "Low-acceleration IMU update succeeds", - mahony_update_imu - ( - &imu_filter, - gyro_body_rad_s, - low_accel, - 0.001f, - true - ) + imu_status |= mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + low_accel, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Gyro-only updates succeed", + gyro_status, + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Low-acceleration IMU updates succeed", + imu_status, + MAHONY_OK + ); + assert_quat_components ( "Low acceleration matches gyro-only propagation", @@ -1748,7 +1835,7 @@ VECTOR_3F invalid_accel = .z = GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Gyro filter initialization succeeds", mahony_init @@ -1757,10 +1844,11 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "IMU filter initialization succeeds", mahony_init @@ -1769,10 +1857,11 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Gyro-only update succeeds", mahony_update_gyro @@ -1780,10 +1869,11 @@ TEST_ASSERT_TRUE &gyro_filter, gyro_body_rad_s, 0.01f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Invalid-accelerometer IMU update succeeds", mahony_update_imu @@ -1793,7 +1883,8 @@ TEST_ASSERT_TRUE invalid_accel, 0.01f, true - ) + ), + MAHONY_OK ); assert_quat_components @@ -1821,6 +1912,8 @@ void test_mahony_update_imu_accepts_valid_accel_magnitude { int32_t index; +MAHONY_STATUS status = MAHONY_OK; + MAHONY_FILTER filter; QUAT initial_attitude = eul_to_quat @@ -1858,7 +1951,7 @@ QUAT initial_world_z = quat_rotate_body_to_world body_z ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -1867,25 +1960,29 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE - ( - "Valid-acceleration IMU update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - valid_accel, - 0.001f, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Valid-acceleration IMU updates succeed", + status, + MAHONY_OK + ); + QUAT corrected_world_z = quat_rotate_body_to_world ( filter.attitude, @@ -1918,6 +2015,9 @@ void test_mahony_update_imu_rejects_high_accel_magnitude { int32_t index; +MAHONY_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + MAHONY_FILTER gyro_filter; MAHONY_FILTER imu_filter; @@ -1943,7 +2043,7 @@ VECTOR_3F high_accel = .z = 2.0f * GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Gyro filter initialization succeeds", mahony_init @@ -1952,10 +2052,11 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "IMU filter initialization succeeds", mahony_init @@ -1964,36 +2065,43 @@ TEST_ASSERT_TRUE identity, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE + gyro_status |= mahony_update_gyro ( - "Gyro-only update succeeds", - mahony_update_gyro - ( - &gyro_filter, - gyro_body_rad_s, - 0.001f - ) + &gyro_filter, + gyro_body_rad_s, + 0.001f ); - TEST_ASSERT_TRUE - ( - "High-acceleration IMU update succeeds", - mahony_update_imu - ( - &imu_filter, - gyro_body_rad_s, - high_accel, - 0.001f, - true - ) + imu_status |= mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + high_accel, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Gyro-only updates succeed", + gyro_status, + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "High-acceleration IMU updates succeed", + imu_status, + MAHONY_OK + ); + assert_quat_components ( "High acceleration matches gyro-only propagation", @@ -2018,6 +2126,8 @@ int32_t index; MAHONY_FILTER filter; +MAHONY_STATUS status = MAHONY_OK; + QUAT initial_attitude = eul_to_quat ( 0.0f, @@ -2039,7 +2149,7 @@ VECTOR_3F valid_accel = .z = GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -2048,25 +2158,29 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.0f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE - ( - "Zero-Ki update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - valid_accel, - 0.001f, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Zero-Ki updates succeed", + status, + MAHONY_OK + ); + TEST_ASSERT_EQ_FLOAT ( "Zero Ki preserves integral X", @@ -2124,7 +2238,7 @@ VECTOR_3F valid_accel = .z = GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -2133,10 +2247,11 @@ TEST_ASSERT_TRUE initial_attitude, 0.0f, 0.5f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Integral update succeeds", mahony_update_imu @@ -2146,7 +2261,8 @@ TEST_ASSERT_TRUE valid_accel, 0.1f, true - ) + ), + MAHONY_OK ); TEST_ASSERT_TRUE @@ -2184,6 +2300,8 @@ int32_t index; MAHONY_FILTER filter; +MAHONY_STATUS status = MAHONY_OK; + QUAT initial_attitude = eul_to_quat ( 0.0f, @@ -2205,7 +2323,7 @@ VECTOR_3F valid_accel = .z = GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -2214,25 +2332,29 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.5f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE - ( - "Disabled-accelerometer update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - valid_accel, - 0.001f, - false - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + false ); } +TEST_ASSERT_EQ_UINT + ( + "Disabled-accelerometer updates succeed", + status, + MAHONY_OK + ); + TEST_ASSERT_EQ_FLOAT ( "Disabled accelerometer preserves integral X", @@ -2269,6 +2391,8 @@ void test_mahony_integral_invalid_accel_does_not_accumulate { int32_t index; +MAHONY_STATUS status = MAHONY_OK; + MAHONY_FILTER filter; QUAT initial_attitude = eul_to_quat @@ -2292,7 +2416,7 @@ VECTOR_3F invalid_accel = .z = 2.0f * GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -2301,25 +2425,29 @@ TEST_ASSERT_TRUE initial_attitude, 1.0f, 0.5f - ) + ), + MAHONY_OK ); for ( index = 0; index < 1000; index++ ) { - TEST_ASSERT_TRUE - ( - "Invalid-accelerometer update succeeds", - mahony_update_imu - ( - &filter, - zero_gyro, - invalid_accel, - 0.001f, - true - ) + status |= mahony_update_imu + ( + &filter, + zero_gyro, + invalid_accel, + 0.001f, + true ); } +TEST_ASSERT_EQ_UINT + ( + "Invalid-accelerometer updates succeed", + status, + MAHONY_OK + ); + TEST_ASSERT_EQ_FLOAT ( "Invalid acceleration preserves integral X", @@ -2379,7 +2507,7 @@ VECTOR_3F valid_accel = .z = GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Mahony initialization succeeds", mahony_init @@ -2388,10 +2516,11 @@ TEST_ASSERT_TRUE initial_attitude, 0.0f, 100.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "High-integral-gain update succeeds", mahony_update_imu @@ -2401,7 +2530,8 @@ TEST_ASSERT_TRUE valid_accel, 1.0f, true - ) + ), + MAHONY_OK ); TEST_ASSERT_TRUE @@ -2474,7 +2604,7 @@ VECTOR_3F valid_accel = .z = GRAVITY }; -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "No-integral filter initialization succeeds", mahony_init @@ -2483,10 +2613,11 @@ TEST_ASSERT_TRUE initial_attitude, 0.0f, 0.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Integral filter initialization succeeds", mahony_init @@ -2495,10 +2626,11 @@ TEST_ASSERT_TRUE initial_attitude, 0.0f, 1.0f - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "No-integral update succeeds", mahony_update_imu @@ -2508,10 +2640,11 @@ TEST_ASSERT_TRUE valid_accel, 0.1f, true - ) + ), + MAHONY_OK ); -TEST_ASSERT_TRUE +TEST_ASSERT_EQ_UINT ( "Integral update succeeds", mahony_update_imu @@ -2521,7 +2654,8 @@ TEST_ASSERT_TRUE valid_accel, 0.1f, true - ) + ), + MAHONY_OK ); TEST_ASSERT_TRUE From c8efc99942002d395abcd009f6577343b4831c25 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 9 Aug 2026 14:27:26 -0700 Subject: [PATCH 12/18] Handle Mahony status in sensor integration --- sensor/sensor.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 5dfe976..784792d 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -46,6 +46,8 @@ #include "sensor.h" #include "math_sdr.h" #include "mahony.h" +#include "error_sdr.h" +#include "debug_sdr.h" /*------------------------------------------------------------------------------ Private Macros @@ -380,7 +382,7 @@ mahony_tick = imu_velo_tick; sensor_reset_velo(); -(void)mahony_init +MAHONY_STATUS mahony_status = mahony_init ( &mahony_filter, initial_attitude, @@ -388,6 +390,11 @@ sensor_reset_velo(); SENSOR_MAHONY_KI ); +if ( mahony_status != MAHONY_OK ) + { + error_fail_fast( ERROR_SENSOR_CMD_ERROR ); + } + } /* sensor_init */ @@ -521,7 +528,7 @@ accel_body_m_s2.z = imu_converted->accel_z; use_accel = get_fc_state() <= FC_STATE_LAUNCH_DETECT; -(void)mahony_update_imu +MAHONY_STATUS mahony_status = mahony_update_imu ( &mahony_filter, gyro_body_rad_s, @@ -530,6 +537,12 @@ use_accel = use_accel ); +debug_assert + ( + mahony_status == MAHONY_OK, + ERROR_SENSOR_CMD_ERROR + ); + /* * Store the filter's body-to-world quaternion as the system attitude estimate. */ From c9858e96f2b17072c22d1d68baaf17ad9b16f051 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 9 Aug 2026 14:40:43 -0700 Subject: [PATCH 13/18] Fix Mahony status check in release build --- sensor/sensor.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 784792d..474d812 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -537,11 +537,14 @@ MAHONY_STATUS mahony_status = mahony_update_imu use_accel ); -debug_assert - ( - mahony_status == MAHONY_OK, - ERROR_SENSOR_CMD_ERROR - ); +if ( mahony_status != MAHONY_OK ) + { + debug_assert + ( + false, + ERROR_SENSOR_CMD_ERROR + ); + } /* * Store the filter's body-to-world quaternion as the system attitude estimate. From d4e0e781217cb16450f6a89b0c6f3b81bfdd448f Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 13 Aug 2026 19:19:31 -0700 Subject: [PATCH 14/18] Sync sensor orientation and filter cleanup --- sensor/sensor.c | 39 +-------------------------------------- sensor/sensor.h | 5 ++--- 2 files changed, 3 insertions(+), 41 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 474d812..dc0b2ef 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -90,7 +90,7 @@ float velo_z_prev = 0.0f; /*------------------------------------------------------------------------------ Static Variables ------------------------------------------------------------------------------*/ -static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ +static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_NORMAL; /* * Persistent attitude filter state. This instance retains the quaternion and @@ -124,13 +124,6 @@ static QUAT quat_grav_attitude QUAT attitude ); -// ETS: Postponed -// static void gravity_comp_filter -// ( -// QUAT* gyro_attitude, -// QUAT g_orientation -// ); - static float quat_to_yaw ( QUAT q @@ -845,36 +838,6 @@ HAL_NVIC_EnableIRQ( GPS_UART_IRQn ); ------------------------------------------------------------------------------*/ -/******************************************************************************* -* * -* PROCEDURE: * -* gravity_comp_filter * -* * -* DESCRIPTION: * -* Fuses integrated gyroscope rotation data with gravity vector to * -* compensate for drift according to the formula * -* attitude = alpha * gyro_attitude + (1 - alpha) * g_orientation * -* * -* NOTE: * -* This type of sensor fusion is only valid when the vehicle is mostly * -* static (e.g. prelaunch). Do not use this during flight when large * -* accerations come from sources other than gravity. * -* * -*******************************************************************************/ -// ETS: Postponed -// static void gravity_comp_filter -// ( -// QUAT* gyro_attitude, -// QUAT g_orientation -// ) -// { -// QUAT comp_gyro = quat_scale(*gyro_attitude, COMP_ALPHA); -// QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); - -// *gyro_attitude = quat_add(comp_gyro, comp_acc); - -// } - /******************************************************************************* * * diff --git a/sensor/sensor.h b/sensor/sensor.h index 9b42fa7..de1b7a4 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -60,7 +60,6 @@ typedef struct _PRESET_DATA PRESET_DATA; /* From main.h */ /* General */ #define NUM_SENSORS ( 38 ) #define SENSOR_DATA_SIZE ( 128 ) -#define COMP_ALPHA ( 0.98f ) /* Used in sensor fusion */ /*------------------------------------------------------------------------------ @@ -90,8 +89,8 @@ typedef enum /* Mount configuration of FC */ typedef enum { - MOUNT_ORIENTATION_IMU_INVERTED = -1, - MOUNT_ORIENTATION_IMU_NORMAL = 1 + MOUNT_ORIENTATION_IMU_INVERTED = -1, /* Antenna pointing up */ + MOUNT_ORIENTATION_IMU_NORMAL = 1 /* Antenna pointing down */ } MOUNT_ORIENTATION; /* State estimation from processed sensors */ From 8189fd0d3f8ea04f8941902f5dd8fc08ae3e2504 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 13 Aug 2026 19:40:21 -0700 Subject: [PATCH 15/18] Propagate Mahony update failures --- sensor/sensor.c | 24 ++++++++++++++---------- sensor/sensor.h | 2 +- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index dc0b2ef..8a8b06a 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -244,6 +244,7 @@ SENSOR_STATUS sensor_dump Local Variables ------------------------------------------------------------------------------*/ SENSOR_STATUS parallel_status; +SENSOR_STATUS body_state_status; IMU_STATUS imu_status; BARO_STATUS baro_status; IMU_RAW imu_raw; @@ -251,9 +252,10 @@ IMU_RAW imu_raw; /*------------------------------------------------------------------------------ Initializations ------------------------------------------------------------------------------*/ -parallel_status = SENSOR_OK; -imu_status = IMU_OK; -baro_status = BARO_OK; +parallel_status = SENSOR_OK; +body_state_status = SENSOR_OK; +imu_status = IMU_OK; +baro_status = BARO_OK; /* Poll Sensors */ @@ -297,7 +299,7 @@ baro_status = get_baro_it( &(sensor_data_ptr->baro_pressure), &(sensor_data_ptr- sensor_conv_imu( &(sensor_data_ptr->imu_converted), &imu_raw ); /* Calculated to get body state */ -sensor_body_state( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) ); +body_state_status = sensor_body_state( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) ); /* Calculated velocity and position */ sensor_imu_velo( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) ); @@ -321,6 +323,10 @@ if( imu_status != IMU_OK ) { return SENSOR_IMU_FAIL; } +else if ( body_state_status != SENSOR_OK ) + { + return body_state_status; + } else if ( baro_status != BARO_OK) { return SENSOR_BARO_ERROR; @@ -464,7 +470,7 @@ mount_orientation = orientation; * * *******************************************************************************/ -void sensor_body_state +SENSOR_STATUS sensor_body_state ( const IMU_CONVERTED* imu_converted, STATE_ESTIMATION* state_estimate @@ -532,11 +538,7 @@ MAHONY_STATUS mahony_status = mahony_update_imu if ( mahony_status != MAHONY_OK ) { - debug_assert - ( - false, - ERROR_SENSOR_CMD_ERROR - ); + return SENSOR_IMU_FAIL; } /* @@ -549,6 +551,8 @@ state_estimate->attitude = mahony_filter.attitude; */ state_estimate->roll_rate = imu_converted->gyro_x; +return SENSOR_OK; + } /* sensor_body_state */ diff --git a/sensor/sensor.h b/sensor/sensor.h index de1b7a4..ceafe57 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -176,7 +176,7 @@ void sensor_reset_velo ); /* Perform sensor fusion on imu converted data to get body rate */ -void sensor_body_state +SENSOR_STATUS sensor_body_state ( const IMU_CONVERTED* imu_converted, STATE_ESTIMATION* state_estimate From e15f5871e60adbcae190f2a6a7584f7e4541526a Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 13 Aug 2026 19:43:59 -0700 Subject: [PATCH 16/18] Make identity quaternion const --- sensor/sensor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 8a8b06a..5aa4077 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -356,7 +356,7 @@ void sensor_init PRESET_DATA* preset_data ) { -QUAT identity = +const QUAT identity = { .w = 1.0f, .x = 0.0f, From 870a43fcd7f524b53da46314010a4f1121fb33df Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 13 Aug 2026 19:49:17 -0700 Subject: [PATCH 17/18] Format Mahony logical operators --- mahony/mahony.c | 42 +++++++++++++++++++++--------------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index 1a7c359..b21b834 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -52,10 +52,10 @@ static bool mahony_quat_is_finite { return ( - isfinite(quaternion.w) && - isfinite(quaternion.x) && - isfinite(quaternion.y) && - isfinite(quaternion.z) + isfinite(quaternion.w) + && isfinite(quaternion.x) + && isfinite(quaternion.y) + && isfinite(quaternion.z) ); } /* mahony_quat_is_finite */ @@ -70,9 +70,9 @@ static bool mahony_vector_is_finite { return ( - isfinite(vector.x) && - isfinite(vector.y) && - isfinite(vector.z) + isfinite(vector.x) + && isfinite(vector.y) + && isfinite(vector.z) ); } /* mahony_vector_is_finite */ @@ -216,14 +216,14 @@ if ( !mahony_quat_is_finite(initial_attitude) ) return MAHONY_INVALID_QUATERNION; } -if ( !isfinite(proportional_gain) || - !isfinite(integral_gain) ) +if ( !isfinite(proportional_gain) + || !isfinite(integral_gain) ) { return MAHONY_NONFINITE_GAIN; } -if ( proportional_gain < 0.0f || - integral_gain < 0.0f ) +if ( proportional_gain < 0.0f + || integral_gain < 0.0f ) { return MAHONY_NEGATIVE_GAIN; } @@ -272,8 +272,8 @@ if ( !mahony_vector_is_finite(gyro_body_rad_s) ) return MAHONY_INVALID_GYRO; } -if ( !isfinite(delta_time_s) || - delta_time_s <= 0.0f ) +if ( !isfinite(delta_time_s) + || delta_time_s <= 0.0f ) { return MAHONY_INVALID_DELTA_TIME; } @@ -351,8 +351,8 @@ if ( !mahony_vector_is_finite(gyro_body_rad_s) ) return MAHONY_INVALID_GYRO; } -if ( !isfinite(delta_time_s) || - delta_time_s <= 0.0f ) +if ( !isfinite(delta_time_s) + || delta_time_s <= 0.0f ) { return MAHONY_INVALID_DELTA_TIME; } @@ -360,14 +360,14 @@ if ( !isfinite(delta_time_s) || accel_magnitude = vector_magnitude(accel_body); accel_valid = - mahony_vector_is_finite(accel_body) && - isfinite(accel_magnitude) && - accel_magnitude >= MAHONY_ACCEL_MIN_MAGNITUDE && - accel_magnitude <= MAHONY_ACCEL_MAX_MAGNITUDE; + mahony_vector_is_finite(accel_body) + && isfinite(accel_magnitude) + && accel_magnitude >= MAHONY_ACCEL_MIN_MAGNITUDE + && accel_magnitude <= MAHONY_ACCEL_MAX_MAGNITUDE; apply_accel = - use_accel && - accel_valid; + use_accel + && accel_valid; gyro_corrected = gyro_body_rad_s; From d2924528699ec196dba2b60f9dd68e82248d3d2f Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 13 Aug 2026 19:53:55 -0700 Subject: [PATCH 18/18] Document Mahony helper functions --- mahony/mahony.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/mahony/mahony.c b/mahony/mahony.c index b21b834..3ba3557 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -77,6 +77,9 @@ return } /* mahony_vector_is_finite */ +/** + * @brief Calculates the magnitude of a three-dimensional vector. + */ static float vector_magnitude ( VECTOR_3F vector @@ -91,6 +94,9 @@ return sqrtf } /* vector_magnitude */ +/** + * @brief Normalizes a three-dimensional vector in place. + */ static bool vector_normalize ( VECTOR_3F *vector @@ -123,6 +129,9 @@ return true; } /* vector_normalize */ +/** + * @brief Clamps a floating-point value between minimum and maximum bounds. + */ static float clamp_float ( float value, @@ -144,6 +153,9 @@ return value; } /* clamp_float */ +/** + * @brief Calculates the cross product of two three-dimensional vectors. + */ static VECTOR_3F vector_cross ( VECTOR_3F a, @@ -161,6 +173,9 @@ return result; } /* vector_cross */ +/** + * @brief Adds two three-dimensional vectors. + */ static VECTOR_3F vector_add ( VECTOR_3F a, @@ -178,6 +193,9 @@ return result; } /* vector_add */ +/** + * @brief Scales a three-dimensional vector by a scalar. + */ static VECTOR_3F vector_scale ( VECTOR_3F vector,