From f933d8ea916871d8cdb7a5a5bd1206b1ec1cbeca Mon Sep 17 00:00:00 2001 From: Devoalda Date: Sat, 4 Nov 2023 13:38:47 +0800 Subject: [PATCH 1/2] fix(Magnetometer): Removed Com Filter --- frtos/config/magnetometer_config.h | 6 +- frtos/magnetometer/magnetometer_direction.h | 220 +++++++++++++++----- frtos/magnetometer/magnetometer_init.h | 1 - frtos/magnetometer/magnetometer_test.c | 7 +- frtos/magnetometer/map.h | 128 ++++++++++++ 5 files changed, 303 insertions(+), 59 deletions(-) create mode 100644 frtos/magnetometer/map.h diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 2996d79..5acb030 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,14 +7,14 @@ #define DIRECTION_READ_DELAY ( 100 ) -#define ALPHA ( 0.01f ) // Complementary +#define ALPHA ( 0.0f ) // Complementary // Filter Constant // 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 diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index c7d7189..b8addee 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/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); } /** @@ -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 - switch (orientation) + if (yaw >= 337.5 || yaw < 22.5) { - 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; + 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; +// } } /** @@ -169,18 +228,27 @@ read_direction(int16_t acceleration[3], 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, - temperature[0]); +// float compensated_mag_yaw = compensate_magnetometer(yaw_mag, +// temperature[0]); +// compensated_mag_yaw = adjust_yaw(compensated_mag_yaw); - float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI); - float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw); +// float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0f / M_PI); +// 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); } /** @@ -251,34 +319,80 @@ void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) { } } +void updateDirection() { + 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); + + // Temperature in degrees Celsius + printf("Temperature: %d\n", temperature[0]); + + print_orientation_data(); + + printf("Direction: "); + + print_direction(g_direction.orientation); + + switch (g_direction.orientation) + { + 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) { - 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); + updateDirection(); } } } + #endif \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index 96ab986..1de1432 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/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, diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c index b509d42..6b137db 100644 --- a/frtos/magnetometer/magnetometer_test.c +++ b/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(); diff --git a/frtos/magnetometer/map.h b/frtos/magnetometer/map.h new file mode 100644 index 0000000..2737233 --- /dev/null +++ b/frtos/magnetometer/map.h @@ -0,0 +1,128 @@ +// +// Created by junwei on 31/10/23. +// + +#include +#include +#include + +#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 From d0553b6ddba9470e912bbde216e029e7cceea9ef Mon Sep 17 00:00:00 2001 From: Devoalda Date: Tue, 7 Nov 2023 10:02:09 +0800 Subject: [PATCH 2/2] fix(Magnetometer): Added filter for magnetometer outliers --- frtos/config/magnetometer_config.h | 9 ++++-- frtos/magnetometer/magnetometer_direction.h | 23 ++++++++------- frtos/magnetometer/magnetometer_read.h | 31 +++++++++++++-------- 3 files changed, 38 insertions(+), 25 deletions(-) diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 5acb030..1128376 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,8 +7,13 @@ #define DIRECTION_READ_DELAY ( 100 ) -#define ALPHA ( 0.0f ) // Complementary - // Filter Constant +#define NUM_READINGS ( 10 ) // Number of readings to + // 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 diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index b8addee..2cabc97 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -65,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 -calculate_yaw_complementary(float yaw_acc, float yaw_mag) { - return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag; -} +//static inline float +//calculate_yaw_complementary(float yaw_acc, float yaw_mag) { +// return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag; +//} /** * @brief Compensate the magnetometer readings for temperature @@ -220,9 +220,7 @@ 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], - int16_t temperature[1]) { +read_direction(int16_t acceleration[3], int16_t magnetometer[3]) { float roll = calculate_roll(acceleration); float pitch = calculate_pitch(acceleration); @@ -260,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 @@ -331,16 +330,16 @@ void updateDirection() { 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); switch (g_direction.orientation) { diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h index fde4b0b..36f3f35 100644 --- a/frtos/magnetometer/magnetometer_read.h +++ b/frtos/magnetometer/magnetometer_read.h @@ -58,25 +58,34 @@ 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; - 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); - buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M); - buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M); - buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M); + 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); + buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M); + 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 - - magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag + // Calculate the moving average + magnetometer[0] = xMagFiltered / NUM_READINGS; + magnetometer[1] = yMagFiltered / NUM_READINGS; + magnetometer[2] = zMagFiltered / NUM_READINGS; } /**