From adaa34b1ae657d6a0634934d81d15b63bb01c468 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Fri, 24 Nov 2023 09:39:23 +0800 Subject: [PATCH] refactor(Code Cleanup): Modified code structure --- frtos/config/line_sensor_config.h | 10 +-- frtos/line_sensor/line_sensor_test.c | 3 +- frtos/magnetometer/magnetometer_direction.h | 29 ++++--- frtos/magnetometer/magnetometer_test.c | 16 ++-- frtos/magnetometer/map.h | 85 +++++++++++++-------- 5 files changed, 83 insertions(+), 60 deletions(-) diff --git a/frtos/config/line_sensor_config.h b/frtos/config/line_sensor_config.h index d40bb60..ad70b8b 100644 --- a/frtos/config/line_sensor_config.h +++ b/frtos/config/line_sensor_config.h @@ -3,10 +3,10 @@ /* ADC Configuration */ -#define LINE_SENSOR_READ_DELAY ( 100 ) +#define LINE_SENSOR_READ_DELAY (100) -#define LEFT_SENSOR_PIN ( 26 ) -#define RIGHT_SENSOR_PIN ( 27 ) -#define BARCODE_SENSOR_PIN ( 22 ) +#define LEFT_SENSOR_PIN (26) +#define RIGHT_SENSOR_PIN (27) +#define BARCODE_SENSOR_PIN (22) -#endif //CONFIG_H +#endif // CONFIG_H diff --git a/frtos/line_sensor/line_sensor_test.c b/frtos/line_sensor/line_sensor_test.c index a712c51..387794e 100644 --- a/frtos/line_sensor/line_sensor_test.c +++ b/frtos/line_sensor/line_sensor_test.c @@ -2,7 +2,6 @@ #include "line_sensor_init.h" #include "car_config.h" - int main(void) { @@ -10,7 +9,7 @@ main(void) obs_t obs; - car_struct_t car_struct = {.obs = &obs}; + car_struct_t car_struct = { .obs = &obs }; sleep_ms(2000); diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index 19b609e..71e8163 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -9,11 +9,15 @@ * The yaw is calculated using the magnetometer and accelerometer data * The roll, pitch and yaw are combined to calculate the direction * of the car with a complementary filter and compensating for the - * temperature. + * temperature. (existing) * * The complementary filter is used to combine the accelerometer * and magnetometer data (yaw) to calculate the direction of the car * + * Complementary Filter was used previously, but it was not accurate + * enough. The yaw calculated from the magnetometer data was used + * instead. + * * Source: * https://www.nxp.com/docs/en/application-note/AN3461.pdf * https://ahrs.readthedocs.io/en/latest/filters/complementary.html @@ -80,7 +84,8 @@ calculate_yaw_magnetometer(int16_t magnetometer[3]) * @return Compensated Yaw */ float -compensate_magnetometer(float yaw_mag, int16_t temperature) { +compensate_magnetometer(float yaw_mag, int16_t temperature) +{ // Calculate temperature difference from the reference temperature uint delta_temp = temperature - TEMPERATURE_OFFSET; @@ -190,11 +195,11 @@ calculate_compass_direction(float yaw) * @param compass_direction Compass Direction */ static inline void -update_orientation_data(float roll, - float pitch, - float yaw, - compass_direction_t compass_direction, - volatile direction_t *g_direction) +update_orientation_data(float roll, + float pitch, + float yaw, + compass_direction_t compass_direction, + volatile direction_t *g_direction) { g_direction->roll = roll; g_direction->roll_angle = (roll > 0) ? LEFT : RIGHT; @@ -211,8 +216,8 @@ update_orientation_data(float roll, * @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], volatile direction_t *g_direction) { @@ -301,7 +306,7 @@ print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) } void -updateDirection(volatile direction_t * g_direction) +updateDirection(volatile direction_t *g_direction) { int16_t magnetometer[3]; int16_t accelerometer[3]; @@ -323,7 +328,7 @@ void monitor_direction_task(void *pvParameters) { volatile direction_t *p_direction = NULL; - p_direction = (direction_t *) pvParameters; + p_direction = (direction_t *)pvParameters; for (;;) { @@ -339,7 +344,7 @@ magnetometer_tasks_init(car_struct_t *car_struct) xTaskCreate(monitor_direction_task, "Direction Task", configMINIMAL_STACK_SIZE, - (void *) car_struct->p_direction, + (void *)car_struct->p_direction, PRIO, &h_direction_task); } diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c index 703bbbe..26f29d7 100644 --- a/frtos/magnetometer/magnetometer_test.c +++ b/frtos/magnetometer/magnetometer_test.c @@ -3,7 +3,7 @@ #include "magnetometer_direction.h" #include "map.h" -#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL) +#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL) void launch() @@ -21,16 +21,16 @@ launch() } int -main (void) +main(void) { stdio_usb_init(); direction_t direction; - car_struct_t car_struct = {.p_direction = &direction}; + car_struct_t car_struct = { .p_direction = &direction }; - int grid_rows = 10; // Define the number of rows in your grid - int grid_cols = 10; // Define the number of columns in your grid + 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); @@ -39,13 +39,13 @@ main (void) magnetometer_init(&car_struct); -// printf("Magnetometer initialized!\n"); + // printf("Magnetometer initialized!\n"); magnetometer_tasks_init(&car_struct); vTaskStartScheduler(); -// launch(); + // launch(); - return(0); + return (0); } \ No newline at end of file diff --git a/frtos/magnetometer/map.h b/frtos/magnetometer/map.h index 2737233..efe3c5d 100644 --- a/frtos/magnetometer/map.h +++ b/frtos/magnetometer/map.h @@ -9,29 +9,33 @@ #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 +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 * +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) + 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) } } @@ -39,23 +43,32 @@ Grid *create_grid(int rows, int cols) { } // 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) { +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) { +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 + 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++) { +void +destroy_grid(Grid *grid) +{ + for (int i = 0; i < grid->rows; i++) + { free(grid->data[i]); } free(grid->data); @@ -64,12 +77,15 @@ void destroy_grid(Grid *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) { +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) { + switch (orientation) + { case NORTH: offset_y = 1; break; @@ -105,24 +121,27 @@ void update_map(int orientation, int cur_x, int cur_y) { mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y); } - // Function to print the map -void print_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++) { + 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; -// } + // case false: + // printf("0 "); + // break; + // case true: + // printf("1 "); + // break; + // } } printf("\n"); } } -#endif //TEST_PROJECT_MAP_H +#endif // TEST_PROJECT_MAP_H