feature(LSM303DLHC Sensor):

Using both Accelerometer and magnetometer to do a complementary filter to calculate the compass direction.
Added more fine-grained directions (NW, NE, SE, SW)

frtos/config/magnetometer_config.h

@@ -5,7 +5,47 @@
 #define I2C_SDA                                 ( 8 )
 #define I2C_SCL                                 ( 9 )
 
-#define MAGNETOMETER_READ_DELAY                 ( 1000 )
+#define DIRECTION_READ_DELAY                    ( 100 )
-#define ACCELEROMETER_READ_DELAY                ( 1000 )
+
+/**
+ * @brief The orientation of the car
+ */
+
+typedef enum {
+    NORTH,
+    NORTH_EAST,
+    EAST,
+    SOUTH_EAST,
+    SOUTH,
+    SOUTH_WEST,
+    WEST,
+    NORTH_WEST
+} compass_direction_t;
+
+/**
+ * Angle of the car
+ */
+typedef enum {
+    UP = 0,
+    DOWN = 1,
+    LEFT = 2,
+    RIGHT = 3
+} angle_t;
+
+/**
+ * @brief The direction of the car
+ * roll = angle of the car (left or right)
+ * pitch = angle of the car (up or down)
+ * heading = direction of the car (north, east, south, west) in degrees
+ * orientation = orientation of the car (north, east, south, west)
+ */
+typedef struct {
+    float roll;
+    float pitch;
+    float yaw;
+    compass_direction_t orientation;
+    angle_t roll_angle;
+    angle_t pitch_angle;
+} direction_t;
 
 #endif

frtos/magnetometer/magnetometer_direction.h

@@ -1,59 +1,153 @@
+/**
+ * @file    magnetometer_direction.h
+ * @author  Woon Jun Wei
+ * @brief   This file contains the functions to calculate the direction of
+ *          the car using the accelerometer and magnetometer data
+ *
+ * @details The direction of the car is calculated using the roll, pitch and yaw
+ *          The roll and pitch are calculated using the accelerometer data
+ *          The yaw is calculated using the magnetometer data
+ *          The roll, pitch and yaw are combined to calculate the direction
+ *          of the car
+ *
+ *          The direction of the car is calculated using the complementary filter
+ *          The complementary filter is used to combine the accelerometer
+ *          and magnetometer data to calculate the direction of the car
+ *
+ */
+
+#ifndef MAGNETOMETER_DIRECTION_H
+#define MAGNETOMETER_DIRECTION_H
+
 #include "magnetometer_init.h"
 
-static char
+/**
-*direction(int16_t acceleration[3], int16_t magnetometer[3]) {
+ * @brief Roll Calculation with Accelerometer Data
-    // Calculate the angle from accelerometer data
+ * @param acceleration Accelerometer Data
-    float roll = atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
+ * @return
-    float pitch = atan2(- acceleration[0],
+ */
-                        sqrt(acceleration[1] * acceleration[1] +
+float calculate_roll(int16_t acceleration[3]) {
-                             acceleration[2] * acceleration[2])) *
+    return atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
-                  (180.0 / M_PI);
+}
 
-    // Calculate the heading from magnetometer data
+/**
-    float heading = atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
+ * @brief Pitch Calculation with Accelerometer Data
+ * @param acceleration Accelerometer Data
+ * @return
+ */
+float calculate_pitch(int16_t acceleration[3]) {
+    return atan2(- acceleration[0], sqrt(acceleration[1] * acceleration[1] +
+                                         acceleration[2] * acceleration[2])) *
+           (180.0 / M_PI);
+}
 
