Provide gravity as an algorithm output

Author: xioTechnologies

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.
  */

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);

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},

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