Skip to content

Fix the unit tests for mod after Mahony #158

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
ETSells merged 4 commits into from
Oct 4, 2026
Merged

Fix the unit tests for mod after Mahony #158

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3af6d17
Fix the unit tests for mod after Mahony
ETSells Oct 1, 2026
4b48997
Fix coverage for telemetry using a gcovr exclusion
ETSells Oct 3, 2026
dabb1e8
Declare sensor as a software integration test and fix coverage.
ETSells Oct 3, 2026
46a18b5
Update math_sdr requirements and pull up framework
ETSells Oct 3, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 40 additions & 4 deletions _reqs/srd.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
<!-- next traceable tag: RQ.MOD.00035 -->
<!-- next traceable tag: RQ.MOD.00040 -->
<!-- This req doc is written and updated manually and checked in along with source code -->
<!-- Desired behaviors are expected to be specified by human engineers. No AI assistance allowed for requirement specification -->
# SDR "mod" Library Software Requirements Document

### QA level: Safety Critical
### Part Number: A0013-XXX
### Part Number: A0015-XXX

## 1. Structural

Expand Down Expand Up @@ -52,6 +52,8 @@ RQ.MOD.00015 - The error handler shall provide an overridable default error call

RQ.MOD.00016 - The library shall provide a method to compute a Castagnoli 32-bit cyclic redundancy check.

RQ.MOD.00040 - The library shall provide a method to clamp a float within a specified range.

The library shall provide all of the following quaternion operations:

RQ.MOD.00017 - Hamilton Product (multiplication)
Expand All @@ -66,6 +68,28 @@ The library shall provide all of the following quaternion operations:

RQ.MOD.00022 - Conjugation

RQ.MOD.00047 - Creation from euler angles

RQ.MOD.00048 - Determine whether quat is finite

RQ.MOD.00049 - Body-to-world frame transition

RQ.MOD.00050 - World-to-body frame transition

The library shall provide all of the following 3-element vector operations:

RQ.MOD.00041 - Addition

RQ.MOD.00042 - Cross Product

RQ.MOD.00043 - Determine whether vector is finite

RQ.MOD.00044 - Determine magnitude

RQ.MOD.00045 - Normalization

RQ.MOD.00046 - Scalar Multiplication

### 2.5. Sensor

RQ.MOD.00023 - The library shall provide a method to start retrieval of sensor data
Expand All @@ -82,7 +106,7 @@ The library shall provide the following IMU and magnetometer utilities:

RQ.MOD.00027 - Conversion of raw IMU data to floating point values

RQ.MOD.00028 - Integration of acceleration data to yield velocity
RQ.MOD.00028 - Integration of acceleration data and body state to yield velocity

RQ.MOD.00029 - Re-mapping raw IMU data based on orientation

Expand All @@ -100,4 +124,16 @@ The library shall provide a method to construct each of the following messages:

RQ.MOD.00034 - A packet containing vehicle identification information

RQ.MOD.00035 - A packet containing vehicle calibration information
RQ.MOD.00035 - A packet containing vehicle calibration information

### 2.7. Mahony

RQ.MOD.00036 - The library shall provide a Mahony filter to fuse accelerometer and gyroscope data to return an attitude estimate.

RQ.MOD.00037 - The library shall report vehicle orientation with a unit quaternion.

The library shall disable accelerometer fusion in the following cases:

RQ.MOD.00038 - The project disables fusion.

