devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Amended Magnetometer to use global struct

This commit is contained in:
Devoalda 2023-11-09 23:24:45 +08:00
parent 034ccb47e5
commit 9e391da487
7 changed files with 218 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
frtos/config/car_config.h
View File

@ -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;

55
frtos/config/magnetometer_config.h
View File

@ -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

202
frtos/magnetometer/magnetometer_direction.h
View File

@ -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

127
frtos/magnetometer/magnetometer_init.h
View File

@ -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;
}

14
frtos/magnetometer/magnetometer_read.h
View File

@ -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

23
frtos/magnetometer/magnetometer_test.c
View File

@ -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);
}

12
frtos/motor/motor_direction.h
View File

@ -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);