-    // Adjust the heading for negative values
+/**
-    if (heading < 0)
+ * @brief Yaw Calculation with Magnetometer Data
+ * @param magnetometer Magnetometer Data
+ * @return
+ */
+float calculate_yaw_magnetometer(int16_t magnetometer[3]) {
+    return atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
+}
+
+/**
+ * @brief Complementary Filter for Yaw
+ * @param alpha     Complementary Filter Constant
+ * @param yaw_acc   Yaw calculated from Accelerometer Data
+ * @param yaw_mag   Yaw calculated from Magnetometer Data
+ * @return yaw      Yaw calculated from Complementary Filter
+ */
+float calculate_yaw_complementary(float alpha, float yaw_acc, float yaw_mag) {
+    return alpha * yaw_acc + (1 - alpha) * yaw_mag;
+}
+
+/**
+ * @brief Adjust Yaw to be between 0 and 360 degrees
+ * @param yaw   Yaw calculated from Complementary Filter
+ * @return yaw  Yaw adjusted to be between 0 and 360 degrees
+ */
+float adjust_yaw(float yaw) {
+    return (yaw < 0) ? yaw + 360.0f : 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
+ * @param yaw   Yaw calculated from Complementary Filter
+ * @return      Compass Direction
+ */
+compass_direction_t calculate_compass_direction(float yaw) {
+    int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions
+    switch (orientation)
     {
-        heading += 360.0;
+        case 0:
+            return NORTH;
+        case 1:
+            return NORTH_EAST;
+        case 2:
+            return EAST;
+        case 3:
+            return SOUTH_EAST;
+        case 4:
+            return SOUTH;
+        case 5:
+            return SOUTH_WEST;
+        case 6:
+            return WEST;
+        case 7:
+            return NORTH_WEST;
+        default:
+            return NORTH;
     }
+}
 
-    // Determine the direction based on the heading
+/**
-    // TODO: Optimize this
+ * @brief Update the Orientation Data
-    char *dir;
+ * @param roll                  Roll calculated from Accelerometer Data
+ * @param pitch                 Pitch calculated from Accelerometer Data
+ * @param yaw                   Yaw calculated from Complementary Filter
+ * @param compass_direction     Compass Direction
+ */
+void update_orientation_data(float roll, float pitch, float yaw,
+                             compass_direction_t compass_direction) {
+    g_direction.roll = roll;
+    g_direction.pitch = pitch;
+    g_direction.yaw = yaw;
+    g_direction.orientation = compass_direction;
+}
 
-    if (heading >= 315 || heading < 45)
+/**
-    {
+ * @brief Read the Accelerometer and Magnetometer Data and
-        dir = "North";
+ *       Calculate the Direction of the Car
-    }
+ * @details Alpha is set to 0.98 to give more weight to the accelerometer data
-    else
+ * @param acceleration  Accelerometer Data
-    {
+ * @param magnetometer  Magnetometer Data
-        if (heading >= 45 && heading < 135)
+ */
-        {
+static void read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
-            dir = "East";
+    float roll = calculate_roll(acceleration);
-        }
+    float pitch = calculate_pitch(acceleration);
-        else
+    float yaw_mag = calculate_yaw_magnetometer(magnetometer);
-        {
-            if (heading >= 135 && heading < 225)
-            {
-                dir = "South";
-            }
-            else
-            {
-                dir = "West";
-            }
-        }
-    }
 
-    printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading);
+    float alpha = 0.98;
+    float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI);
+    float yaw = calculate_yaw_complementary(alpha, yaw_acc, yaw_mag);
 
-    return dir;
+    yaw = adjust_yaw(yaw);
+
+    compass_direction_t compass_direction = calculate_compass_direction(yaw);
+
+    update_orientation_data(roll, pitch, yaw, compass_direction);
 }
 
 /**
  * FreeRTOS Tasks
  */
 
