From c4dbc33b211297f0f0d71de9425d1f7d92700bb3 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Tue, 21 Jul 2026 11:59:20 -0700 Subject: [PATCH 1/6] Document attitude quaternion frame convention --- sensor/sensor.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/sensor/sensor.h b/sensor/sensor.h index 2e1c65a..9b42fa7 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -96,6 +96,15 @@ typedef enum /* State estimation from processed sensors */ typedef struct _STATE_ESTIMATION { + /* + * Body-to-world attitude quaternion. + * + * Body-frame vector to world frame: + * v_world = q * v_body * conjugate(q) + * + * World-frame vector to body frame: + * v_body = conjugate(q) * v_world * q + */ QUAT attitude; float roll_rate; float velocity; From 4c64bb7de80198c1ac7fc8b9052eb6670be34425 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Tue, 21 Jul 2026 12:20:32 -0700 Subject: [PATCH 2/6] Add world-to-body quaternion vector rotation --- math_sdr/math_sdr.c | 30 +++++++++++++++++++++++++++++- math_sdr/math_sdr.h | 16 +++++++++++++++- 2 files changed, 44 insertions(+), 2 deletions(-) diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 272a993..20da2df 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -262,6 +262,34 @@ return result; } /* quat_conj */ +/** + * @brief Rotates a vector from the world frame into the body frame. + * + * The attitude quaternion represents the body-to-world rotation: + * + * vector_body = conjugate(q) * vector_world * q + * + * @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 + ) +{ + QUAT attitude_conj = quat_conj(attitude); + + return quat_mult + ( + quat_mult(attitude_conj, vector_world), + attitude + ); + +} /* quat_rotate_world_to_body */ + /******************************************************************************* * END OF FILE * -*******************************************************************************/ \ No newline at end of file +*******************************************************************************/ diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 4f7c80d..e928864 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -157,7 +157,21 @@ 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 + ); + #ifdef __cplusplus } #endif From 654782b52358adac1aa90e36310f8b9ea6c064c1 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Tue, 21 Jul 2026 13:01:49 -0700 Subject: [PATCH 3/6] Apply gravity compensation to velocity integration --- math_sdr/math_sdr.c | 28 ++++++++++++++++++++ math_sdr/math_sdr.h | 14 ++++++++++ sensor/sensor.c | 62 ++++++++++++++++++++++++++++++++++++++------- 3 files changed, 95 insertions(+), 9 deletions(-) diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 20da2df..b36e30d 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -290,6 +290,34 @@ QUAT quat_rotate_world_to_body } /* quat_rotate_world_to_body */ +/** + * @brief Rotates a vector from the body frame into the world frame. + * + * The attitude quaternion represents the body-to-world rotation: + * + * vector_world = q * vector_body * conjugate(q) + * + * @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 + ) +{ + QUAT attitude_conj = quat_conj(attitude); + + return quat_mult + ( + quat_mult(attitude, vector_body), + attitude_conj + ); + +} /* quat_rotate_body_to_world */ + /******************************************************************************* * END OF FILE * *******************************************************************************/ diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index e928864..37b594a 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -172,6 +172,20 @@ QUAT quat_rotate_world_to_body 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 + ); + #ifdef __cplusplus } #endif diff --git a/sensor/sensor.c b/sensor/sensor.c index 04f44ef..79ba662 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -57,7 +57,9 @@ extern IMU_OFFSET imu_offset; uint64_t imu_velo_tick = 0; /* IMU */ -float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; +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 }; @@ -550,23 +552,65 @@ void sensor_imu_velo { float velo_x, velo_y, velo_z, velocity; -float accel_x = imu_converted->accel_x; -float accel_y = imu_converted->accel_y; -float accel_z = imu_converted->accel_z; +/* The world frame defines gravity in the +Z direction. */ +const QUAT gravity_world = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +/* + * The attitude quaternion is body-to-world, so rotate world gravity + * into the body frame before subtracting it from the accelerometer. + */ +QUAT gravity_body = quat_rotate_world_to_body + ( + state_estimate->attitude, + gravity_world + ); + +QUAT linear_accel_body = + { + .w = 0.0f, + .x = imu_converted->accel_x - gravity_body.x, + .y = imu_converted->accel_y - gravity_body.y, + .z = imu_converted->accel_z - gravity_body.z + }; + +/* + * Rotate gravity-compensated acceleration into the world frame so the + * integrated velocity components remain in a fixed coordinate frame. + */ +QUAT linear_accel_world = quat_rotate_body_to_world + ( + state_estimate->attitude, + linear_accel_body + ); + +float accel_world_x = linear_accel_world.x; +float accel_world_y = linear_accel_world.y; +float accel_world_z = linear_accel_world.z; float ts_delta; uint64_t current_tick = get_us_tick(); uint64_t imu_tdelta = current_tick - imu_velo_tick; -ts_delta = imu_tdelta / 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_x*ts_delta; -velo_y = velo_y_prev + accel_y*ts_delta; -velo_z = velo_z_prev + accel_z*ts_delta; +velo_x = velo_x_prev + accel_world_x * ts_delta; +velo_y = velo_y_prev + accel_world_y * ts_delta; +velo_z = velo_z_prev + accel_world_z * ts_delta; // Calculate the velocity scalar -velocity = sqrtf(powf(velo_x, 2.0) + powf(velo_y, 2.0) + powf(velo_z, 2.0)); +velocity = sqrtf + ( + velo_x * velo_x + + velo_y * velo_y + + velo_z * velo_z + ); /* Update state estimations*/ state_estimate->velo_x = velo_x; From 15ecd7222640f4a5d7617093a97b3f850fbc701b Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 23 Jul 2026 11:58:58 -0700 Subject: [PATCH 4/6] Fix quaternion rotation function formatting --- math_sdr/math_sdr.c | 69 +++++++++++++++++++++++---------------------- 1 file changed, 35 insertions(+), 34 deletions(-) diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index b36e30d..cbdfe2a 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -263,58 +263,59 @@ return result; } /* quat_conj */ /** - * @brief Rotates a vector from the world frame into the body frame. - * - * The attitude quaternion represents the body-to-world rotation: - * - * vector_body = conjugate(q) * vector_world * q - * - * @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. - */ + * @brief Rotates a vector from the world frame into the body frame. + * + * The attitude quaternion represents the body-to-world rotation: + * + * vector_body = conjugate(q) * vector_world * q + * + * @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 ) { - QUAT attitude_conj = quat_conj(attitude); +QUAT attitude_conj = quat_conj(attitude); - return quat_mult - ( - quat_mult(attitude_conj, vector_world), - attitude - ); +return quat_mult + ( + quat_mult(attitude_conj, vector_world), + attitude + ); } /* quat_rotate_world_to_body */ + /** - * @brief Rotates a vector from the body frame into the world frame. - * - * The attitude quaternion represents the body-to-world rotation: - * - * vector_world = q * vector_body * conjugate(q) - * - * @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. - */ + * @brief Rotates a vector from the body frame into the world frame. + * + * The attitude quaternion represents the body-to-world rotation: + * + * vector_world = q * vector_body * conjugate(q) + * + * @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 ) { - QUAT attitude_conj = quat_conj(attitude); +QUAT attitude_conj = quat_conj(attitude); - return quat_mult - ( - quat_mult(attitude, vector_body), - attitude_conj - ); +return quat_mult + ( + quat_mult(attitude, vector_body), + attitude_conj + ); } /* quat_rotate_body_to_world */ From 2243a5d8fc5aa529821bf14f2113eb75697f9ee3 Mon Sep 17 00:00:00 2001 From: Bjorn Date: Fri, 24 Jul 2026 14:54:24 -0700 Subject: [PATCH 5/6] Add math_sdr unit tests --- .gitmodules | 3 + test/framework | 1 + test/math_sdr/.gitignore | 6 + test/math_sdr/Makefile | 103 ++++++++++++ test/math_sdr/main.h | 8 + test/math_sdr/test_math_sdr.c | 286 ++++++++++++++++++++++++++++++++++ 6 files changed, 407 insertions(+) create mode 100644 .gitmodules create mode 160000 test/framework create mode 100644 test/math_sdr/.gitignore create mode 100644 test/math_sdr/Makefile create mode 100644 test/math_sdr/main.h create mode 100644 test/math_sdr/test_math_sdr.c diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..28a3575 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "test/framework"] + path = test/framework + url = https://github.com/SunDevilRocketry/sdrtf.git diff --git a/test/framework b/test/framework new file mode 160000 index 0000000..29ac66b --- /dev/null +++ b/test/framework @@ -0,0 +1 @@ +Subproject commit 29ac66bf93b2b0934c168797761dbb2be740a85f diff --git a/test/math_sdr/.gitignore b/test/math_sdr/.gitignore new file mode 100644 index 0000000..9bf4711 --- /dev/null +++ b/test/math_sdr/.gitignore @@ -0,0 +1,6 @@ +build/ +coverage/ +results.txt +*.gcda +*.gcno +*.gcov diff --git a/test/math_sdr/Makefile b/test/math_sdr/Makefile new file mode 100644 index 0000000..4951fa4 --- /dev/null +++ b/test/math_sdr/Makefile @@ -0,0 +1,103 @@ +################################################################ +# +# math_sdr unit tests (based on gcc) +# +################################################################ + +################################################################ +# target +################################################################ +TARGET = math_sdr + +################################################################ +# build variables +################################################################ +DEBUG ?= 0 +OPT = -Og + +################################################################ +# paths +################################################################ +BUILD_DIR = build +ROOT_DIR = ../.. + +################################################################ +# source +################################################################ +TEST_SOURCES = \ +test_math_sdr.c + +COV_C_SOURCES = \ +$(ROOT_DIR)/math_sdr/math_sdr.c + +FRAMEWORK_SOURCES = \ +$(ROOT_DIR)/test/framework/src/test_assert.c \ +$(ROOT_DIR)/test/framework/src/test_runner.c + +C_SOURCES = $(TEST_SOURCES) $(COV_C_SOURCES) $(FRAMEWORK_SOURCES) + +################################################################ +# compiler +################################################################ +CC = gcc + +################################################################ +# C flags +################################################################ +C_INCLUDES = \ +-I. \ +-I$(ROOT_DIR)/math_sdr \ +-I$(ROOT_DIR)/test/framework/src + +C_DEFS = \ +-DUNIT_TEST + +CFLAGS = $(C_INCLUDES) $(C_DEFS) $(OPT) -Wall -g + +CFLAGS += -Wno-unused-function +CFLAGS += -Wno-unused-variable +CFLAGS += -ftest-coverage +CFLAGS += -fprofile-arcs +ifeq ($(DEBUG), 1) +CFLAGS += -DDEBUG +else +CFLAGS += -DRELBLD +endif + +################################################################ +# build +################################################################ +all: clean $(BUILD_DIR)/$(TARGET) + +OBJECTS = $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o))) +vpath %.c $(sort $(dir $(C_SOURCES))) + +$(BUILD_DIR)/%.o: %.c $(BUILD_DIR) + $(CC) -c $(CFLAGS) $< -o $@ + +$(BUILD_DIR)/$(TARGET): $(OBJECTS) + $(CC) $(OBJECTS) -o $@ -lgcov -lm + +$(BUILD_DIR): + mkdir $@ + +################################################################ +# test +################################################################ +test: + @echo THIS TEST MUST BE EXECUTED IN A BASH TERMINAL. CMD/PS do not work. + -rm -fR $(BUILD_DIR) + $(MAKE) all + $(BUILD_DIR)/$(TARGET) + tail -n 7 "results.txt" + mkdir -p coverage + gcovr $(BUILD_DIR) \ + --filter $(COV_C_SOURCES) \ + --html-details coverage/coverage.html \ + --json coverage/coverage.json + +################################################################ +# clean +################################################################ +clean: + -rm -fR $(BUILD_DIR) diff --git a/test/math_sdr/main.h b/test/math_sdr/main.h new file mode 100644 index 0000000..5fa2343 --- /dev/null +++ b/test/math_sdr/main.h @@ -0,0 +1,8 @@ +#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 new file mode 100644 index 0000000..75e58e6 --- /dev/null +++ b/test/math_sdr/test_math_sdr.c @@ -0,0 +1,286 @@ +/******************************************************************************* + * + * 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 + ******************************************************************************/ From e841587a1d379ed87e5235f99f0bd9ac8f1edd4f Mon Sep 17 00:00:00 2001 From: Bjorn Date: Sat, 25 Jul 2026 16:30:39 -0700 Subject: [PATCH 6/6] Rename test directory and document NED frame --- .gitmodules | 4 ++-- {test => _test}/framework | 0 {test => _test}/math_sdr/.gitignore | 0 {test => _test}/math_sdr/Makefile | 6 +++--- {test => _test}/math_sdr/main.h | 0 {test => _test}/math_sdr/test_math_sdr.c | 0 sensor/sensor.c | 5 ++++- 7 files changed, 9 insertions(+), 6 deletions(-) rename {test => _test}/framework (100%) rename {test => _test}/math_sdr/.gitignore (100%) rename {test => _test}/math_sdr/Makefile (95%) rename {test => _test}/math_sdr/main.h (100%) rename {test => _test}/math_sdr/test_math_sdr.c (100%) diff --git a/.gitmodules b/.gitmodules index 28a3575..f90af2f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "test/framework"] - path = test/framework +[submodule "_test/framework"] + path = _test/framework url = https://github.com/SunDevilRocketry/sdrtf.git diff --git a/test/framework b/_test/framework similarity index 100% rename from test/framework rename to _test/framework diff --git a/test/math_sdr/.gitignore b/_test/math_sdr/.gitignore similarity index 100% rename from test/math_sdr/.gitignore rename to _test/math_sdr/.gitignore diff --git a/test/math_sdr/Makefile b/_test/math_sdr/Makefile similarity index 95% rename from test/math_sdr/Makefile rename to _test/math_sdr/Makefile index 4951fa4..af1f5b5 100644 --- a/test/math_sdr/Makefile +++ b/_test/math_sdr/Makefile @@ -31,8 +31,8 @@ COV_C_SOURCES = \ $(ROOT_DIR)/math_sdr/math_sdr.c FRAMEWORK_SOURCES = \ -$(ROOT_DIR)/test/framework/src/test_assert.c \ -$(ROOT_DIR)/test/framework/src/test_runner.c +$(ROOT_DIR)/_test/framework/src/test_assert.c \ +$(ROOT_DIR)/_test/framework/src/test_runner.c C_SOURCES = $(TEST_SOURCES) $(COV_C_SOURCES) $(FRAMEWORK_SOURCES) @@ -47,7 +47,7 @@ CC = gcc C_INCLUDES = \ -I. \ -I$(ROOT_DIR)/math_sdr \ --I$(ROOT_DIR)/test/framework/src +-I$(ROOT_DIR)/_test/framework/src C_DEFS = \ -DUNIT_TEST diff --git a/test/math_sdr/main.h b/_test/math_sdr/main.h similarity index 100% rename from test/math_sdr/main.h rename to _test/math_sdr/main.h diff --git a/test/math_sdr/test_math_sdr.c b/_test/math_sdr/test_math_sdr.c similarity index 100% rename from test/math_sdr/test_math_sdr.c rename to _test/math_sdr/test_math_sdr.c diff --git a/sensor/sensor.c b/sensor/sensor.c index 79ba662..93f8f0c 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -552,7 +552,10 @@ void sensor_imu_velo { float velo_x, velo_y, velo_z, velocity; -/* The world frame defines gravity in the +Z direction. */ +/* + * The world frame uses North-East-Down (NED) coordinates, + * so gravity points in the world-frame +Z direction. + */ const QUAT gravity_world = { .w = 0.0f,