diff --git a/frtos/config/car_config.h b/frtos/config/car_config.h index 6e1a9de..98bc088 100644 --- a/frtos/config/car_config.h +++ b/frtos/config/car_config.h @@ -21,6 +21,7 @@ typedef struct motor_t *p_left_motor; motor_t *p_right_motor; motor_pid_t *p_pid; + direction_t *p_direction; } car_struct_t; diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 1128376..4aeb752 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -1,37 +1,40 @@ #ifndef MAGNETOMETER_CONFIG_H #define MAGNETOMETER_CONFIG_H -#define I2C_PORT i2c0 -#define I2C_SDA ( 8 ) -#define I2C_SCL ( 9 ) +#define I2C_PORT i2c0 +#define I2C_SDA (8) +#define I2C_SCL (9) -#define DIRECTION_READ_DELAY ( 100 ) +#define DIRECTION_READ_DELAY (100) #define NUM_READINGS ( 10 ) // Number of readings to // take before // calculating // direction -//#define ALPHA ( 0.1f ) // Low Pass Filter - // Coefficient +// #define ALPHA ( 0.1f ) // Low Pass Filter +// Coefficient // LSM303DLHC temperature compensation coefficients -#define SCALE_Z ( 1.0f ) // Scale for Z-axis -#define OFFSET_Z ( 0.0f ) // Offset for Z-axis +#define SCALE_Z (1.0f) // Scale for Z-axis +#define OFFSET_Z (0.0f) // Offset for Z-axis -#define TEMPERATURE_OFFSET ( 32.0f ) // Reference - // temperature for - // calibration +#define TEMPERATURE_OFFSET \ + (32.0f) // Reference + // temperature for + // calibration -#define TEMPERATURE_COEFFICIENT_Z ( 0.33f ) // Temperature - // coefficient for - // Z-axis +#define TEMPERATURE_COEFFICIENT_Z \ + (0.33f) // Temperature + // coefficient for + // Z-axis /** * @brief The orientation of the car */ -typedef enum { +typedef enum +{ NORTH, NORTH_EAST, EAST, @@ -45,10 +48,11 @@ typedef enum { /** * Angle of the car */ -typedef enum { - UP = 0, - DOWN = 1, - LEFT = 2, +typedef enum +{ + UP = 0, + DOWN = 1, + LEFT = 2, RIGHT = 3 } angle_t; @@ -59,13 +63,14 @@ typedef enum { * 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; +typedef struct +{ + float roll; + float pitch; + float yaw; compass_direction_t orientation; - angle_t roll_angle; - angle_t pitch_angle; + angle_t roll_angle; + angle_t pitch_angle; } direction_t; #endif diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index b2b1ba5..0c20347 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -32,7 +32,8 @@ * @return */ static inline float -calculate_roll(int16_t acceleration[3]) { +calculate_roll(int16_t acceleration[3]) +{ return atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI); } @@ -42,11 +43,12 @@ calculate_roll(int16_t acceleration[3]) { * @return */ static inline float -calculate_pitch(int16_t acceleration[3]) { - return atan2(- acceleration[0], - sqrt((acceleration[1] * acceleration[1]) + - (acceleration[2] * acceleration[2])) - ) * (180.0 / M_PI); +calculate_pitch(int16_t acceleration[3]) +{ + return atan2(-acceleration[0], + sqrt((acceleration[1] * acceleration[1]) + + (acceleration[2] * acceleration[2]))) + * (180.0 / M_PI); } /** @@ -55,7 +57,8 @@ calculate_pitch(int16_t acceleration[3]) { * @return */ static inline float -calculate_yaw_magnetometer(int16_t magnetometer[3]) { +calculate_yaw_magnetometer(int16_t magnetometer[3]) +{ return atan2(magnetometer[1], magnetometer[0]) * (180.0f / M_PI); } @@ -65,10 +68,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 @@ -82,8 +85,8 @@ compensate_magnetometer(float yaw_mag, int16_t temperature) { uint delta_temp = temperature - TEMPERATURE_OFFSET; // Apply temperature compensation to each axis using macros - float compensated_yaw_mag = - yaw_mag - ((float) delta_temp * TEMPERATURE_COEFFICIENT_Z); + float compensated_yaw_mag + = 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 +100,8 @@ compensate_magnetometer(float yaw_mag, int16_t temperature) { * @return yaw Yaw adjusted to be between 0 and 360 degrees */ static inline float -adjust_yaw(float yaw) { +adjust_yaw(float yaw) +{ if (yaw < 0) { yaw += 360; @@ -121,7 +125,8 @@ adjust_yaw(float yaw) { * @return Compass Direction */ static inline compass_direction_t -calculate_compass_direction(float yaw) { +calculate_compass_direction(float yaw) +{ if (yaw >= 337.5 || yaw < 22.5) { return NORTH; @@ -175,28 +180,6 @@ calculate_compass_direction(float yaw) { } } } -// 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; -// } } /** @@ -207,51 +190,43 @@ 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) { - 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; +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; + g_direction->pitch = pitch; + g_direction->pitch_angle = (pitch > 0) ? UP : DOWN; + g_direction->yaw = yaw; + g_direction->orientation = compass_direction; } /** * @brief Read the Accelerometer and Magnetometer Data and * Calculate the Direction of the Car - * @details Alpha is set to 0.98 to give more weight to the accelerometer data * @param acceleration Accelerometer Data * @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) +{ - float roll = calculate_roll(acceleration); - float pitch = calculate_pitch(acceleration); + 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, -// temperature[0]); -// compensated_mag_yaw = adjust_yaw(compensated_mag_yaw); + compass_direction_t compass_direction + = calculate_compass_direction(yaw_mag); -// 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); -// printf("Yaw: %f\n", yaw); - - compass_direction_t compass_direction = calculate_compass_direction(yaw_mag); - - update_orientation_data(roll, - pitch, - yaw_mag, - compass_direction); + update_orientation_data( + roll, pitch, yaw_mag, compass_direction, g_direction); } /** @@ -262,16 +237,16 @@ read_direction(int16_t acceleration[3], int16_t magnetometer[3]) { * @brief Task to Monitor the Direction of the Car * @param params */ -void print_orientation_data() { -// printf("Roll: %f, Pitch: %f, Yaw: %f\n", - printf("%f %f %f\n", - g_direction.roll, - g_direction.pitch, - g_direction.yaw - ); +void +print_orientation_data(volatile direction_t g_direction) +{ + // printf("Roll: %f, Pitch: %f, Yaw: %f\n", + printf("%f %f %f\n", g_direction.roll, g_direction.pitch, g_direction.yaw); } -void print_direction(compass_direction_t direction) { +void +print_direction(compass_direction_t direction) +{ switch (direction) { case NORTH: @@ -301,7 +276,9 @@ void print_direction(compass_direction_t direction) { } } -void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) { +void +print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) +{ switch (roll_angle) { case LEFT: @@ -323,7 +300,9 @@ void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) { } } -void updateDirection() { +void +updateDirection(volatile direction_t * g_direction) +{ int16_t magnetometer[3]; int16_t accelerometer[3]; int16_t temperature[1]; @@ -335,68 +314,83 @@ void updateDirection() { read_accelerometer(accelerometer); read_temperature(temperature); - read_direction(accelerometer, magnetometer); + read_direction(accelerometer, magnetometer, g_direction); + + print_orientation_data(*g_direction); // Temperature in degrees Celsius -// printf("Temperature: %d\n", temperature[0]); + // printf("Temperature: %d\n", temperature[0]); -// print_orientation_data(); + // print_orientation_data(); -// printf("Direction: "); + // printf("Direction: "); -// print_direction(g_direction.orientation); + // print_direction(g_direction.orientation); - switch (g_direction.orientation) + switch (g_direction->orientation) { case NORTH: - cur_y ++; + cur_y++; break; case EAST: - cur_x ++; + cur_x++; break; case SOUTH: - cur_y --; + cur_y--; break; case WEST: - cur_x --; + cur_x--; break; case NORTH_EAST: - cur_x ++; - cur_y ++; + cur_x++; + cur_y++; break; case SOUTH_EAST: - cur_x ++; - cur_y --; + cur_x++; + cur_y--; break; case SOUTH_WEST: - cur_x --; - cur_y --; + cur_x--; + cur_y--; break; case NORTH_WEST: - cur_x --; - cur_y ++; + 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); + // update_map(g_direction.orientation, cur_x, cur_y); -// printf("Current Position: (%d, %d)\n", cur_x, cur_y); -// print_map(); + // printf("Current Position: (%d, %d)\n", cur_x, cur_y); + // print_map(); -// print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle); + // print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle); } +void +monitor_direction_task(void *pvParameters) +{ + volatile direction_t *p_direction = NULL; + p_direction = (direction_t *) pvParameters; -void monitor_direction_task(__unused void *params) { for (;;) { - if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE) - { - updateDirection(); - } + updateDirection(p_direction); + vTaskDelay(pdMS_TO_TICKS(DIRECTION_READ_DELAY)); } } +void +magnetometer_tasks_init(car_struct_t *car_struct) +{ + TaskHandle_t h_direction_task = NULL; + xTaskCreate(monitor_direction_task, + "Direction Task", + configMINIMAL_STACK_SIZE, + car_struct, + PRIO, + &h_direction_task); +} #endif \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index f704de8..dc8da76 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -23,30 +23,29 @@ #include "message_buffer.h" #include "semphr.h" -#include "magnetometer_config.h" +#include "car_config.h" #include "LSM303DLHC_register.h" // Semaphores SemaphoreHandle_t g_direction_sem = NULL; -direction_t g_direction = { - .roll = 0, - .pitch = 0, - .yaw = 0, - .orientation = NORTH, - .roll_angle = LEFT, - .pitch_angle = UP -}; +// direction_t g_direction = { +// .roll = 0, +// .pitch = 0, +// .yaw = 0, +// .orientation = NORTH, +// .roll_angle = LEFT, +// .pitch_angle = UP +// }; -struct s_calibration_data { +struct s_calibration_data +{ int16_t accelerometerBias[3]; int16_t magnetometerBias[3]; }; -struct s_calibration_data g_calibration_data = { - .accelerometerBias = {0, 0, 0}, - .magnetometerBias = {0, 0, 0} -}; +struct s_calibration_data g_calibration_data + = { .accelerometerBias = { 0, 0, 0 }, .magnetometerBias = { 0, 0, 0 } }; /** * @brief Read Data with I2C, given the address and register @@ -55,7 +54,8 @@ struct s_calibration_data g_calibration_data = { * @return 1 piece of data read from the register */ static inline int -read_data(uint8_t addr, uint8_t reg) { +read_data(uint8_t addr, uint8_t reg) +{ uint8_t data[1]; // Send the register address to read from @@ -72,7 +72,8 @@ read_data(uint8_t addr, uint8_t reg) { * @param accelerometer Accelerometer Data */ static inline void -read_accelerometer(int16_t accelerometer[3]) { +read_accelerometer(int16_t accelerometer[3]) +{ uint8_t buffer[6]; buffer[0] = read_data(ACCEL_ADDR, LSM303_OUT_X_L_A); @@ -85,19 +86,18 @@ read_accelerometer(int16_t accelerometer[3]) { // Combine high and low bytes // xAcceleration - accelerometer[0] = (int16_t) ((buffer[1] << 8) | buffer[0]); + accelerometer[0] = (int16_t)((buffer[1] << 8) | buffer[0]); // yAcceleration - accelerometer[1] = (int16_t) ((buffer[3] << 8) | buffer[2]); + accelerometer[1] = (int16_t)((buffer[3] << 8) | buffer[2]); // zAcceleration - accelerometer[2] = (int16_t) ((buffer[5] << 8) | buffer[4]); + accelerometer[2] = (int16_t)((buffer[5] << 8) | buffer[4]); // Apply the calibration data accelerometer[0] -= g_calibration_data.accelerometerBias[0]; accelerometer[1] -= g_calibration_data.accelerometerBias[1]; accelerometer[2] -= g_calibration_data.accelerometerBias[2]; - } /** @@ -105,13 +105,14 @@ read_accelerometer(int16_t accelerometer[3]) { * @param magnetometer Magnetometer Data */ static inline void -read_magnetometer(int16_t magnetometer[3]) { +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 ++) + 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); @@ -121,9 +122,9 @@ read_magnetometer(int16_t magnetometer[3]) { buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M); // 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]); + xMagFiltered += (int16_t)(buffer[0] << 8 | buffer[1]); + yMagFiltered += (int16_t)(buffer[2] << 8 | buffer[3]); + zMagFiltered += (int16_t)(buffer[4] << 8 | buffer[5]); } // Calculate the moving average @@ -142,7 +143,8 @@ read_magnetometer(int16_t magnetometer[3]) { * @param temperature Temperature Data in Degrees Celsius */ static inline void -read_temperature(int16_t temperature[1]) { +read_temperature(int16_t temperature[1]) +{ uint8_t buffer[2]; buffer[0] = read_data(MAG_ADDR, LSM303_TEMP_OUT_H_M); @@ -155,35 +157,37 @@ read_temperature(int16_t temperature[1]) { * Source: https://electronics.stackexchange.com/a/356964 */ - int16_t raw_temperature = - (20 << 3) + (((int16_t) buffer[0] << 8 | buffer[1]) >> 4); + 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; + float temperature_celsius = (float)raw_temperature / 8.0; // Store the result in the temperature array - temperature[0] = (int16_t) temperature_celsius; + temperature[0] = (int16_t)temperature_celsius; } -static void initial_calibration() { +void +initial_calibration() +{ int16_t accelerometer[3]; int16_t magnetometer[3]; - int16_t accelerometerMin[3] = {0, 0, 0}; - int16_t accelerometerMax[3] = {0, 0, 0}; - int16_t magnetometerMin[3] = {0, 0, 0}; - int16_t magnetometerMax[3] = {0, 0, 0}; + int16_t accelerometerMin[3] = { 0, 0, 0 }; + int16_t accelerometerMax[3] = { 0, 0, 0 }; + int16_t magnetometerMin[3] = { 0, 0, 0 }; + int16_t magnetometerMax[3] = { 0, 0, 0 }; printf("Initial Calibration\n"); - for (int i = 0; i < 100; i ++) + for (int i = 0; i < 100; i++) { printf("Calibrating... %d\n", i); read_accelerometer(accelerometer); read_magnetometer(magnetometer); - for (int j = 0; j < 3; j ++) + for (int j = 0; j < 3; j++) { if (accelerometer[j] > accelerometerMax[j]) { @@ -205,19 +209,19 @@ static void initial_calibration() { sleep_ms(10); } - g_calibration_data.accelerometerBias[0] = - (accelerometerMax[0] + accelerometerMin[0]) / 2; - g_calibration_data.accelerometerBias[1] = - (accelerometerMax[1] + accelerometerMin[1]) / 2; - g_calibration_data.accelerometerBias[2] = - (accelerometerMax[2] + accelerometerMin[2]) / 2; + g_calibration_data.accelerometerBias[0] + = (accelerometerMax[0] + accelerometerMin[0]) / 2; + g_calibration_data.accelerometerBias[1] + = (accelerometerMax[1] + accelerometerMin[1]) / 2; + g_calibration_data.accelerometerBias[2] + = (accelerometerMax[2] + accelerometerMin[2]) / 2; - g_calibration_data.magnetometerBias[0] = - (magnetometerMax[0] + magnetometerMin[0]) / 2; - g_calibration_data.magnetometerBias[1] = - (magnetometerMax[1] + magnetometerMin[1]) / 2; - g_calibration_data.magnetometerBias[2] = - (magnetometerMax[2] + magnetometerMin[2]) / 2; + g_calibration_data.magnetometerBias[0] + = (magnetometerMax[0] + magnetometerMin[0]) / 2; + g_calibration_data.magnetometerBias[1] + = (magnetometerMax[1] + magnetometerMin[1]) / 2; + g_calibration_data.magnetometerBias[2] + = (magnetometerMax[2] + magnetometerMin[2]) / 2; printf("Accelerometer Bias: %d, %d, %d\n", g_calibration_data.accelerometerBias[0], @@ -242,14 +246,15 @@ static void initial_calibration() { * @return None */ static void -LSM303DLHC_init() { +LSM303DLHC_init() +{ /** * Accelerometer Setup */ // 0x20 = CTRL_REG1_A // Normal power mode, all axes enabled, 10 Hz - uint8_t buf[2] = {LSM303_CTRL_REG1_A, 0x27}; + uint8_t buf[2] = { LSM303_CTRL_REG1_A, 0x27 }; i2c_write_blocking(i2c_default, ACCEL_ADDR, buf, 2, false); // Reboot memory content (0x40 = CTRL_REG4_A) @@ -285,8 +290,15 @@ LSM303DLHC_init() { * @details Initialise the I2C Port, SDA and SCL Pins, and the LSM303DLHC Sensor */ void -magnetometer_init() +magnetometer_init(car_struct_t *p_car_struct) { + p_car_struct->p_direction->roll = 0; + p_car_struct->p_direction->pitch = 0; + p_car_struct->p_direction->yaw = 0; + p_car_struct->p_direction->orientation = NORTH; + p_car_struct->p_direction->roll_angle = LEFT; + p_car_struct->p_direction->pitch_angle = UP; + i2c_init(I2C_PORT, 400 * 1000); gpio_set_function(I2C_SDA, GPIO_FUNC_I2C); @@ -296,12 +308,9 @@ magnetometer_init() LSM303DLHC_init(); -// initial_calibration(); + // initial_calibration(); -// sleep_ms(3000); printf("Magnetometer Initialised\n"); - // Semaphore - // g_direction_sem = xSemaphoreCreateBinary(); } /** @@ -310,11 +319,11 @@ magnetometer_init() * @return True (To keep the timer running) */ bool -h_direction_timer_handler(repeating_timer_t *repeatingTimer) { +h_direction_timer_handler(repeating_timer_t *repeatingTimer) +{ BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_direction_sem, - &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(g_direction_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return true; } diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h deleted file mode 100644 index 8f8911b..0000000 --- a/frtos/magnetometer/magnetometer_read.h +++ /dev/null @@ -1,14 +0,0 @@ -/** - * @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" - - -#endif \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c index 6b137db..703bbbe 100644 --- a/frtos/magnetometer/magnetometer_test.c +++ b/frtos/magnetometer/magnetometer_test.c @@ -1,6 +1,5 @@ #include "magnetometer_init.h" -#include "magnetometer_read.h" #include "magnetometer_direction.h" #include "map.h" @@ -9,11 +8,6 @@ void launch() { - struct repeating_timer g_direction_timer; - add_repeating_timer_ms(DIRECTION_READ_DELAY, - h_direction_timer_handler, - NULL, - &g_direction_timer); TaskHandle_t h_monitor_direction_task = NULL; xTaskCreate(monitor_direction_task, @@ -31,14 +25,27 @@ main (void) { stdio_usb_init(); + direction_t 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 car_path_grid = create_grid(grid_rows, grid_cols); - magnetometer_init(); + sleep_ms(2000); + printf("Test started!\n"); - launch(); + magnetometer_init(&car_struct); + +// printf("Magnetometer initialized!\n"); + + magnetometer_tasks_init(&car_struct); + + vTaskStartScheduler(); + +// launch(); return(0); } \ No newline at end of file diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index fed0c87..dd5febc 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -85,8 +85,8 @@ spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct) for (;;) { - updateDirection(); - if (check_direction(g_direction.yaw, target_yaw, 1)) + updateDirection(pp_car_struct->p_direction); + if (check_direction(pp_car_struct->p_direction->yaw, target_yaw, 1)) { set_wheel_direction(DIRECTION_MASK); set_wheel_speed_synced(0u, pp_car_struct); @@ -104,8 +104,8 @@ spin_right(float degree, car_struct_t *pp_car_struct) set_wheel_direction(DIRECTION_MASK); vTaskDelay(pdMS_TO_TICKS(50)); - updateDirection(); - float initial_yaw = g_direction.yaw; + updateDirection(pp_car_struct->p_direction); + float initial_yaw = pp_car_struct->p_direction->yaw; float target_yaw = adjust_yaw(initial_yaw + degree); spin_to_yaw(DIRECTION_RIGHT, target_yaw, pp_car_struct); @@ -117,8 +117,8 @@ spin_left(float degree, car_struct_t *pp_car_struct) set_wheel_direction(DIRECTION_MASK); vTaskDelay(pdMS_TO_TICKS(50)); - updateDirection(); - float initial_yaw = g_direction.yaw; + updateDirection(pp_car_struct->p_direction); + float initial_yaw = pp_car_struct->p_direction->yaw; float target_yaw = adjust_yaw(initial_yaw - degree); spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct);