feature(LSM303DLHC): Added Temperature Compensation

Added:
- Temperature reading
- Temperature compensation for yaw
- Modified Alpha to put more weight on magnetometer

frtos/config/magnetometer_config.h

@@ -7,7 +7,7 @@
 
 #define DIRECTION_READ_DELAY                    ( 100 )
 
-#define ALPHA                                   ( 0.98f ) // Complementary
+#define ALPHA                                   ( 0.01f ) // Complementary
                                                           // Filter Constant
 
 /**

frtos/magnetometer/LSM303DLHC_register.h

@@ -24,6 +24,20 @@
 #define LSM303_OUT_Y_L_M            0x08
 #define LSM303_SR_REG_M             0x09
 
+// Temperature registers
+#define LSM303_TEMP_OUT_H_M         0x31
+#define LSM303_TEMP_OUT_L_M         0x32
+
+// LSM303DLHC temperature compensation coefficients
+#define SCALE_Z                     0.9 // Scale factor for Z-axis
+#define OFFSET_Z                    5.0 // Offset for Z-axis
+
+#define TEMPERATURE_OFFSET          25.0 // Reference temperature for calibration
+#define TEMPERATURE_COEFFICIENT_Z   0.33
+
+#define OFFSET_Z                    5.0
+#define SCALE_Z                     0.9
+
 #define ACCEL_ADDR                  0x19
 #define MAG_ADDR                    0x1E
 

frtos/magnetometer/magnetometer_direction.h

@@ -16,6 +16,10 @@
  *          The complementary filter is used to combine the accelerometer
  *          and magnetometer data to calculate the direction of the car
  *
+ *          Source:
+ *          https://www.nxp.com/docs/en/application-note/AN3461.pdf
+ *          https://ahrs.readthedocs.io/en/latest/filters/complementary.html
+ *
  */
 
 #ifndef MAGNETOMETER_DIRECTION_H