+/**
+ * @brief Task to Monitor the Direction of the Car
+ * @param params
+ */
 void
 monitor_direction_task(__unused void *params) {
     for (;;)
@@ -67,53 +161,63 @@ monitor_direction_task(__unused void *params) {
             read_magnetometer(magnetometer);
             read_accelerometer(accelerometer);
 
-            // Calculate the angle from accelerometer data
+            read_direction(accelerometer, magnetometer);
-            float roll =
-                    atan2(accelerometer[1], accelerometer[2]) * (180.0 / M_PI);
-            float pitch = atan2(- accelerometer[0],
-                                sqrt(accelerometer[1] * accelerometer[1] +
-                                     accelerometer[2] * accelerometer[2])) *
-                          (180.0 / M_PI);
 
-            // Calculate the heading from magnetometer data
+            printf("Roll: %f, Pitch: %f, Yaw: %f\n",
-            float heading =
+                   g_direction.roll, g_direction.pitch, g_direction.yaw);
-                    atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
 
-            // Adjust the heading for negative values
+            printf("Direction:");
-            if (heading < 0)
+
+            switch (g_direction.orientation)
             {
-                heading += 360.0;
+                case NORTH:
+                    printf("North\n");
+                    break;
+                case NORTH_EAST:
+                    printf("North East\n");
+                    break;
+                case EAST:
+                    printf("East\n");
+                    break;
+                case SOUTH_EAST:
+                    printf("South East\n");
+                    break;
+                case SOUTH:
+                    printf("South\n");
+                    break;
+                case SOUTH_WEST:
+                    printf("South West\n");
+                    break;
+                case WEST:
+                    printf("West\n");
+                    break;
+                case NORTH_WEST:
+                    printf("North West\n");
+                    break;
             }
 
-            // Determine the direction based on the heading
+            switch (g_direction.roll_angle)
-            char *dir;
-
-            if (heading >= 315 || heading < 45)
             {
-                dir = "North";
+                case LEFT:
-            }
+                    printf("You're Flying!\n");
-            else
+                    break;
-            {
+                case RIGHT:
-                if (heading >= 45 && heading < 135)
+                    printf("You're Plunging!\n");
-                {
+                    break;
-                    dir = "East";
-                }
-                else
-                {
-                    if (heading >= 135 && heading < 225)
-                    {
-                        dir = "South";
-                    }
-                    else
-                    {
-                        dir = "West";
-                    }
-                }
             }
 
-            printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading);
+            switch (g_direction.pitch_angle)
+            {
+                case UP:
+                    printf("Your left wheel is in the air!\n");
+                    break;
+                case DOWN:
+                    printf("Your right wheel is in the air!\n");
+                    break;
+            }
 
-            printf("Direction: %s\n", dir);
         }
     }
 }
+
+#endif

frtos/magnetometer/magnetometer_init.h

@@ -1,10 +1,12 @@
 /**
- * @file magnetometer_init.h
+ * @file    magnetometer_init.h
- * @brief Initialise the magnetometer sensor and calculate the direction of the car
+ * @author  Woon Jun Wei
- * @details This file contains the function prototypes for the magnetometer sensor
+ * @brief   Initialise the magnetometer sensor and
- *         and the function to calculate the direction of the car
+ *          calculate the direction of the car
- *         based on the magnetometer sensor data
+ *
- * @author Woon Jun Wei
+ * @details This file contains the function prototypes for the
+ *          magnetometer sensor and the function to calculate
+ *          the direction of the car based on the magnetometer sensor data
  */
 
 #ifndef MAGNETOMETER_INIT_H
@@ -25,16 +27,31 @@
 #include "LSM303DLHC_register.h"
 
 // Semaphores
-//SemaphoreHandle_t g_magnetometer_sem = NULL;
-//SemaphoreHandle_t g_accelerometer_sem = NULL;
 SemaphoreHandle_t g_direction_sem = NULL;
 
-// Message Buffers
+direction_t g_direction = {
-//MessageBufferHandle_t g_magnetometer_buffer = NULL;
+        .roll = 0,
-//MessageBufferHandle_t g_accelerometer_buffer = NULL;
+        .pitch = 0,
+//        .heading = 0,
+        .yaw = 0,
+        .orientation = NORTH,
+        .roll_angle = LEFT,
+        .pitch_angle = UP
+};
 
+/**
+ * @brief   Initialise the LSM303DLHC sensor (Accelerometer and Magnetometer)
+ * @details
+ *          Accelerometer - Normal power mode, all axes enabled, 10 Hz,
+ *          Full Scale +-2g, continuous update
+ *
+ *          Magnetometer - Continuous-conversion mode, Gain = +/- 1.3,
+ *          Enable temperature sensor, 220 Hz
+ *
+ * @return None
+ */
 static void
