Merge pull request #11 from Devoalda/main

Magnetometer Accuracy Fix

frtos/config/magnetometer_config.h

@@ -7,14 +7,19 @@
 
 #define DIRECTION_READ_DELAY                    ( 100 )
 
-#define ALPHA                                   ( 0.01f ) // Complementary
+#define NUM_READINGS                            ( 10 ) // Number of readings to
-                                                          // Filter Constant
+                                                        // take before
+                                                        // calculating
+                                                        // direction
+
+//#define ALPHA                                   ( 0.1f ) // Low Pass Filter
+                                                          // Coefficient
 
 // LSM303DLHC temperature compensation coefficients
 #define SCALE_Z                                 ( 1.0f ) // Scale for Z-axis
-#define OFFSET_Z                                ( 3.0f ) // Offset for Z-axis
+#define OFFSET_Z                                ( 0.0f ) // Offset for Z-axis
 
-#define TEMPERATURE_OFFSET                      ( 25.0f ) // Reference
+#define TEMPERATURE_OFFSET                      ( 32.0f ) // Reference
                                                           // temperature for
                                                           // calibration
 

frtos/magnetometer/magnetometer_direction.h

@@ -24,6 +24,7 @@
 #define MAGNETOMETER_DIRECTION_H
 
 #include "magnetometer_init.h"
+#include "map.h"
 
 /**
  * @brief Roll Calculation with Accelerometer Data
@@ -55,7 +56,7 @@ calculate_pitch(int16_t acceleration[3]) {
  */
 static inline float
 calculate_yaw_magnetometer(int16_t magnetometer[3]) {
-    return atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
+    return atan2(magnetometer[1], magnetometer[0]) * (180.0f / M_PI);
 }
 
 /**
@@ -64,10 +65,10 @@ calculate_yaw_magnetometer(int16_t magnetometer[3]) {
  * @param yaw_mag   Yaw calculated from Magnetometer Data
  * @return yaw      Yaw calculated from Complementary Filter
  */
