diff --git a/_test/math_sdr/main.h b/_test/math_sdr/main.h deleted file mode 100644 index 5fa2343..0000000 --- a/_test/math_sdr/main.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef TEST_MATH_SDR_MAIN_H -#define TEST_MATH_SDR_MAIN_H - -#include -#include -#include - -#endif /* TEST_MATH_SDR_MAIN_H */ diff --git a/_test/math_sdr/test_math_sdr.c b/_test/math_sdr/test_math_sdr.c deleted file mode 100644 index 75e58e6..0000000 --- a/_test/math_sdr/test_math_sdr.c +++ /dev/null @@ -1,286 +0,0 @@ -/******************************************************************************* - * - * FILE: - * test_math_sdr.c - * - * DESCRIPTION: - * Unit tests for quaternion vector rotations in math_sdr. - * - ******************************************************************************/ - -/*------------------------------------------------------------------------------ - Standard Includes - ------------------------------------------------------------------------------*/ -#include -#include -#include - -#define TEST_PI 3.14159265358979323846f - -/*------------------------------------------------------------------------------ - Project Includes - ------------------------------------------------------------------------------*/ -#include "math_sdr.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 */ - -/*------------------------------------------------------------------------------ - Tests - ------------------------------------------------------------------------------*/ - -void test_world_to_body_identity - ( - void - ) -{ -QUAT attitude = - { - .w = 1.0f, - .x = 0.0f, - .y = 0.0f, - .z = 0.0f - }; - -QUAT vector_world = - { - .w = 0.0f, - .x = 1.0f, - .y = 2.0f, - .z = 3.0f - }; - -QUAT actual = quat_rotate_world_to_body(attitude, vector_world); - -assert_quat_components - ( - "Identity world-to-body rotation", - actual, - vector_world - ); - -} /* test_world_to_body_identity */ - -void test_body_to_world_identity - ( - void - ) -{ -QUAT attitude = - { - .w = 1.0f, - .x = 0.0f, - .y = 0.0f, - .z = 0.0f - }; - -QUAT vector_body = - { - .w = 0.0f, - .x = -2.0f, - .y = 4.0f, - .z = GRAVITY - }; - -QUAT actual = quat_rotate_body_to_world(attitude, vector_body); - -assert_quat_components - ( - "Identity body-to-world rotation", - actual, - vector_body - ); - -} /* test_body_to_world_identity */ - -void test_body_world_round_trip - ( - void - ) -{ -const float half_angle = 0.25f * TEST_PI; - -QUAT attitude = - { - .w = cosf(half_angle), - .x = 0.0f, - .y = sinf(half_angle), - .z = 0.0f - }; - -QUAT vector_body = - { - .w = 0.0f, - .x = 1.0f, - .y = -2.0f, - .z = 3.0f - }; - -QUAT vector_world = quat_rotate_body_to_world - ( - attitude, - vector_body - ); - -QUAT recovered_body = quat_rotate_world_to_body - ( - attitude, - vector_world - ); - -assert_quat_components - ( - "Body-to-world-to-body round trip", - recovered_body, - vector_body - ); - -} /* test_body_world_round_trip */ - -void test_positive_90_degree_yaw - ( - void - ) -{ -const float half_angle = 0.25f * TEST_PI; - -QUAT attitude = - { - .w = cosf(half_angle), - .x = 0.0f, - .y = 0.0f, - .z = sinf(half_angle) - }; - -QUAT vector_body = - { - .w = 0.0f, - .x = 1.0f, - .y = 0.0f, - .z = 0.0f - }; - -QUAT expected_world = - { - .w = 0.0f, - .x = 0.0f, - .y = 1.0f, - .z = 0.0f - }; - -QUAT actual = quat_rotate_body_to_world(attitude, vector_body); - -assert_quat_components - ( - "Positive 90 degree yaw rotates body +X to world +Y", - actual, - expected_world - ); - -} /* test_positive_90_degree_yaw */ - -void test_stationary_identity_gravity - ( - void - ) -{ -QUAT attitude = - { - .w = 1.0f, - .x = 0.0f, - .y = 0.0f, - .z = 0.0f - }; - -QUAT gravity_world = - { - .w = 0.0f, - .x = 0.0f, - .y = 0.0f, - .z = GRAVITY - }; - -QUAT measured_accel_body = - { - .w = 0.0f, - .x = 0.0f, - .y = 0.0f, - .z = GRAVITY - }; - -QUAT gravity_body = quat_rotate_world_to_body - ( - attitude, - gravity_world - ); - -QUAT linear_accel_body = - { - .w = 0.0f, - .x = measured_accel_body.x - gravity_body.x, - .y = measured_accel_body.y - gravity_body.y, - .z = measured_accel_body.z - gravity_body.z - }; - -QUAT expected = - { - .w = 0.0f, - .x = 0.0f, - .y = 0.0f, - .z = 0.0f - }; - -assert_quat_components - ( - "Stationary identity attitude removes gravity", - linear_accel_body, - expected - ); - -} /* test_stationary_identity_gravity */ - -/*------------------------------------------------------------------------------ - Main - ------------------------------------------------------------------------------*/ - -int main - ( - void - ) -{ -unit_test tests[] = - { - { "world_to_body_identity", test_world_to_body_identity }, - { "body_to_world_identity", test_body_to_world_identity }, - { "body_world_round_trip", test_body_world_round_trip }, - { "positive_90_degree_yaw", test_positive_90_degree_yaw }, - { "stationary_identity_gravity", test_stationary_identity_gravity } - }; - -TEST_INITIALIZE_TEST("math_sdr.c", tests); - -} /* main */ - -/******************************************************************************* - * END OF FILE - ******************************************************************************/ diff --git a/_test/math_sdr/.gitignore b/_test/mod/mahony/.gitignore similarity index 100% rename from _test/math_sdr/.gitignore rename to _test/mod/mahony/.gitignore diff --git a/_test/math_sdr/Makefile b/_test/mod/mahony/Makefile similarity index 86% rename from _test/math_sdr/Makefile rename to _test/mod/mahony/Makefile index af1f5b5..5accf47 100644 --- a/_test/math_sdr/Makefile +++ b/_test/mod/mahony/Makefile @@ -7,7 +7,7 @@ ################################################################ # target ################################################################ -TARGET = math_sdr +TARGET = mahony ################################################################ # build variables @@ -18,21 +18,24 @@ OPT = -Og ################################################################ # paths ################################################################ + BUILD_DIR = build -ROOT_DIR = ../.. +ROOT_DIR = ../../../.. +FRAMEWORK_DIR = ../../framework ################################################################ # source ################################################################ TEST_SOURCES = \ -test_math_sdr.c +test_mahony.c COV_C_SOURCES = \ -$(ROOT_DIR)/math_sdr/math_sdr.c +$(ROOT_DIR)/mod/mahony/mahony.c \ +$(ROOT_DIR)/mod/math_sdr/math_sdr.c FRAMEWORK_SOURCES = \ -$(ROOT_DIR)/_test/framework/src/test_assert.c \ -$(ROOT_DIR)/_test/framework/src/test_runner.c +$(FRAMEWORK_DIR)/src/test_assert.c \ +$(FRAMEWORK_DIR)/src/test_runner.c C_SOURCES = $(TEST_SOURCES) $(COV_C_SOURCES) $(FRAMEWORK_SOURCES) @@ -46,8 +49,9 @@ CC = gcc ################################################################ C_INCLUDES = \ -I. \ --I$(ROOT_DIR)/math_sdr \ --I$(ROOT_DIR)/_test/framework/src +-I$(ROOT_DIR)/mod/mahony \ +-I$(ROOT_DIR)/mod/math_sdr \ +-I$(FRAMEWORK_DIR)/src C_DEFS = \ -DUNIT_TEST @@ -92,7 +96,8 @@ test: tail -n 7 "results.txt" mkdir -p coverage gcovr $(BUILD_DIR) \ - --filter $(COV_C_SOURCES) \ + --filter '.*(? +#include +#include + +#endif /* TEST_MAHONY_MAIN_H */ \ No newline at end of file diff --git a/_test/mod/mahony/test_mahony.c b/_test/mod/mahony/test_mahony.c new file mode 100644 index 0000000..4534785 --- /dev/null +++ b/_test/mod/mahony/test_mahony.c @@ -0,0 +1,2693 @@ +/******************************************************************************* + * + * 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_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); + +} /* assert_quat_components */ + + +/** + * @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 + ------------------------------------------------------------------------------*/ + + /** + * @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 + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +assert_quat_components + ( + "Identity attitude remains identity", + filter.attitude, + identity + ); + +} /* 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 + ) +{ +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +assert_quat_components + ( + "Initial attitude is normalized", + filter.attitude, + expected + ); + +} /* 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 + ) +{ +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + zero_quaternion, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +assert_quat_components + ( + "Zero quaternion falls back to identity", + filter.attitude, + expected + ); + +} /* 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 + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.1f + ), + MAHONY_OK + ); + +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 */ + +/** + * @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 + ) +{ +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_EQ_UINT + ( + "Null filter is rejected", + mahony_init + ( + NULL, + identity, + 1.0f, + 0.0f + ), + MAHONY_NULL_POINTER + ); + +} /* 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 + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_EQ_UINT + ( + "Negative proportional gain is rejected", + mahony_init + ( + &filter, + identity, + -1.0f, + 0.0f + ), + MAHONY_NEGATIVE_GAIN + ); + +TEST_ASSERT_EQ_UINT + ( + "Negative integral gain is rejected", + mahony_init + ( + &filter, + identity, + 1.0f, + -0.1f + ), + MAHONY_NEGATIVE_GAIN + ); + +} /* test_mahony_init_rejects_negative_gain */ + + +/*------------------------------------------------------------------------------ + 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 + ) +{ +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Zero-rate update succeeds", + mahony_update_gyro + ( + &filter, + zero_rate, + 0.01f + ), + MAHONY_OK + ); + +assert_quat_components + ( + "Zero angular velocity preserves identity", + filter.attitude, + identity + ); + +} /* 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 + ) +{ +int32_t index; + +MAHONY_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + status |= mahony_update_gyro + ( + &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", + 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 */ + +/** + * @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 + ) +{ +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Zero delta time is rejected", + mahony_update_gyro + ( + &filter, + zero_rate, + 0.0f + ), + MAHONY_INVALID_DELTA_TIME + ); + +TEST_ASSERT_EQ_UINT + ( + "Negative delta time is rejected", + mahony_update_gyro + ( + &filter, + zero_rate, + -0.01f + ), + MAHONY_INVALID_DELTA_TIME + ); + +} /* 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_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + 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", + 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_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 2000; index++ ) + { + 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, + 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_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 2000; index++ ) + { + 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, + 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_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 2000; index++ ) + { + 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, + 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_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + +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_EQ_UINT + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + gyro_status |= mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ); + + 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", + 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_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + +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_EQ_UINT + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + gyro_status |= mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ); + + 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", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* 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; + +MAHONY_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Gyro diagnostic filter initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ), + MAHONY_OK + ); + +print_attitude_heading + ( + "GYROSCOPE-ONLY ATTITUDE PROPAGATION" + ); + +print_attitude_sample + ( + 0, + 0.0f, + filter.attitude + ); + +for ( interval = 1; interval <= interval_count; interval++ ) + { + status |= mahony_update_gyro + ( + &filter, + gyro_body_rad_s, + delta_time_s + ); + + print_attitude_sample + ( + interval, + interval * delta_time_s, + filter.attitude + ); + } + +TEST_ASSERT_EQ_UINT + ( + "Gyro diagnostic propagation succeeds", + status, + MAHONY_OK + ); + +/* + * 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; + +MAHONY_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Accelerometer diagnostic filter initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.5f, + 0.0f + ), + MAHONY_OK + ); + +print_attitude_heading + ( + "MAHONY PROPORTIONAL ACCELEROMETER CORRECTION" + ); + +print_attitude_sample + ( + 0, + 0.0f, + filter.attitude + ); + +for ( interval = 1; interval <= interval_count; interval++ ) + { + status |= mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + delta_time_s, + true + ); + + print_attitude_sample + ( + interval, + interval * delta_time_s, + filter.attitude + ); + } + +TEST_ASSERT_EQ_UINT + ( + "Accelerometer diagnostic updates succeed", + status, + MAHONY_OK + ); + +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 */ + +/** + * @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_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + +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_EQ_UINT + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + gyro_status |= mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ); + + 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", + 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_EQ_UINT + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.01f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Invalid-accelerometer IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + invalid_accel, + 0.01f, + true + ), + MAHONY_OK + ); + +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_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + 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, + 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_STATUS gyro_status = MAHONY_OK; +MAHONY_STATUS imu_status = MAHONY_OK; + +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_EQ_UINT + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + gyro_status |= mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ); + + 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", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* 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; + +MAHONY_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + 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", + 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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 0.0f, + 0.5f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Integral update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.1f, + true + ), + MAHONY_OK + ); + +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; + +MAHONY_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.5f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + 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", + 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_STATUS status = MAHONY_OK; + +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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.5f + ), + MAHONY_OK + ); + +for ( index = 0; index < 1000; index++ ) + { + 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", + 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_EQ_UINT + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 0.0f, + 100.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "High-integral-gain update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 1.0f, + true + ), + MAHONY_OK + ); + +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_EQ_UINT + ( + "No-integral filter initialization succeeds", + mahony_init + ( + &no_integral_filter, + initial_attitude, + 0.0f, + 0.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Integral filter initialization succeeds", + mahony_init + ( + &integral_filter, + initial_attitude, + 0.0f, + 1.0f + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "No-integral update succeeds", + mahony_update_imu + ( + &no_integral_filter, + zero_gyro, + valid_accel, + 0.1f, + true + ), + MAHONY_OK + ); + +TEST_ASSERT_EQ_UINT + ( + "Integral update succeeds", + mahony_update_imu + ( + &integral_filter, + zero_gyro, + valid_accel, + 0.1f, + true + ), + MAHONY_OK + ); + +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 + ------------------------------------------------------------------------------*/ + +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 }, + { "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 }, + { "mahony_print_gyro_propagation", test_mahony_print_gyro_propagation }, + { "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 }, + { "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); + +} /* main */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file diff --git a/_test/mod/sensor/Makefile b/_test/mod/sensor/Makefile index 3c82cf0..43f5bb4 100644 --- a/_test/mod/sensor/Makefile +++ b/_test/mod/sensor/Makefile @@ -58,6 +58,8 @@ C_INCLUDES = \ -I$(ROOT_DIR)/mod/sensor \ -I$(ROOT_DIR)/mod/math_sdr \ -I$(ROOT_DIR)/mod/error_sdr \ +-I$(ROOT_DIR)/mod/debug_sdr \ +-I$(ROOT_DIR)/mod/mahony \ -I$(ROOT_DIR)/driver/imu \ -I$(ROOT_DIR)/driver/baro \ -I$(ROOT_DIR)/driver/gps \ diff --git a/_test/run_tests.sh b/_test/run_tests.sh index 874863e..3f05c6c 100644 --- a/_test/run_tests.sh +++ b/_test/run_tests.sh @@ -9,6 +9,7 @@ tests=( debug_sdr error_sdr math_sdr + mahony sensor telemetry ) diff --git a/mahony/mahony.c b/mahony/mahony.c new file mode 100644 index 0000000..5d01395 --- /dev/null +++ b/mahony/mahony.c @@ -0,0 +1,307 @@ +/** + ****************************************************************************** + * @file : mahony.c + * @brief : Mahony attitude filter interface. + ****************************************************************************** + * @copyright + * + * Copyright (c) 2025 Sun Devil Rocketry. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is covered under the + * BSD-3-Clause. + * + * https://opensource.org/license/bsd-3-clause + * + ****************************************************************************** + @verbatim + ============================================================================== + ##### Integration Guide ##### + ============================================================================== + [..] + (+) Initialize the filter with a pointer to an existing or uninitialized filter, + an initial attiutude estimate, and the proportional and integral gains for + accelerometer correction. + (+) Call updates to the filter with gyroscope and accelerometer vectors and + time elapsed. Optionally disable accelerometer fusion during dynamic + flight. + ****************************************************************************** + @endverbatim + */ + +/* Standard Includes ---------------------------------------------------------*/ +#include +#include + +/* Project Includes ----------------------------------------------------------*/ +#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) + +/* + * 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 + +/* Public Functions ----------------------------------------------------------*/ + +/** + * @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 MAHONY_OK when initialization succeeds; otherwise a Mahony status + * code describing the failure. + */ +MAHONY_STATUS mahony_init + ( + MAHONY_FILTER *filter, + QUAT initial_attitude, + float proportional_gain, + float integral_gain + ) +{ +if ( filter == NULL ) + { + return MAHONY_NULL_POINTER; + } + +if ( !quat_is_finite(initial_attitude) ) + { + return MAHONY_INVALID_QUATERNION; + } + +if ( !isfinite(proportional_gain) || !isfinite(integral_gain) ) + { + return MAHONY_NONFINITE_GAIN; + } + +if ( proportional_gain < 0.0f || integral_gain < 0.0f ) + { + 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 MAHONY_OK; +} /* mahony_init */ + + +/** + * @brief Propagates attitude using body-frame gyroscope measurements. + * + * 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. + * + * @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 MAHONY_OK when initialization succeeds; otherwise a Mahony status + code describing the failure. + */ +MAHONY_STATUS 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 MAHONY_NULL_POINTER; + } + +if ( !quat_is_finite(filter->attitude) ) + { + return MAHONY_INVALID_QUATERNION; + } + +if ( !vector_is_finite(gyro_body_rad_s) ) + { + return MAHONY_INVALID_GYRO; + } + +if ( !isfinite(delta_time_s) || delta_time_s <= 0.0f ) + { + return MAHONY_INVALID_DELTA_TIME; + } + +/* + * The attitude quaternion is a body-to-world rotation and angular velocity + * is expressed in the body frame: + */ +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; + +/* q_dot = 0.5 * q * omega_body */ +attitude_derivative = quat_mult(filter->attitude, angular_velocity); +attitude_derivative = quat_scale(attitude_derivative, 0.5f); + +/* Scale q_dot by delta_t, add to attitude, and normalize*/ +attitude_delta = quat_scale(attitude_derivative, delta_time_s); +filter->attitude = quat_add(filter->attitude, attitude_delta); +filter->attitude = quat_normalize(filter->attitude); + +return MAHONY_OK; + +} /* mahony_update_gyro */ + + +/** + * @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. + * + * 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. + * @param delta_time_s Elapsed time in seconds. + * @param use_accel Whether accelerometer feedback should be applied. + * + * @return MAHONY_OK when the attitude was updated; otherwise a Mahony status + code describing the failure. + */ +MAHONY_STATUS 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; + +float accel_magnitude; + +bool accel_valid; +bool apply_accel; + +if ( filter == NULL ) + { + return MAHONY_NULL_POINTER; + } + +if ( !vector_is_finite(gyro_body_rad_s) ) + { + return MAHONY_INVALID_GYRO; + } + +if ( !isfinite(delta_time_s) || delta_time_s <= 0.0f ) + { + return MAHONY_INVALID_DELTA_TIME; + } + +accel_magnitude = vector_magnitude(accel_body); + +accel_valid = 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 ( apply_accel && 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); + + /* + * 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; + /* The filter has no yaw reference, so avoid accumulating drift on the z axis */ + /* filter->integral_error.z += filter->integral_gain * attitude_error.z * delta_time_s; */ + + filter->integral_error.x = clamp_float(filter->integral_error.x, -MAHONY_INTEGRAL_LIMIT_RAD_S, MAHONY_INTEGRAL_LIMIT_RAD_S); + filter->integral_error.y = clamp_float(filter->integral_error.y, -MAHONY_INTEGRAL_LIMIT_RAD_S, MAHONY_INTEGRAL_LIMIT_RAD_S); + /* filter->integral_error.z = clamp_float(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(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 new file mode 100644 index 0000000..2dfa312 --- /dev/null +++ b/mahony/mahony.h @@ -0,0 +1,112 @@ +/** + ****************************************************************************** + * @file : mahony.h + * @brief : Mahony attitude filter interface. + ****************************************************************************** + * @copyright + * + * Copyright (c) 2025 Sun Devil Rocketry. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is covered under the + * BSD-3-Clause. + * + * https://opensource.org/license/bsd-3-clause + * + ****************************************************************************** + @verbatim + ============================================================================== + ##### Mahony filter features ##### + ============================================================================== + [..] + (+) Quaternion-based gyroscope attitude propogation + (+) Accelerometer proportional and integral feedback filtering + ****************************************************************************** + @endverbatim + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef MAHONY_H +#define MAHONY_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +/* Standard Includes ---------------------------------------------------------*/ +#include + +/* Project Includes ----------------------------------------------------------*/ +#include "math_sdr.h" + +/* Typedefs ------------------------------------------------------------------*/ + +/** + * @brief Mahony attitude filter status codes. + */ +typedef enum + { + MAHONY_OK = 0, + MAHONY_NULL_POINTER, + MAHONY_INVALID_QUATERNION, + MAHONY_NONFINITE_GAIN, + MAHONY_NEGATIVE_GAIN, + MAHONY_INVALID_GYRO, + MAHONY_INVALID_DELTA_TIME + } MAHONY_STATUS; + +/** + * @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 -------------------------------------------------------*/ + +MAHONY_STATUS mahony_init + ( + MAHONY_FILTER *filter, + QUAT initial_attitude, + float proportional_gain, + float integral_gain + ); + + +MAHONY_STATUS mahony_update_gyro + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + float delta_time_s + ); + +MAHONY_STATUS 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 + +#endif /* MAHONY_H */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 17642e7..422eb50 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -18,20 +18,14 @@ ****************************************************************************** */ -/*------------------------------------------------------------------------------ - Standard Includes -------------------------------------------------------------------------------*/ +/* Standard Includes ---------------------------------------------------------*/ #include -/*------------------------------------------------------------------------------ - Project Includes -------------------------------------------------------------------------------*/ +/* Project Includes ----------------------------------------------------------*/ #include "main.h" #include "math_sdr.h" -/*------------------------------------------------------------------------------ - API Functions -------------------------------------------------------------------------------*/ +/* API Functions -------------------------------------------------------------*/ /** * @brief Computes a CRC-32 checksum over a block of data. @@ -58,6 +52,74 @@ return ~crc; } /* crc32 */ +/** + * @brief Clamps a floating-point value between minimum and maximum bounds. + * + * @param value Value to clamp. + * @param minimum Minimum value. + * @param maximum Maximum value. + */ +float clamp_float + ( + float value, + float minimum, + float maximum + ) +{ +if ( value < minimum ) + { + return minimum; + } + +if ( value > maximum ) + { + return maximum; + } + +return value; + +} /* clamp_float */ + +/* Quaternions ---------------------------------------------------------------*/ + +/** + * @brief Converts ZYX Euler angles to a quaternion. + * + * @note Angles must be provided in radians. + * @note The rotation order is ZYX (standard aerospace convention). + * @todo The rotation sequence has not been validated yet. + * + * @param yaw Rotation about the Z axis in radians. + * @param pitch Rotation about the Y axis in radians. + * @param roll Rotation about the X axis in radians. + * + * @return The quaternion representing the rotation. + */ +QUAT eul_to_quat + ( + float yaw, + float pitch, + float roll + ) +{ +float cos_yaw = cosf(yaw / 2.0f); +float cos_pitch = cosf(pitch / 2.0f); +float cos_roll = cosf(roll / 2.0f); + +float sin_yaw = sinf(yaw / 2.0f); +float sin_pitch = sinf(pitch / 2.0f); +float sin_roll = sinf(roll / 2.0f); + +QUAT q; +q.w = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw; +q.x = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw; +q.y = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw; +q.z = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw; + +return q; + +} /* eul_to_quat */ + /** * @brief Hamilton product of two quaternions. @@ -280,6 +342,155 @@ return quat_mult } /* quat_rotate_body_to_world */ +/** + * @brief Determines whether every quaternion component is finite. + */ +bool quat_is_finite + ( + QUAT quaternion + ) +{ +return + ( + isfinite(quaternion.w) + && isfinite(quaternion.x) + && isfinite(quaternion.y) + && isfinite(quaternion.z) + ); + +} /* quat_is_finite */ + +/* Vectors -------------------------------------------------------------------*/ + +/** + * @brief Determines whether every vector component is finite. + */ +bool vector_is_finite + ( + VECTOR_3F vector + ) +{ +return + ( + isfinite(vector.x) + && isfinite(vector.y) + && isfinite(vector.z) + ); + +} /* vector_is_finite */ + +/** + * @brief Calculates the magnitude of a three-dimensional vector. + */ +float vector_magnitude + ( + VECTOR_3F vector + ) +{ +return sqrtf + ( + vector.x * vector.x + + vector.y * vector.y + + vector.z * vector.z + ); + +} /* vector_magnitude */ + +/** + * @brief Normalizes a three-dimensional vector in place. + */ +bool vector_normalize + ( + VECTOR_3F *vector + ) +{ +float magnitude; + +if ( vector == NULL ) + { + return false; + } + +if ( !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 */ + + +/** + * @brief Calculates the cross product of two three-dimensional vectors. + */ +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 */ + + +/** + * @brief Adds two three-dimensional vectors. + */ +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 */ + + +/** + * @brief Scales a three-dimensional vector by a scalar. + */ +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 */ + /******************************************************************************* * END OF FILE * -*******************************************************************************/ +*******************************************************************************/ \ No newline at end of file diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 3e467c7..b8b2e02 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -38,16 +38,14 @@ extern "C" { #endif -/*------------------------------------------------------------------------------ - Includes -------------------------------------------------------------------------------*/ -#include +/* Includes ------------------------------------------------------------------*/ #include +#include +#include +#include -/*------------------------------------------------------------------------------ - Typedefs -------------------------------------------------------------------------------*/ +/* Typedefs ------------------------------------------------------------------*/ /* Quaternion */ typedef struct _QUAT @@ -55,13 +53,21 @@ typedef struct _QUAT float w, x, y, z; } QUAT; +/** + * @brief Three-dimensional floating-point vector. + */ +typedef struct _VECTOR_3F + { + float x; + float y; + float z; + } VECTOR_3F; -/*------------------------------------------------------------------------------ - Macros -------------------------------------------------------------------------------*/ +/* Macros --------------------------------------------------------------------*/ /* Constants */ #define GRAVITY 9.8f +#define IDENTITY_QUAT ((QUAT) {1.0f, 0.0f, 0.0f, 0.0f }) /** @@ -107,9 +113,7 @@ typedef struct _QUAT */ #define array_size( array ) ( sizeof( array ) / sizeof( array[0] ) ) -/*------------------------------------------------------------------------------ - Function Prototypes -------------------------------------------------------------------------------*/ +/* Function Prototypes -------------------------------------------------------*/ uint32_t crc32 ( @@ -117,6 +121,22 @@ uint32_t crc32 size_t len ); +float clamp_float + ( + float value, + float minimum, + float maximum + ); + +/* Quaternions ---------------------------------------------------------------*/ + +QUAT eul_to_quat + ( + float yaw, + float pitch, + float roll + ); + QUAT quat_mult ( QUAT a, @@ -151,34 +171,58 @@ QUAT quat_conj QUAT q ); -/** - * @brief Rotates a vector from the world frame into the body frame. - * - * @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. - */ QUAT quat_rotate_world_to_body ( QUAT attitude, QUAT vector_world ); -/** - * @brief Rotates a vector from the body frame into the world frame. - * - * @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. - */ QUAT quat_rotate_body_to_world ( QUAT attitude, QUAT vector_body ); +bool quat_is_finite + ( + QUAT quaternion + ); + +/* Vectors -------------------------------------------------------------------*/ + +bool vector_is_finite + ( + VECTOR_3F vector + ); + +float vector_magnitude + ( + VECTOR_3F vector + ); + +bool vector_normalize + ( + VECTOR_3F *vector + ); + +VECTOR_3F vector_cross + ( + VECTOR_3F a, + VECTOR_3F b + ); + +VECTOR_3F vector_add + ( + VECTOR_3F a, + VECTOR_3F b + ); + +VECTOR_3F vector_scale + ( + VECTOR_3F vector, + float scalar + ); + #ifdef __cplusplus } #endif diff --git a/sensor/sensor.c b/sensor/sensor.c index d2ed0c4..8515f1e 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -43,6 +43,35 @@ #include "usb.h" #include "sensor.h" #include "math_sdr.h" +#include "mahony.h" +#include "error_sdr.h" +#include "debug_sdr.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. + * + * + * 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 + +/* Barometer EMA Alphas */ +#define BARO_PRESS_ALPHA (0.7f) +#define BARO_TEMP_ALPHA (0.7f) /*------------------------------------------------------------------------------ @@ -58,25 +87,29 @@ uint64_t imu_velo_tick = 0; float velo_x_prev = 0.0f; float velo_y_prev = 0.0f; float velo_z_prev = 0.0f; -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 }; -/* Barometer EMA Alphas */ -#define BARO_PRESS_ALPHA (0.7f) -#define BARO_TEMP_ALPHA (0.7f) - /*------------------------------------------------------------------------------ Static Variables ------------------------------------------------------------------------------*/ static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_NORMAL; + +/* + * 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; + static float ema_press_prev = 0.0f; static float ema_temp_prev = 0.0f; + /*------------------------------------------------------------------------------ Internal function prototypes ------------------------------------------------------------------------------*/ @@ -92,6 +125,19 @@ static void sensor_conv_mag IMU_RAW* imu_raw ); +static QUAT quat_grav_attitude + ( + float ax, + float ay, + float az, + QUAT attitude + ); + +static float quat_to_yaw + ( + QUAT q + ); + static void sensor_baro_ema ( SENSOR_DATA* sen_data_ptr @@ -102,8 +148,6 @@ static void sensor_baro_ema API Functions ------------------------------------------------------------------------------*/ - - /** * @brief Executes a sensor subcommand and transmits the requested readings. * @param subcommand Sensor subcommand code. @@ -206,6 +250,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; @@ -213,9 +258,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 */ @@ -259,7 +305,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) ); @@ -286,6 +332,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; @@ -301,24 +351,44 @@ else } /* sensor_dump */ - /** * @brief Initializes sensor timing and resets velocity state. * @param preset_data Pointer to the preset calibration data. */ void sensor_init - ( - PRESET_DATA* preset_data - ) + ( + PRESET_DATA* preset_data + ) { +float ax = preset_data->imu_offset.accel_x; +float ay = preset_data->imu_offset.accel_y; +float az = preset_data->imu_offset.accel_z; + +QUAT initial_attitude = quat_grav_attitude + ( + ax, + ay, + az, + IDENTITY_QUAT + ); + imu_velo_tick = get_us_tick(); +mahony_tick = imu_velo_tick; sensor_reset_velo(); -// NA: deferred to Mahony filter -// float ax = preset_data->imu_offset.accel_x; -// float ay = preset_data->imu_offset.accel_y; -// float az = preset_data->imu_offset.accel_z; +MAHONY_STATUS mahony_status = mahony_init + ( + &mahony_filter, + initial_attitude, + SENSOR_MAHONY_KP, + SENSOR_MAHONY_KI + ); + +if ( mahony_status != MAHONY_OK ) + { + error_fail_fast( ERROR_SENSOR_CMD_ERROR ); + } } /* sensor_init */ @@ -370,7 +440,7 @@ sensor_conv_mag(imu_converted, imu_raw); * * @return The configured mount orientation */ -MOUNT_ORIENTATION get_mount_orientation +MOUNT_ORIENTATION sensor_get_mount_orientation ( void ) @@ -385,7 +455,7 @@ return mount_orientation; * * @param orientation The new mount orientation */ -void set_mount_orientation +void sensor_set_mount_orientation ( MOUNT_ORIENTATION orientation ) @@ -395,65 +465,90 @@ mount_orientation = orientation; } /* set_mount_orientation */ - -static uint32_t last_tick = 0; /** * @brief Integrates gyro data to update the estimated body attitude and rate. * @param imu_converted Converted IMU data. * @param state_estimate State estimate to update. */ -void sensor_body_state - ( - const IMU_CONVERTED* imu_converted, - STATE_ESTIMATION* state_estimate - ) +SENSOR_STATUS sensor_body_state + ( + 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; +uint64_t current_tick; +uint64_t imu_tdelta; -/* Copy IMU data for readability */ -// float ax = imu_converted->accel_x; -// float ay = imu_converted->accel_y; -// float az = imu_converted->accel_z; +float delta_time_s; -/* Raw gyro data in deg/s */ -float gx = imu_converted->gyro_x; -float gy = imu_converted->gyro_y; -float gz = imu_converted->gyro_z; +bool use_accel; -/* 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); +VECTOR_3F gyro_body_rad_s; +VECTOR_3F accel_body_m_s2; -/* q_rate = 0.5 * attitude * q_gyro */ -QUAT q_rate = quat_mult(attitude, q_gyro); -q_rate = quat_scale(q_rate, 0.5f); +/* Calculate elapsed time between attitude updates using the microsecond timer. */ +current_tick = get_us_tick(); +imu_tdelta = current_tick - mahony_tick; -/* Dead reckoing orientation by integrating gyro */ -/* attitude += dt * q_rate */ -QUAT rate_dt = quat_scale(q_rate, dt); -attitude = quat_add(attitude, rate_dt); +delta_time_s = (float)imu_tdelta / (float)MICROSEC_PER_SEC; -/* Sensor fuson with gravity if not in flight */ -// NA: sensor fusion deferred to Mahony filter +if ( mahony_tick == 0 + || delta_time_s <= 0.0f + || delta_time_s > 1.0f ) + { + delta_time_s = 0.01f; + } -/* Scale back to unit quaternion to avoid drift */ -attitude = quat_normalize(attitude); +mahony_tick = current_tick; -/* Store results */ -state_estimate->attitude = attitude; -state_estimate->roll_rate = gx; /* Rate in deg/s */ +/* + * 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; + +MAHONY_STATUS mahony_status = mahony_update_imu + ( + &mahony_filter, + gyro_body_rad_s, + accel_body_m_s2, + delta_time_s, + use_accel + ); + +if ( mahony_status != MAHONY_OK ) + { + return SENSOR_IMU_FAIL; + } + +/* + * 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; + +return SENSOR_OK; + +} /* sensor_body_state */ @@ -470,9 +565,9 @@ void sensor_axis_remap float* z ) { - *x *= mount_orientation; +*x *= mount_orientation; (void)y; - *z *= mount_orientation; +*z *= mount_orientation; } @@ -534,7 +629,7 @@ const QUAT gravity_world = .w = 0.0f, .x = 0.0f, .y = 0.0f, - .z = GRAVITY + .z = -GRAVITY }; /* @@ -574,7 +669,6 @@ float ts_delta; uint64_t current_tick = get_us_tick(); uint64_t imu_tdelta = current_tick - imu_velo_tick; ts_delta = (float) imu_tdelta / (float) MICROSEC_PER_SEC; -ts_delta = (float) imu_tdelta / (float) MICROSEC_PER_SEC; // Calculate 3 velocity vectors using motion equations velo_x = velo_x_prev + accel_world_x * ts_delta; @@ -629,7 +723,6 @@ void sensor_baro_alt(SENSOR_DATA* sen_data) } - /** * @brief Resets accumulated velocity values to prevent drift. */ @@ -706,6 +799,49 @@ HAL_NVIC_EnableIRQ( GPS_UART_IRQn ); Internal procedures ------------------------------------------------------------------------------*/ + + +/******************************************************************************* +* * +* PROCEDURE: * +* quat_grav_attitude * +* * +* DESCRIPTION: * +* Computes quaternion attitude from static accelerometer data * +* Experiences gimbal lock at pitch = +/- 90 degrees * +* * +*******************************************************************************/ +static QUAT quat_grav_attitude + ( + float ax, + float ay, + float az, + QUAT attitude + ) +{ +/* Compute pitch/roll from accelerometer */ +float grav_pitch = atan2f(ax, sqrtf(ay * ay + az * az)); +float grav_roll = atan2f(ay, az); + +float yaw = quat_to_yaw(attitude); + +return eul_to_quat(yaw, grav_pitch, grav_roll); + +} + +/* Comment deferred until mod#132 + Formula in https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles */ +static float quat_to_yaw + ( + QUAT q + ) +{ +float y = 2.0f * (q.w * q.z + q.x * q.y); +float x = 1.0f - 2.0f * (q.y * q.y + q.z * q.z); + +return atan2f(y, x); +} + #ifdef A0002_REV2 /** diff --git a/sensor/sensor.h b/sensor/sensor.h index 647ca87..e0f8efc 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -58,7 +58,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 */ /*------------------------------------------------------------------------------ @@ -202,7 +201,7 @@ void sensor_init * * @return The configured mount orientation. */ -MOUNT_ORIENTATION get_mount_orientation +MOUNT_ORIENTATION sensor_get_mount_orientation ( void ); @@ -212,7 +211,7 @@ MOUNT_ORIENTATION get_mount_orientation * * @param orientation Mount orientation to use for axis remapping. */ -void set_mount_orientation +void sensor_set_mount_orientation ( MOUNT_ORIENTATION orientation ); @@ -231,7 +230,7 @@ void sensor_reset_velo * @param imu_converted Converted IMU data. * @param state_estimate State estimate to update. */ -void sensor_body_state +SENSOR_STATUS sensor_body_state ( const IMU_CONVERTED* imu_converted, STATE_ESTIMATION* state_estimate