feature: Add dummy mode

ardriver/xreal/xrealsrc/FusionCompass.c

@@ -0,0 +1,49 @@
+/**
+ * @file FusionCompass.c
+ * @author Seb Madgwick
+ * @brief Tilt-compensated compass to calculate the magnetic heading using
+ * accelerometer and magnetometer measurements.
+ */
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionAxes.h"
+#include "FusionCompass.h"
+#include <math.h>
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Calculates the magnetic heading.
+ * @param convention Earth axes convention.
+ * @param accelerometer Accelerometer measurement in any calibrated units.
+ * @param magnetometer Magnetometer measurement in any calibrated units.
+ * @return Heading angle in degrees.
+ */
+float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer) {
+    switch (convention) {
+        case FusionConventionNwu: {
+            const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
+            const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
+            return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
+        }
+        case FusionConventionEnu: {
+            const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
+            const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
+            const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f);
+            return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x));
+        }
+        case FusionConventionNed: {
+            const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f);
+            const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer));
+            const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up));
+            return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
+        }
+    }
+    return 0; // avoid compiler warning
+}
+
+//------------------------------------------------------------------------------
+// End of file