-static inline float
+//static inline float
-calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
+//calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
-    return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
+//    return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
-}
+//}
 
 /**
  * @brief Compensate the magnetometer readings for temperature
@@ -78,11 +79,11 @@ calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
 float
 compensate_magnetometer(float yaw_mag, int16_t temperature) {
     // Calculate temperature difference from the reference temperature
-    float delta_temp = (float) (temperature - TEMPERATURE_OFFSET);
+    uint delta_temp = temperature - TEMPERATURE_OFFSET;
 
     // Apply temperature compensation to each axis using macros
     float compensated_yaw_mag =
-            yaw_mag - (delta_temp * TEMPERATURE_COEFFICIENT_Z);
+            yaw_mag - ((float) delta_temp * TEMPERATURE_COEFFICIENT_Z);
 
     // Apply scale and offset corrections using macros
     compensated_yaw_mag = (compensated_yaw_mag - OFFSET_Z) * SCALE_Z;
@@ -97,7 +98,12 @@ compensate_magnetometer(float yaw_mag, int16_t temperature) {
  */
 static inline float
 adjust_yaw(float yaw) {
-    return (yaw < 0) ? yaw + 360.0f : yaw;
+    if (yaw < 0)
+    {
+        yaw += 360;
+    }
+
+    return yaw;
 }
 
 /**
@@ -106,33 +112,86 @@ adjust_yaw(float yaw) {
  * 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
+ * @param yaw   Yaw calculated
  * @return      Compass Direction
  */
 static inline compass_direction_t
 calculate_compass_direction(float yaw) {
-    int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions
+    if (yaw >= 337.5 || yaw < 22.5)
-    switch (orientation)
     {
-        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;
     }
+    else
+    {
+        if (yaw >= 22.5 && yaw < 67.5)
+        {
+            return NORTH_EAST;
+        }
+        else
+        {
+            if (yaw >= 67.5 && yaw < 112.5)
+            {
+                return EAST;
+            }
+            else
+            {
+                if (yaw >= 112.5 && yaw < 157.5)
+                {
+                    return SOUTH_EAST;
+                }
+                else
+                {
+                    if (yaw >= 157.5 && yaw < 202.5)
+                    {
+                        return SOUTH;
+                    }
+                    else
+                    {
+                        if (yaw >= 202.5 && yaw < 247.5)
+                        {
+                            return SOUTH_WEST;
+                        }
+                        else
+                        {
+                            if (yaw >= 247.5 && yaw < 292.5)
+                            {
+                                return WEST;
+                            }
+                            else
+                            {
+                                if (yaw >= 292.5 && yaw < 337.5)
+                                {
+                                    return NORTH_WEST;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+//    int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions
+//    switch (orientation)
+//    {
+//        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;
+//    }
 }
 
 /**
@@ -161,26 +220,33 @@ update_orientation_data(float roll, float pitch, float yaw,
  * @param magnetometer  Magnetometer Data
  */
 static void
-read_direction(int16_t acceleration[3],
+read_direction(int16_t acceleration[3], int16_t magnetometer[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);
 
+    yaw_mag = adjust_yaw(yaw_mag);
+
     // Apply temperature compensation to the magnetometer data
-    float compensated_mag_yaw = compensate_magnetometer(yaw_mag,
+//    float compensated_mag_yaw = compensate_magnetometer(yaw_mag,
-                                                        temperature[0]);
+//                                                        temperature[0]);
+//    compensated_mag_yaw = adjust_yaw(compensated_mag_yaw);
 
-    float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI);
+//    float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0f / M_PI);
-    float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw);
+//    yaw_acc = adjust_yaw(yaw_acc);
+//
+//    float yaw = calculate_yaw_complementary(yaw_acc, yaw_mag);
 
-    yaw = adjust_yaw(yaw);
+//    yaw = adjust_yaw(yaw);
+//    printf("Yaw: %f\n", yaw);
 
-    compass_direction_t compass_direction = calculate_compass_direction(yaw);
+    compass_direction_t compass_direction = calculate_compass_direction(yaw_mag);
 
-    update_orientation_data(roll, pitch, yaw, compass_direction);
+    update_orientation_data(roll,
+                            pitch,
+                            yaw_mag,
+                            compass_direction);
 }
 
 /**
@@ -192,7 +258,8 @@ read_direction(int16_t acceleration[3],
  * @param params
  */
 void print_orientation_data() {
-    printf("Roll: %f, Pitch: %f, Yaw: %f\n",
+//    printf("Roll: %f, Pitch: %f, Yaw: %f\n",
+           printf("%f %f %f\n",
            g_direction.roll,
            g_direction.pitch,
            g_direction.yaw
@@ -251,34 +318,80 @@ void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) {
     }
 }
 
-void monitor_direction_task(__unused void *params) {
+void updateDirection() {
-    for (;;)
-    {
-        if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
-        {
     int16_t magnetometer[3];
     int16_t accelerometer[3];
     int16_t temperature[1];
 
+    static int cur_x = 0;
+    static int cur_y = 0;
+
     read_magnetometer(magnetometer);
     read_accelerometer(accelerometer);
     read_temperature(temperature);
 
-            read_direction(accelerometer, magnetometer, temperature);
+    read_direction(accelerometer, magnetometer);
 
     // Temperature in degrees Celsius
-            printf("Temperature: %d\n", temperature[0]);
+//    printf("Temperature: %d\n", temperature[0]);
 
     print_orientation_data();
 
-            printf("Direction: ");
+//    printf("Direction: ");
 
-            print_direction(g_direction.orientation);
+//    print_direction(g_direction.orientation);
 
-            print_roll_and_pitch(g_direction.roll_angle,
+    switch (g_direction.orientation)
-                                 g_direction.pitch_angle);
+    {
+        case NORTH:
+            cur_y ++;
+            break;
+        case EAST:
+            cur_x ++;
+            break;
+        case SOUTH:
+            cur_y --;
+            break;
+        case WEST:
+            cur_x --;
+            break;
+        case NORTH_EAST:
+            cur_x ++;
+            cur_y ++;
+            break;
+        case SOUTH_EAST:
+            cur_x ++;
+            cur_y --;
+            break;
+        case SOUTH_WEST:
+            cur_x --;
+            cur_y --;
+            break;
+        case NORTH_WEST:
+            cur_x --;
+            cur_y ++;
+            break;
+    }
+
+    // Update the map based on the direction of the car (N, E, S, W)
+//    update_map(g_direction.orientation, cur_x, cur_y);
+
+//    printf("Current Position: (%d, %d)\n", cur_x, cur_y);
+//     print_map();
+
+//    print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle);
+}
+
+
+void monitor_direction_task(__unused void *params) {
+    for (;;)
+    {
+        if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
+        {
+            updateDirection();
         }
     }
 }
 
+
 #endif

frtos/magnetometer/magnetometer_init.h

@@ -32,7 +32,6 @@ SemaphoreHandle_t g_direction_sem = NULL;
 direction_t g_direction = {
         .roll = 0,
         .pitch = 0,
-//        .heading = 0,
         .yaw = 0,
         .orientation = NORTH,
         .roll_angle = LEFT,

frtos/magnetometer/magnetometer_read.h

@@ -58,13 +58,17 @@ read_accelerometer(int16_t accelerometer[3]) {
 }
 
 /**
- * @brief Read Magnetometer Data
+ * @brief Read Magnetometer Data with Moving Average
  * @param magnetometer  Magnetometer Data
  */
 static inline void
 read_magnetometer(int16_t magnetometer[3]) {
     uint8_t buffer[6];
+    int32_t xMagFiltered = 0;
+    int32_t yMagFiltered = 0;
+    int32_t zMagFiltered = 0;
 
+    for (int i = 0; i < NUM_READINGS; i++) {
         buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
         buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M);
         buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M);
@@ -72,11 +76,16 @@ read_magnetometer(int16_t magnetometer[3]) {
         buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M);
         buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M);
 
-    magnetometer[0] = (int16_t) (buffer[0] << 8 | buffer[1]); //xMag
+        // Update the cumulative sum of the magnetometer data
+        xMagFiltered += (int16_t)(buffer[0] << 8 | buffer[1]);
+        yMagFiltered += (int16_t)(buffer[2] << 8 | buffer[3]);
+        zMagFiltered += (int16_t)(buffer[4] << 8 | buffer[5]);
+    }
 
-    magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag
+    // Calculate the moving average
-
+    magnetometer[0] = xMagFiltered / NUM_READINGS;
-    magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
+    magnetometer[1] = yMagFiltered / NUM_READINGS;
+    magnetometer[2] = zMagFiltered / NUM_READINGS;
 }
 
 /**

frtos/magnetometer/magnetometer_test.c

@@ -2,6 +2,7 @@
 #include "magnetometer_init.h"
 #include "magnetometer_read.h"
 #include "magnetometer_direction.h"
+#include "map.h"
 
 #define DIRECTION_TASK_PRIORITY             (tskIDLE_PRIORITY + 1UL)
 
@@ -30,9 +31,11 @@ main (void)
 {
     stdio_usb_init();
 
-//    sleep_ms(2000);
+    int grid_rows = 10;  // Define the number of rows in your grid
+    int grid_cols = 10;  // Define the number of columns in your grid
+
+    car_path_grid = create_grid(grid_rows, grid_cols);
 
-//    printf("Test started!\n");
     magnetometer_init();
 
     launch();

frtos/magnetometer/map.h

@@ -0,0 +1,128 @@
+//
+// Created by junwei on 31/10/23.
+//
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+
+#ifndef TEST_PROJECT_MAP_H
+#define TEST_PROJECT_MAP_H
+
+
+// Define the grid structure
+typedef struct {
+    bool **data;  // 2D array to represent the grid
+    int rows;     // Number of rows in the grid
+    int cols;     // Number of columns in the grid
+} Grid;
+
+// Global grid to track the car's path
+Grid *car_path_grid;
+
+// Function to create and initialize a grid
+Grid *create_grid(int rows, int cols) {
+    Grid *grid = (Grid *) malloc(sizeof(Grid));
+    grid->rows = rows;
+    grid->cols = cols;
+
+    // Allocate memory for the 2D array
+    grid->data = (bool **) malloc(rows * sizeof(bool *));
+    for (int i = 0; i < rows; i++) {
+        grid->data[i] = (bool *) malloc(cols * sizeof(bool));
+        for (int j = 0; j < cols; j++) {
+            grid->data[i][j] = false;  // Initialize to 'false' (unvisited)
+        }
+    }
+
+    return grid;
+}
+
+// Function to mark a cell as visited
+void mark_cell(Grid *grid, int row, int col) {
+    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
+        grid->data[row][col] = true;
+    }
+}
+
+// Function to check if a cell has been visited
+bool is_cell_visited(Grid *grid, int row, int col) {
+    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
+        return grid->data[row][col];
+    }
+    return false;  // Consider out-of-bounds as unvisited
+}
+
+// Function to destroy the grid and free memory
+void destroy_grid(Grid *grid) {
+    for (int i = 0; i < grid->rows; i++) {
+        free(grid->data[i]);
+    }
+    free(grid->data);
+    free(grid);
+}
+
+// Function to update the map based on car's current orientation
+// Function to update the map based on car's current orientation and position
+void update_map(int orientation, int cur_x, int cur_y) {
+    // Define offsets for different orientations
+    int offset_x = 0;
+    int offset_y = 0;
+
+    switch (orientation) {
+        case NORTH:
+            offset_y = 1;
+            break;
+        case EAST:
+            offset_x = 1;
+            break;
+        case SOUTH:
+            offset_y = -1;
+            break;
+        case WEST:
+            offset_x = -1;
+            break;
+        case NORTH_EAST:
+            offset_x = 1;
+            offset_y = 1;
+            break;
+        case SOUTH_EAST:
+            offset_x = 1;
+            offset_y = -1;
+            break;
+        case SOUTH_WEST:
+            offset_x = -1;
+            offset_y = -1;
+            break;
+        case NORTH_WEST:
+            offset_x = -1;
+            offset_y = 1;
+            break;
+    }
+
+    // Update the map based on the car's current position and orientation
+    mark_cell(car_path_grid, cur_x, cur_y);
+    mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y);
+}
+
+
+// Function to print the map
+void print_map() {
+    // Invert the map, 0,0 is at the Middle
+    // Print 1 for visited cells and 0 for unvisited cells
+    for (int i = car_path_grid->rows - 1; i >= 0; i--) {
+        for (int j = 0; j < car_path_grid->cols; j++) {
+            (car_path_grid->data[j][i]) ? printf("1 ") : printf("0 ");
+//                case false:
+//                    printf("0 ");
+//                    break;
+//                case true:
+//                    printf("1 ");
+//                    break;
+//            }
+        }
+        printf("\n");
+    }
+}
+
+#endif //TEST_PROJECT_MAP_H