@@ -40,9 +44,10 @@ calculate_roll(int16_t acceleration[3]) {
  */
 static inline float
 calculate_pitch(int16_t acceleration[3]) {
-    return atan2(- acceleration[0], sqrt(acceleration[1] * acceleration[1] +
+    return atan2(- acceleration[0],
-                                         acceleration[2] * acceleration[2])) *
+                 sqrt((acceleration[1] * acceleration[1]) +
-           (180.0 / M_PI);
+                      (acceleration[2] * acceleration[2]))
+    ) * (180.0 / M_PI);
 }
 
 /**
@@ -66,6 +71,27 @@ calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
     return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
 }
 
+/**
+ * @brief Compensate the magnetometer readings for temperature
+ * @param yaw_mag       Yaw calculated from Magnetometer Data
+ * @param temperature   Temperature in degrees Celsius
+ * @return              Compensated Yaw
+ */
+float
+compensate_magnetometer(float yaw_mag, int16_t temperature) {
+    // Calculate temperature difference from the reference temperature
+    float delta_temp = (float) (temperature - TEMPERATURE_OFFSET);
+
+    // Apply temperature compensation to each axis using macros
+    float compensated_yaw_mag =
+            yaw_mag - (delta_temp * TEMPERATURE_COEFFICIENT_Z);
+
+    // Apply scale and offset corrections using macros
+    compensated_yaw_mag = (compensated_yaw_mag - OFFSET_Z) * SCALE_Z;
+
+    return compensated_yaw_mag;
+}
+
 /**
  * @brief Adjust Yaw to be between 0 and 360 degrees
  * @param yaw   Yaw calculated from Complementary Filter
@@ -80,6 +106,8 @@ adjust_yaw(float yaw) {
  * @brief Calculate the Compass Direction (N, NE, E, SE, S, SW, W, NW)
  * 22.5 = 360 / 16, used to calculate the compass direction from
  * the compass direction enum
+ * 45.0 = 360 / 8, used to calculate the compass direction from
+ * the orientation (0 - 7)
  * @param yaw   Yaw calculated from Complementary Filter
  * @return      Compass Direction
  */
@@ -120,7 +148,9 @@ static inline void
 update_orientation_data(float roll, float pitch, float yaw,
                         compass_direction_t compass_direction) {
     g_direction.roll = roll;
+    g_direction.roll_angle = (roll > 0) ? LEFT : RIGHT;
     g_direction.pitch = pitch;
+    g_direction.pitch_angle = (pitch > 0) ? UP : DOWN;
     g_direction.yaw = yaw;
     g_direction.orientation = compass_direction;
 }
@@ -133,13 +163,20 @@ update_orientation_data(float roll, float pitch, float yaw,
  * @param magnetometer  Magnetometer Data
  */
 static void
-read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
+read_direction(int16_t acceleration[3],
+               int16_t magnetometer[3],
+               int16_t temperature[1]) {
+
     float roll = calculate_roll(acceleration);
     float pitch = calculate_pitch(acceleration);
     float yaw_mag = calculate_yaw_magnetometer(magnetometer);
 
+    // Apply temperature compensation to the magnetometer data
+    float compensated_mag_yaw = compensate_magnetometer(yaw_mag,
+                                                        temperature[0]);
+
     float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI);
-    float yaw = calculate_yaw_complementary(yaw_acc, yaw_mag);
+    float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw);
 
     yaw = adjust_yaw(yaw);
 
@@ -156,27 +193,16 @@ read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
  * @brief Task to Monitor the Direction of the Car
  * @param params
  */
-void
+void print_orientation_data() {
-monitor_direction_task(__unused void *params) {
-    for (;;)
-    {
-        if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
-        {
-            // Read from message buffer
-            int16_t magnetometer[3];
-            int16_t accelerometer[3];
-
-            read_magnetometer(magnetometer);
-            read_accelerometer(accelerometer);
-
-            read_direction(accelerometer, magnetometer);
-
     printf("Roll: %f, Pitch: %f, Yaw: %f\n",
-                   g_direction.roll, g_direction.pitch, g_direction.yaw);
+           g_direction.roll,
+           g_direction.pitch,
+           g_direction.yaw
+    );
+}
 
-            printf("Direction:");
+void print_direction(compass_direction_t direction) {
-
+    switch (direction)
-            switch (g_direction.orientation)
     {
         case NORTH:
             printf("North\n");
@@ -203,27 +229,56 @@ monitor_direction_task(__unused void *params) {
             printf("North West\n");
             break;
     }
+}
 
-            switch (g_direction.roll_angle)
+void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) {
+    switch (roll_angle)
     {
         case LEFT:
-                    printf("You're Flying!\n");
-                    break;
-                case RIGHT:
-                    printf("You're Plunging!\n");
-                    break;
-            }
-
-            switch (g_direction.pitch_angle)
-            {
-                case UP:
             printf("Your left wheel is in the air!\n");
             break;
-                case DOWN:
+        case RIGHT:
             printf("Your right wheel is in the air!\n");
             break;
     }
 
+    switch (pitch_angle)
+    {
+        case UP:
+            printf("You're Flying!\n");
+            break;
+        case DOWN:
+            printf("You're Plunging!\n");
+            break;
+    }
+}
+
+void monitor_direction_task(__unused void *params) {
+    for (;;)
+    {
+        if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
+        {
+            int16_t magnetometer[3];
+            int16_t accelerometer[3];
+            int16_t temperature[1];
+
+            read_magnetometer(magnetometer);
+            read_accelerometer(accelerometer);
+            read_temperature(temperature);
+
+            read_direction(accelerometer, magnetometer, temperature);
+
+            // Temperature in degrees Celsius
+            printf("Temperature: %d\n", temperature[0]);
+
+            print_orientation_data();
+
+            printf("Direction: ");
+
+            print_direction(g_direction.orientation);
+
+            print_roll_and_pitch(g_direction.roll_angle,
+                                 g_direction.pitch_angle);
         }
     }
 }

frtos/magnetometer/magnetometer_read.h

@@ -7,6 +7,7 @@
 
 #ifndef MAGNETOMETER_READ_H
 #define MAGNETOMETER_READ_H
+
 #include "magnetometer_init.h"
 
 /**
@@ -78,4 +79,32 @@ read_magnetometer(int16_t magnetometer[3]) {
     magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
 }
 
+/**
+ * @brief Read Temperature Data in Degrees Celsius
+ * @param temperature  Temperature Data in Degrees Celsius
+ */
+static inline void
+read_temperature(int16_t temperature[1]) {
+    uint8_t buffer[2];
+
+    buffer[0] = read_data(MAG_ADDR, LSM303_TEMP_OUT_H_M);
+    buffer[1] = read_data(MAG_ADDR, LSM303_TEMP_OUT_L_M);
+
+    /**
+     * Normalize temperature; it is big-endian, fixed-point
+     * 9 bits signed integer, 3 bits fractional part, 4 bits zeros
+     * and is relative to 20 degrees Celsius
+     * Source: https://electronics.stackexchange.com/a/356964
+     */
+
+    int16_t raw_temperature =
+            (20 << 3) + (((int16_t) buffer[0] << 8 | buffer[1]) >> 4);
+
+    // Convert the raw temperature data to degrees Celsius
+    float temperature_celsius = (float) raw_temperature / 8.0;
+
+    // Store the result in the temperature array
+    temperature[0] = (int16_t) temperature_celsius;
+}
+
 #endif