-lsm303_reset() {
+LSM303DLHC_init() {
     /**
      * Accelerometer Setup
      */
@@ -59,22 +76,23 @@ lsm303_reset() {
     buf[1] = 0x00;
     i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
 
-    // CRA_REG_M (0x00), 15 Hz (0x10 -> 00010000)
-//    buf[0] = LSM303_CRA_REG_M;
-//    buf[1] = 0x10;
-//    i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
-
     // CRB_REG_M (0x01) - Gain = +/- 1.3 (0x20 -> 00100000)
     buf[0] = LSM303_CRB_REG_M;
     buf[1] = 0x20;
     i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
 
     // CRA_REG_M (0x00), 0x9C = 0b1001 1100
+    // Enable temperature sensor (0x80 -> 1000 0000)
+    // 220 Hz (0x1C -> 0001 1100)
     buf[0] = LSM303_CRA_REG_M;
     buf[1] = 0x9C;
     i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
 }
 
+/**
+ * @brief Initialise the Magnetometer Sensor
+ * @details Initialise the I2C Port, SDA and SCL Pins, and the LSM303DLHC Sensor
+ */
 void
 magnetometer_init()
 {
@@ -85,50 +103,12 @@ magnetometer_init()
     gpio_pull_up(I2C_SDA);
     gpio_pull_up(I2C_SCL);
 
-    lsm303_reset();
+    LSM303DLHC_init();
 
     // Semaphore
-//    g_magnetometer_sem = xSemaphoreCreateBinary();
-//    g_accelerometer_sem = xSemaphoreCreateBinary();
     g_direction_sem = xSemaphoreCreateBinary();
-
-    // Message Buffers
-//    g_magnetometer_buffer = xMessageBufferCreate(sizeof(int16_t) * 3);
-//    g_accelerometer_buffer = xMessageBufferCreate(sizeof(int16_t) * 3);
-
-    printf("Magnetometer initialised\n");
 }
 
-/**
- * @brief Timer Interrupt Handler for the magnetometer
- * @param repeatingTimer The timer handler
- * @return True (To keep the timer running)
- */
-//bool
-//h_magnetometer_timer_handler(repeating_timer_t *repeatingTimer) {
-//
-//    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-//    xSemaphoreGiveFromISR(g_magnetometer_sem,
-//                          &xHigherPriorityTaskWoken);
-//    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
-//    return true;
-//}
-
-/**
- * @brief Timer Interrupt Handler for the accelerometer
- * @param repeatingTimer The timer handler
- * @return True (To keep the timer running)
- */
-//bool
-//h_accelerometer_timer_handler(repeating_timer_t *repeatingTimer) {
-//
-//    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-//    xSemaphoreGiveFromISR(g_accelerometer_sem,
-//                          &xHigherPriorityTaskWoken);
-//    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
-//    return true;
-//}
-
 /**
  * @brief Timer Interrupt Handler To calculate the direction of the car
  * @param repeatingTimer The timer handler

frtos/magnetometer/magnetometer_read.h

@@ -1,5 +1,20 @@
+/**
+ * @file magnetometer_read.h
+ * @author Woon Jun Wei
+ * @brief This file contains the functions to read the data
+ * from the LSM303DLHC accelerometer and magnetometer sensor
+ */
+
+#ifndef MAGNETOMETER_READ_H
+#define MAGNETOMETER_READ_H
 #include "magnetometer_init.h"
 
+/**
+ * @brief Read Data with I2C, given the address and register
+ * @param addr  Address of the device
+ * @param reg   Register to read from
+ * @return      1 piece of data read from the register
+ */
 static inline int
 read_data(uint8_t addr, uint8_t reg) {
     uint8_t data[1];
@@ -13,6 +28,10 @@ read_data(uint8_t addr, uint8_t reg) {
     return data[0];
 }
 
+/**
+ * @brief Read Accelerometer Data
+ * @param accelerometer   Accelerometer Data
+ */
 static inline void
 read_accelerometer(int16_t accelerometer[3]) {
     uint8_t buffer[6];
@@ -37,6 +56,10 @@ read_accelerometer(int16_t accelerometer[3]) {
 
 }
 
+/**
+ * @brief Read Magnetometer Data
+ * @param magnetometer  Magnetometer Data
+ */
 static inline void
 read_magnetometer(int16_t magnetometer[3]) {
     uint8_t buffer[6];
@@ -55,52 +78,4 @@ read_magnetometer(int16_t magnetometer[3]) {
     magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
 }
 
-/**
+#endif
- * FreeRTOS Tasks
- */
-
-
-//void
-//monitor_magnetometer_task(__unused void *params) {
-//    for (;;)
-//    {
-//        if (xSemaphoreTake(g_magnetometer_sem, portMAX_DELAY) == pdTRUE)
-//        {
-////            printf("Magnetometer Task");
-//            int16_t magnetometer[3];
-//            read_magnetometer(magnetometer);
-//
-//            // Send to message buffer
-//            xMessageBufferSend(g_magnetometer_buffer,
-//                               &magnetometer,
-//                               sizeof(magnetometer),
-//                               0
-//            );
-//
-//            printf("Magnetometer: %d, %d, %d\n", magnetometer[0],
-//                   magnetometer[1], magnetometer[2]);
-//        }
-//    }
-//}
-
-//void
-//monitor_accelerometer_task(__unused void *params) {
-//    for (;;)
-//    {
-//        if (xSemaphoreTake(g_accelerometer_sem, portMAX_DELAY) == pdTRUE)
-//        {
-//            int16_t accelerometer[3];
-//            read_accelerometer(accelerometer);
-//
-//            // Send to message buffer
-//            xMessageBufferSend(g_accelerometer_buffer,
-//                               &accelerometer,
-//                               sizeof(accelerometer),
-//                               0
-//            );
-//
-//            printf("Accelerometer: %d, %d, %d\n", accelerometer[0],
-//                   accelerometer[1], accelerometer[2]);
-//        }
-//    }
-//}

frtos/magnetometer/magnetometer_test.c

@@ -3,47 +3,17 @@
 #include "magnetometer_read.h"
 #include "magnetometer_direction.h"
 
-//#define READ_MAGNETOMETER_PRIORITY          (tskIDLE_PRIORITY + 2UL)
-//#define READ_ACCELEROMETER_PRIORITY         (tskIDLE_PRIORITY + 3UL)
 #define DIRECTION_TASK_PRIORITY             (tskIDLE_PRIORITY + 1UL)
 
 void
 launch()
 {
     struct repeating_timer g_direction_timer;
-    add_repeating_timer_ms(1000,
+    add_repeating_timer_ms(DIRECTION_READ_DELAY,
                            h_direction_timer_handler,
                            NULL,
                            &g_direction_timer);
 
-//    struct repeating_timer g_magnetometer_timer;
-//    add_repeating_timer_ms(MAGNETOMETER_READ_DELAY,
-//                           h_magnetometer_timer_handler,
-//                           NULL,
-//                           &g_magnetometer_timer);
-//
-//    struct repeating_timer g_accelerometer_timer;
-//    add_repeating_timer_ms(ACCELEROMETER_READ_DELAY,
-//                           h_accelerometer_timer_handler,
-//                           NULL,
-//                           &g_accelerometer_timer);
-
-//    TaskHandle_t h_monitor_magnetometer_task = NULL;
-//    xTaskCreate(monitor_magnetometer_task,
-//                "Monitor Magnetometer Task",
-//                configMINIMAL_STACK_SIZE,
-//                NULL,
-//                READ_MAGNETOMETER_PRIORITY,
-//                &h_monitor_magnetometer_task);
-
-//    TaskHandle_t h_monitor_accelerometer_task = NULL;
-//    xTaskCreate(monitor_accelerometer_task,
-//                "Monitor Accelerometer Task",
-//                configMINIMAL_STACK_SIZE,
-//                NULL,
-//                READ_ACCELEROMETER_PRIORITY,
-//                &h_monitor_accelerometer_task);
-
     TaskHandle_t h_monitor_direction_task = NULL;
     xTaskCreate(monitor_direction_task,
                 "Monitor Direction Task",