From bdc93242fdde059f3af9218336affc8a5bde0df7 Mon Sep 17 00:00:00 2001 From: x-io Technologies Date: Wed, 11 Sep 2024 10:31:24 +0100 Subject: [PATCH] Provide gravity as an algorithm output --- Fusion/FusionAhrs.c | 27 ++++++++++++++++----------- Fusion/FusionAhrs.h | 2 ++ Python/Python-C-API/Ahrs.h | 11 +++++++++++ README.md | 2 +- 4 files changed, 30 insertions(+), 12 deletions(-) diff --git a/Fusion/FusionAhrs.c b/Fusion/FusionAhrs.c index 661e6e8..b353c44 100644 --- a/Fusion/FusionAhrs.c +++ b/Fusion/FusionAhrs.c @@ -380,38 +380,43 @@ void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quat } /** - * @brief Returns the linear acceleration measurement equal to the accelerometer - * measurement with the 1 g of gravity removed. + * @brief Returns the direction of gravity in the sensor coordinate frame. * @param ahrs AHRS algorithm structure. - * @return Linear acceleration measurement in g. + * @return Direction of gravity in the sensor coordinate frame. */ -FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) { +FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element - - // Calculate gravity in the sensor coordinate frame const FusionVector gravity = {.axis = { .x = 2.0f * (Q.x * Q.z - Q.w * Q.y), .y = 2.0f * (Q.y * Q.z + Q.w * Q.x), .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), }}; // third column of transposed rotation matrix + return gravity; +#undef Q +} - // Remove gravity from accelerometer measurement +/** + * @brief Returns the linear acceleration measurement equal to the accelerometer + * measurement with gravity removed. + * @param ahrs AHRS algorithm structure. + * @return Linear acceleration measurement in g. + */ +FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) { switch (ahrs->settings.convention) { case FusionConventionNwu: case FusionConventionEnu: { - return FusionVectorSubtract(ahrs->accelerometer, gravity); + return FusionVectorSubtract(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); } case FusionConventionNed: { - return FusionVectorAdd(ahrs->accelerometer, gravity); + return FusionVectorAdd(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); } } return FUSION_VECTOR_ZERO; // avoid compiler warning -#undef Q } /** * @brief Returns the Earth acceleration measurement equal to accelerometer - * measurement in the Earth coordinate frame with the 1 g of gravity removed. + * measurement in the Earth coordinate frame with gravity removed. * @param ahrs AHRS algorithm structure. * @return Earth acceleration measurement in g. */ diff --git a/Fusion/FusionAhrs.h b/Fusion/FusionAhrs.h index 15473ad..4cf785b 100644 --- a/Fusion/FusionAhrs.h +++ b/Fusion/FusionAhrs.h @@ -93,6 +93,8 @@ FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion); +FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs); + FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs); FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs); diff --git a/Python/Python-C-API/Ahrs.h b/Python/Python-C-API/Ahrs.h index c2d061f..dd44b92 100644 --- a/Python/Python-C-API/Ahrs.h +++ b/Python/Python-C-API/Ahrs.h @@ -52,6 +52,16 @@ static int ahrs_set_quaternion(Ahrs *self, PyObject *value, void *closure) { return 0; } +static PyObject *ahrs_get_gravity(Ahrs *self) { + FusionVector *const gravity = malloc(sizeof(FusionVector)); + *gravity = FusionAhrsGetGravity(&self->ahrs); + + const npy_intp dims[] = {3}; + PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, gravity->array); + PyArray_ENABLEFLAGS((PyArrayObject *) array, NPY_ARRAY_OWNDATA); + return array; +} + static PyObject *ahrs_get_linear_acceleration(Ahrs *self) { FusionVector *const linear_acceleration = malloc(sizeof(FusionVector)); *linear_acceleration = FusionAhrsGetLinearAcceleration(&self->ahrs); @@ -204,6 +214,7 @@ static int ahrs_set_heading(Ahrs *self, PyObject *value, void *closure) { static PyGetSetDef ahrs_get_set[] = { {"settings", NULL, (setter) ahrs_set_settings, "", NULL}, {"quaternion", (getter) ahrs_get_quaternion, (setter) ahrs_set_quaternion, "", NULL}, + {"gravity", (getter) ahrs_get_gravity, NULL, "", NULL}, {"linear_acceleration", (getter) ahrs_get_linear_acceleration, NULL, "", NULL}, {"earth_acceleration", (getter) ahrs_get_earth_acceleration, NULL, "", NULL}, {"internal_states", (getter) ahrs_get_internal_states, NULL, "", NULL}, diff --git a/README.md b/README.md index 146ca5c..abbdb5d 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,7 @@ The magnetic rejection feature reduces the errors that result from temporary mag ### Algorithm outputs -The algorithm provides three outputs: quaternion, linear acceleration, and Earth acceleration. The quaternion describes the orientation of the sensor relative to the Earth. This can be converted to a rotation matrix using the `FusionQuaternionToMatrix` function or to Euler angles using the `FusionQuaternionToEuler` function. The linear acceleration is the accelerometer measurement with the 1 g of gravity removed. The Earth acceleration is the accelerometer measurement in the Earth coordinate frame with the 1 g of gravity removed. The algorithm supports North-West-Up (NWU), East-North-Up (ENU), and North-East-Down (NED) axes conventions. +The algorithm provides four outputs: quaternion, gravity, linear acceleration, and Earth acceleration. The quaternion describes the orientation of the sensor relative to the Earth. This can be converted to a rotation matrix using the `FusionQuaternionToMatrix` function or to Euler angles using the `FusionQuaternionToEuler` function. Gravity is a direction of gravity in the sensor coordinate frame. Linear acceleration is the accelerometer measurement with gravity removed. Earth acceleration is the accelerometer measurement in the Earth coordinate frame with gravity removed. The algorithm supports North-West-Up (NWU), East-North-Up (ENU), and North-East-Down (NED) axes conventions. ### Algorithm settings