RQ.MOD.00039 - The provided parameters for accelerometer fusion are invalid.
2 changes: 1 addition & 1 deletion _test/framework
Open in desktop
Submodule framework updated 4 files
+2 −1 src/sdrtf_prv.h
+14 −5 src/sdrtf_pub.h
+31 −7 src/test_runner.c
+169 −0 tools/trace_report.py
38 changes: 19 additions & 19 deletions _test/mod/mahony/test_mahony.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2656,32 +2656,32 @@ int main
unit_test tests[] =
{
{ "mahony_init_identity_attitude", test_mahony_init_identity_attitude },
{ "mahony_init_normalizes_attitude", test_mahony_init_normalizes_attitude },
{ "mahony_init_normalizes_attitude", test_mahony_init_normalizes_attitude, "RQ.MOD.00037" },
{ "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_zero_rate", test_mahony_update_gyro_zero_rate, "RQ.MOD.00036" },
{ "mahony_update_gyro_positive_yaw", test_mahony_update_gyro_positive_yaw, "RQ.MOD.00036" },
{ "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_update_imu_aligned_gravity", test_mahony_update_imu_aligned_gravity, "RQ.MOD.00036" },
{ "mahony_update_imu_roll_error_converges", test_mahony_update_imu_roll_error_converges, "RQ.MOD.00036" },
{ "mahony_update_imu_pitch_error_converges", test_mahony_update_imu_pitch_error_converges, "RQ.MOD.00036" },
{ "mahony_update_imu_yaw_error_does_not_converge", test_mahony_update_imu_yaw_error_does_not_converge, "RQ.MOD.00036" },
{ "mahony_update_imu_zero_accel_uses_gyro_only", test_mahony_update_imu_zero_accel_uses_gyro_only, "RQ.MOD.00036" },
{ "mahony_update_imu_disabled_accel_uses_gyro_only", test_mahony_update_imu_disabled_accel_uses_gyro_only, "RQ.MOD.00038" },
{ "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 },
{ "mahony_update_imu_rejects_low_accel_magnitude", test_mahony_update_imu_rejects_low_accel_magnitude, "RQ.MOD.00039" },
{ "mahony_update_imu_rejects_high_accel_magnitude", test_mahony_update_imu_rejects_high_accel_magnitude, "RQ.MOD.00039" },
{ "mahony_update_imu_rejects_nonfinite_accel", test_mahony_update_imu_rejects_nonfinite_accel, "RQ.MOD.00036" },
{ "mahony_update_imu_accepts_valid_accel_magnitude", test_mahony_update_imu_accepts_valid_accel_magnitude, "RQ.MOD.00036" },
{ "mahony_integral_zero_gain_does_not_accumulate", test_mahony_integral_zero_gain_does_not_accumulate, "RQ.MOD.00036" },
{ "mahony_integral_valid_error_accumulates", test_mahony_integral_valid_error_accumulates, "RQ.MOD.00036" },
{ "mahony_integral_disabled_accel_does_not_accumulate", test_mahony_integral_disabled_accel_does_not_accumulate, "RQ.MOD.00036, RQ.MOD.00038" },
{ "mahony_integral_invalid_accel_does_not_accumulate", test_mahony_integral_invalid_accel_does_not_accumulate, "RQ.MOD.00036, RQ.MOD.00039" },
{ "mahony_integral_is_limited", test_mahony_integral_is_limited, "RQ.MOD.00036" },
{ "mahony_integral_correction_affects_attitude", test_mahony_integral_correction_affects_attitude, "RQ.MOD.00036" },
};

TEST_INITIALIZE_TEST("mahony.c", tests);
Expand Down
173 changes: 172 additions & 1 deletion _test/mod/math_sdr/test_math_sdr.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
/*------------------------------------------------------------------------------
Standard Includes
------------------------------------------------------------------------------*/
#include <math.h>
#include <stdint.h>

/*------------------------------------------------------------------------------
Expand Down Expand Up @@ -52,6 +53,27 @@ TEST_ASSERT_EQ_UINT( "CRC-32C matches the standard check value.", crc32( test_da
} /* test_math_sdr_crc32 */


/*******************************************************************************
* *
* PROCEDURE: *
* test_math_sdr_clamp_float *
* *
* DESCRIPTION: *
* Test float clamping below, within, and above its range. *
* *
*******************************************************************************/
void test_math_sdr_clamp_float
(
void
)
{
TEST_ASSERT_EQ_FLOAT( "Values below the range clamp to its minimum.", clamp_float( -2.0f, -1.0f, 1.0f ), -1.0f );
TEST_ASSERT_EQ_FLOAT( "Values within the range are unchanged.", clamp_float( 0.5f, -1.0f, 1.0f ), 0.5f );
TEST_ASSERT_EQ_FLOAT( "Values above the range clamp to its maximum.", clamp_float( 2.0f, -1.0f, 1.0f ), 1.0f );

} /* test_math_sdr_clamp_float */


/*******************************************************************************
* *
* PROCEDURE: *
Expand Down Expand Up @@ -258,6 +280,149 @@ TEST_ASSERT_EQ_FLOAT( "Quaternion conjugation negates z.", actual.z, -4.0f );
} /* test_math_sdr_quat_conj */


/*******************************************************************************
* *
* PROCEDURE: *
* test_math_sdr_eul_to_quat *
* *
* DESCRIPTION: *
* Test quaternion creation from ZYX Euler angles. *
* *
*******************************************************************************/
void test_math_sdr_eul_to_quat
(
void
)
{
QUAT actual = eul_to_quat( 1.57079632679f, 0.0f, 0.0f );

TEST_ASSERT_EQ_FLOAT( "A zero Euler rotation produces identity w.", eul_to_quat( 0.0f, 0.0f, 0.0f ).w, 1.0f );
TEST_ASSERT_EQ_FLOAT( "A zero Euler rotation produces identity x.", eul_to_quat( 0.0f, 0.0f, 0.0f ).x, 0.0f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw calculates w.", actual.w, 0.70710678f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw calculates x.", actual.x, 0.0f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw calculates y.", actual.y, 0.0f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw calculates z.", actual.z, 0.70710678f );

} /* test_math_sdr_eul_to_quat */


/*******************************************************************************
* *
* PROCEDURE: *
* test_math_sdr_quat_is_finite *
* *
* DESCRIPTION: *
* Test detection of finite and non-finite quaternion components. *
* *
*******************************************************************************/
void test_math_sdr_quat_is_finite
(
void
)
{
TEST_ASSERT_TRUE( "A finite quaternion is reported as finite.", quat_is_finite( ( QUAT ){ 1.0f, -2.0f, 3.0f, -4.0f } ) );
TEST_ASSERT_FALSE( "A quaternion containing NaN is not finite.", quat_is_finite( ( QUAT ){ 1.0f, NAN, 0.0f, 0.0f } ) );
TEST_ASSERT_FALSE( "A quaternion containing infinity is not finite.", quat_is_finite( ( QUAT ){ 1.0f, 0.0f, 0.0f, INFINITY } ) );

} /* test_math_sdr_quat_is_finite */


/*******************************************************************************
* *
* PROCEDURE: *
* test_math_sdr_quat_rotate_body_to_world *
* *
* DESCRIPTION: *
* Test body-frame vector rotation into the world frame. *
* *
*******************************************************************************/
void test_math_sdr_quat_rotate_body_to_world
(
void
)
{
QUAT attitude = eul_to_quat( 1.57079632679f, 0.0f, 0.0f );
QUAT actual = quat_rotate_body_to_world( attitude, ( QUAT ){ 0.0f, 1.0f, 0.0f, 0.0f } );

TEST_ASSERT_EQ_FLOAT( "Rotation preserves a pure vector scalar component.", actual.w, 0.0f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw rotates body x out of world x.", actual.x, 0.0f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw rotates body x into world y.", actual.y, 1.0f );
TEST_ASSERT_EQ_FLOAT( "A yaw rotation preserves the z vector component.", actual.z, 0.0f );

} /* test_math_sdr_quat_rotate_body_to_world */


/*******************************************************************************
* *
* PROCEDURE: *
* test_math_sdr_quat_rotate_world_to_body *
* *
* DESCRIPTION: *
* Test world-frame vector rotation into the body frame. *
* *
*******************************************************************************/
void test_math_sdr_quat_rotate_world_to_body
(
void
)
{
QUAT attitude = eul_to_quat( 1.57079632679f, 0.0f, 0.0f );
QUAT actual = quat_rotate_world_to_body( attitude, ( QUAT ){ 0.0f, 0.0f, 1.0f, 0.0f } );

TEST_ASSERT_EQ_FLOAT( "Inverse rotation preserves a pure vector scalar component.", actual.w, 0.0f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw rotates world y into body x.", actual.x, 1.0f );
TEST_ASSERT_EQ_FLOAT( "A 90 degree yaw rotates world y out of body y.", actual.y, 0.0f );
TEST_ASSERT_EQ_FLOAT( "Inverse yaw rotation preserves the z vector component.", actual.z, 0.0f );

} /* test_math_sdr_quat_rotate_world_to_body */


/*******************************************************************************
* *
* PROCEDURE: *
* test_math_sdr_vector_operations *
* *
* DESCRIPTION: *
* Test the required three-element vector operations. *
* *
*******************************************************************************/
void test_math_sdr_vector_operations
(
void
)
{
VECTOR_3F actual;
VECTOR_3F normalized = { 3.0f, 4.0f, 0.0f };
VECTOR_3F zero = { 0.0f, 0.0f, 0.0f };

actual = vector_add( ( VECTOR_3F ){ 1.0f, -2.0f, 3.0f }, ( VECTOR_3F ){ 4.0f, 5.0f, -6.0f } );
TEST_ASSERT_EQ_FLOAT( "Vector addition calculates x.", actual.x, 5.0f );
TEST_ASSERT_EQ_FLOAT( "Vector addition calculates y.", actual.y, 3.0f );
TEST_ASSERT_EQ_FLOAT( "Vector addition calculates z.", actual.z, -3.0f );

actual = vector_cross( ( VECTOR_3F ){ 1.0f, 0.0f, 0.0f }, ( VECTOR_3F ){ 0.0f, 1.0f, 0.0f } );
TEST_ASSERT_EQ_FLOAT( "Vector cross product calculates x.", actual.x, 0.0f );
TEST_ASSERT_EQ_FLOAT( "Vector cross product calculates y.", actual.y, 0.0f );
TEST_ASSERT_EQ_FLOAT( "Vector cross product calculates z.", actual.z, 1.0f );

TEST_ASSERT_TRUE( "A finite vector is reported as finite.", vector_is_finite( ( VECTOR_3F ){ 1.0f, -2.0f, 3.0f } ) );
TEST_ASSERT_FALSE( "A vector containing NaN is not finite.", vector_is_finite( ( VECTOR_3F ){ 1.0f, NAN, 3.0f } ) );
TEST_ASSERT_EQ_FLOAT( "Vector magnitude is calculated correctly.", vector_magnitude( ( VECTOR_3F ){ 3.0f, 4.0f, 12.0f } ), 13.0f );

TEST_ASSERT_TRUE( "A nonzero vector is normalized.", vector_normalize( &normalized ) );
TEST_ASSERT_EQ_FLOAT( "Vector normalization calculates x.", normalized.x, 0.6f );
TEST_ASSERT_EQ_FLOAT( "Vector normalization calculates y.", normalized.y, 0.8f );
TEST_ASSERT_EQ_FLOAT( "Vector normalization calculates z.", normalized.z, 0.0f );
TEST_ASSERT_FALSE( "A zero vector cannot be normalized.", vector_normalize( &zero ) );

actual = vector_scale( ( VECTOR_3F ){ 1.0f, -2.0f, 3.0f }, -2.0f );
TEST_ASSERT_EQ_FLOAT( "Vector scaling calculates x.", actual.x, -2.0f );
TEST_ASSERT_EQ_FLOAT( "Vector scaling calculates y.", actual.y, 4.0f );
TEST_ASSERT_EQ_FLOAT( "Vector scaling calculates z.", actual.z, -6.0f );

} /* test_math_sdr_vector_operations */


/*******************************************************************************
* *
* PROCEDURE: *
Expand All @@ -278,12 +443,18 @@ Test Cases
unit_test tests[] =
{
{ "Math SDR: CRC-32C", test_math_sdr_crc32, "RQ.MOD.00016" },
{ "Math SDR: Clamp Float", test_math_sdr_clamp_float, "RQ.MOD.00040" },
{ "Math SDR: Quaternion Multiplication", test_math_sdr_quat_mult, "RQ.MOD.00017" },
{ "Math SDR: Quaternion Dot Product", test_math_sdr_quat_dot, "RQ.MOD.00018" },
{ "Math SDR: Quaternion Addition", test_math_sdr_quat_add, "RQ.MOD.00019" },
{ "Math SDR: Quaternion Scaling", test_math_sdr_quat_scale, "RQ.MOD.00020" },
{ "Math SDR: Quaternion Normalization", test_math_sdr_quat_normalize, "RQ.MOD.00021" },
{ "Math SDR: Quaternion Conjugation", test_math_sdr_quat_conj, "RQ.MOD.00022" }
{ "Math SDR: Quaternion Conjugation", test_math_sdr_quat_conj, "RQ.MOD.00022" },
{ "Math SDR: Quaternion from Euler Angles", test_math_sdr_eul_to_quat, "RQ.MOD.00047" },
{ "Math SDR: Quaternion Finiteness", test_math_sdr_quat_is_finite, "RQ.MOD.00048" },
{ "Math SDR: Body to World Rotation", test_math_sdr_quat_rotate_body_to_world, "RQ.MOD.00049" },
{ "Math SDR: World to Body Rotation", test_math_sdr_quat_rotate_world_to_body, "RQ.MOD.00050" },
{ "Math SDR: Vector Operations", test_math_sdr_vector_operations, "RQ.MOD.00041 RQ.MOD.00042 RQ.MOD.00043 RQ.MOD.00044 RQ.MOD.00045 RQ.MOD.00046" }
};

/*------------------------------------------------------------------------------
Expand Down
4 changes: 3 additions & 1 deletion _test/mod/sensor/Makefile
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,9 @@ test_sensor.c \
mock/test_sensor_stubs.c

COV_C_SOURCES = \
$(ROOT_DIR)/mod/sensor/sensor.c
$(ROOT_DIR)/mod/sensor/sensor.c \
$(ROOT_DIR)/mod/math_sdr/math_sdr.c \
$(ROOT_DIR)/mod/mahony/mahony.c

FRAMEWORK_SOURCES = \
$(FRAMEWORK_DIR)/src/test_assert.c \
Expand Down
Loading
Loading