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_left_motor;
motor_t *p_right_motor; motor_t *p_right_motor;
motor_pid_t *p_pid; motor_pid_t *p_pid;
direction_t *p_direction;
} car_struct_t; } car_struct_t;

55
frtos/config/magnetometer_config.h
View File

@ -1,37 +1,40 @@
#ifndef MAGNETOMETER_CONFIG_H #ifndef MAGNETOMETER_CONFIG_H
#define MAGNETOMETER_CONFIG_H #define MAGNETOMETER_CONFIG_H
#define I2C_PORT i2c0 #define I2C_PORT i2c0
#define I2C_SDA ( 8 ) #define I2C_SDA (8)
#define I2C_SCL ( 9 ) #define I2C_SCL (9)
#define DIRECTION_READ_DELAY ( 100 ) #define DIRECTION_READ_DELAY (100)
#define NUM_READINGS ( 10 ) // Number of readings to #define NUM_READINGS ( 10 ) // Number of readings to
// take before // take before
// calculating // calculating
// direction // direction
//#define ALPHA ( 0.1f ) // Low Pass Filter // #define ALPHA ( 0.1f ) // Low Pass Filter
// Coefficient // Coefficient
// LSM303DLHC temperature compensation coefficients // LSM303DLHC temperature compensation coefficients
#define SCALE_Z ( 1.0f ) // Scale for Z-axis #define SCALE_Z (1.0f) // Scale for Z-axis
#define OFFSET_Z ( 0.0f ) // Offset for Z-axis #define OFFSET_Z (0.0f) // Offset for Z-axis
#define TEMPERATURE_OFFSET ( 32.0f ) // Reference #define TEMPERATURE_OFFSET \
// temperature for (32.0f) // Reference
// calibration // temperature for
// calibration
#define TEMPERATURE_COEFFICIENT_Z ( 0.33f ) // Temperature #define TEMPERATURE_COEFFICIENT_Z \
// coefficient for (0.33f) // Temperature
// Z-axis // coefficient for
// Z-axis
/** /**
* @brief The orientation of the car * @brief The orientation of the car
*/ */
typedef enum { typedef enum
{
NORTH, NORTH,
NORTH_EAST, NORTH_EAST,
EAST, EAST,
@ -45,10 +48,11 @@ typedef enum {
/** /**
* Angle of the car * Angle of the car
*/ */
typedef enum { typedef enum
UP = 0, {
DOWN = 1, UP = 0,
LEFT = 2, DOWN = 1,
LEFT = 2,
RIGHT = 3 RIGHT = 3
} angle_t; } angle_t;
@ -59,13 +63,14 @@ typedef enum {
* heading = direction of the car (north, east, south, west) in degrees * heading = direction of the car (north, east, south, west) in degrees
* orientation = orientation of the car (north, east, south, west) * orientation = orientation of the car (north, east, south, west)
*/ */
typedef struct { typedef struct
float roll; {
float pitch; float roll;
float yaw; float pitch;
float yaw;
compass_direction_t orientation; compass_direction_t orientation;
angle_t roll_angle; angle_t roll_angle;
angle_t pitch_angle; angle_t pitch_angle;
} direction_t; } direction_t;
#endif #endif

202
frtos/magnetometer/magnetometer_direction.h
View File

@ -32,7 +32,8 @@
* @return * @return
*/ */
static inline float 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); return atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
} }
@ -42,11 +43,12 @@ calculate_roll(int16_t acceleration[3]) {
* @return * @return
*/ */
static inline float static inline float
calculate_pitch(int16_t acceleration[3]) { calculate_pitch(int16_t acceleration[3])
return atan2(- acceleration[0], {
sqrt((acceleration[1] * acceleration[1]) + return atan2(-acceleration[0],
(acceleration[2] * acceleration[2])) sqrt((acceleration[1] * acceleration[1])
) * (180.0 / M_PI); + (acceleration[2] * acceleration[2])))
* (180.0 / M_PI);
} }
/** /**
@ -55,7 +57,8 @@ calculate_pitch(int16_t acceleration[3]) {
* @return * @return
*/ */
static inline float 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); 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 * @param yaw_mag Yaw calculated from Magnetometer Data
* @return yaw Yaw calculated from Complementary Filter * @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 * @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; uint delta_temp = temperature - TEMPERATURE_OFFSET;
// Apply temperature compensation to each axis using macros // Apply temperature compensation to each axis using macros
float compensated_yaw_mag = float compensated_yaw_mag
yaw_mag - ((float) delta_temp * TEMPERATURE_COEFFICIENT_Z); = yaw_mag - ((float)delta_temp * TEMPERATURE_COEFFICIENT_Z);
// Apply scale and offset corrections using macros // Apply scale and offset corrections using macros
compensated_yaw_mag = (compensated_yaw_mag - OFFSET_Z) * SCALE_Z; 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 * @return yaw Yaw adjusted to be between 0 and 360 degrees
*/ */
static inline float static inline float
adjust_yaw(float yaw) { adjust_yaw(float yaw)
{
if (yaw < 0) if (yaw < 0)
{ {
yaw += 360; yaw += 360;
@ -121,7 +125,8 @@ adjust_yaw(float yaw) {
* @return Compass Direction * @return Compass Direction
*/ */
static inline compass_direction_t static inline compass_direction_t
calculate_compass_direction(float yaw) { calculate_compass_direction(float yaw)
{
if (yaw >= 337.5 || yaw < 22.5) if (yaw >= 337.5 || yaw < 22.5)
{ {
return NORTH; 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 * @param compass_direction Compass Direction
*/ */
static inline void static inline void
update_orientation_data(float roll, float pitch, float yaw, update_orientation_data(float roll,
compass_direction_t compass_direction) { float pitch,
g_direction.roll = roll; float yaw,
g_direction.roll_angle = (roll > 0) ? LEFT : RIGHT; compass_direction_t compass_direction,
g_direction.pitch = pitch; volatile direction_t *g_direction)
g_direction.pitch_angle = (pitch > 0) ? UP : DOWN; {
g_direction.yaw = yaw; g_direction->roll = roll;
g_direction.orientation = compass_direction; 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 * @brief Read the Accelerometer and Magnetometer Data and
* Calculate the Direction of the Car * 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 acceleration Accelerometer Data
* @param magnetometer Magnetometer Data * @param magnetometer Magnetometer Data
*/ */
static void 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 roll = calculate_roll(acceleration);
float pitch = calculate_pitch(acceleration); float pitch = calculate_pitch(acceleration);
float yaw_mag = calculate_yaw_magnetometer(magnetometer); float yaw_mag = calculate_yaw_magnetometer(magnetometer);
yaw_mag = adjust_yaw(yaw_mag); yaw_mag = adjust_yaw(yaw_mag);
// Apply temperature compensation to the magnetometer data compass_direction_t compass_direction
// float compensated_mag_yaw = compensate_magnetometer(yaw_mag, = calculate_compass_direction(yaw_mag);
// temperature[0]);
// compensated_mag_yaw = adjust_yaw(compensated_mag_yaw);
// float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0f / M_PI); update_orientation_data(
// yaw_acc = adjust_yaw(yaw_acc); roll, pitch, yaw_mag, compass_direction, g_direction);
//
// 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);
} }
/** /**
@ -262,16 +237,16 @@ read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
* @brief Task to Monitor the Direction of the Car * @brief Task to Monitor the Direction of the Car
* @param params * @param params
*/ */
void print_orientation_data() { void
// printf("Roll: %f, Pitch: %f, Yaw: %f\n", print_orientation_data(volatile direction_t g_direction)
printf("%f %f %f\n", {
g_direction.roll, // printf("Roll: %f, Pitch: %f, Yaw: %f\n",
g_direction.pitch, printf("%f %f %f\n", g_direction.roll, g_direction.pitch, g_direction.yaw);
g_direction.yaw
);
} }
void print_direction(compass_direction_t direction) { void
print_direction(compass_direction_t direction)
{
switch (direction) switch (direction)
{ {
case NORTH: 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) switch (roll_angle)
{ {
case LEFT: 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 magnetometer[3];
int16_t accelerometer[3]; int16_t accelerometer[3];
int16_t temperature[1]; int16_t temperature[1];
@ -335,68 +314,83 @@ void updateDirection() {
read_accelerometer(accelerometer); read_accelerometer(accelerometer);
read_temperature(temperature); read_temperature(temperature);
read_direction(accelerometer, magnetometer); read_direction(accelerometer, magnetometer, g_direction);
print_orientation_data(*g_direction);
// Temperature in degrees Celsius // 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: case NORTH:
cur_y ++; cur_y++;
break; break;
case EAST: case EAST:
cur_x ++; cur_x++;
break; break;
case SOUTH: case SOUTH:
cur_y --; cur_y--;
break; break;
case WEST: case WEST:
cur_x --; cur_x--;
break; break;
case NORTH_EAST: case NORTH_EAST:
cur_x ++; cur_x++;
cur_y ++; cur_y++;
break; break;
case SOUTH_EAST: case SOUTH_EAST:
cur_x ++; cur_x++;
cur_y --; cur_y--;
break; break;
case SOUTH_WEST: case SOUTH_WEST:
cur_x --; cur_x--;
cur_y --; cur_y--;
break; break;
case NORTH_WEST: case NORTH_WEST:
cur_x --; cur_x--;
cur_y ++; cur_y++;
break; break;
} }
// Update the map based on the direction of the car (N, E, S, W) // 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); // printf("Current Position: (%d, %d)\n", cur_x, cur_y);
// print_map(); // 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 (;;) for (;;)
{ {
if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE) updateDirection(p_direction);
{ vTaskDelay(pdMS_TO_TICKS(DIRECTION_READ_DELAY));
updateDirection();
}
} }
} }
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 #endif

127
frtos/magnetometer/magnetometer_init.h
View File

@ -23,30 +23,29 @@
#include "message_buffer.h" #include "message_buffer.h"
#include "semphr.h" #include "semphr.h"
#include "magnetometer_config.h" #include "car_config.h"
#include "LSM303DLHC_register.h" #include "LSM303DLHC_register.h"
// Semaphores // Semaphores
SemaphoreHandle_t g_direction_sem = NULL; SemaphoreHandle_t g_direction_sem = NULL;
direction_t g_direction = { // direction_t g_direction = {
.roll = 0, // .roll = 0,
.pitch = 0, // .pitch = 0,
.yaw = 0, // .yaw = 0,
.orientation = NORTH, // .orientation = NORTH,
.roll_angle = LEFT, // .roll_angle = LEFT,
.pitch_angle = UP // .pitch_angle = UP
}; // };
struct s_calibration_data { struct s_calibration_data
{
int16_t accelerometerBias[3]; int16_t accelerometerBias[3];
int16_t magnetometerBias[3]; int16_t magnetometerBias[3];
}; };
struct s_calibration_data g_calibration_data = { struct s_calibration_data g_calibration_data
.accelerometerBias = {0, 0, 0}, = { .accelerometerBias = { 0, 0, 0 }, .magnetometerBias = { 0, 0, 0 } };
.magnetometerBias = {0, 0, 0}
};
/** /**
* @brief Read Data with I2C, given the address and register * @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 * @return 1 piece of data read from the register
*/ */
static inline int static inline int
read_data(uint8_t addr, uint8_t reg) { read_data(uint8_t addr, uint8_t reg)
{
uint8_t data[1]; uint8_t data[1];
// Send the register address to read from // Send the register address to read from
@ -72,7 +72,8 @@ read_data(uint8_t addr, uint8_t reg) {
* @param accelerometer Accelerometer Data * @param accelerometer Accelerometer Data
*/ */
static inline void static inline void
read_accelerometer(int16_t accelerometer[3]) { read_accelerometer(int16_t accelerometer[3])
{
uint8_t buffer[6]; uint8_t buffer[6];
buffer[0] = read_data(ACCEL_ADDR, LSM303_OUT_X_L_A); 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 // Combine high and low bytes
// xAcceleration // xAcceleration
accelerometer[0] = (int16_t) ((buffer[1] << 8) | buffer[0]); accelerometer[0] = (int16_t)((buffer[1] << 8) | buffer[0]);
// yAcceleration // yAcceleration
accelerometer[1] = (int16_t) ((buffer[3] << 8) | buffer[2]); accelerometer[1] = (int16_t)((buffer[3] << 8) | buffer[2]);
// zAcceleration // zAcceleration
accelerometer[2] = (int16_t) ((buffer[5] << 8) | buffer[4]); accelerometer[2] = (int16_t)((buffer[5] << 8) | buffer[4]);
// Apply the calibration data // Apply the calibration data
accelerometer[0] -= g_calibration_data.accelerometerBias[0]; accelerometer[0] -= g_calibration_data.accelerometerBias[0];
accelerometer[1] -= g_calibration_data.accelerometerBias[1]; accelerometer[1] -= g_calibration_data.accelerometerBias[1];
accelerometer[2] -= g_calibration_data.accelerometerBias[2]; accelerometer[2] -= g_calibration_data.accelerometerBias[2];
} }
/** /**
@ -105,13 +105,14 @@ read_accelerometer(int16_t accelerometer[3]) {
* @param magnetometer Magnetometer Data * @param magnetometer Magnetometer Data
*/ */
static inline void static inline void
read_magnetometer(int16_t magnetometer[3]) { read_magnetometer(int16_t magnetometer[3])
{
uint8_t buffer[6]; uint8_t buffer[6];
int32_t xMagFiltered = 0; int32_t xMagFiltered = 0;
int32_t yMagFiltered = 0; int32_t yMagFiltered = 0;
int32_t zMagFiltered = 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[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_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); buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M);
// Update the cumulative sum of the magnetometer data // Update the cumulative sum of the magnetometer data
xMagFiltered += (int16_t) (buffer[0] << 8 | buffer[1]); xMagFiltered += (int16_t)(buffer[0] << 8 | buffer[1]);
yMagFiltered += (int16_t) (buffer[2] << 8 | buffer[3]); yMagFiltered += (int16_t)(buffer[2] << 8 | buffer[3]);
zMagFiltered += (int16_t) (buffer[4] << 8 | buffer[5]); zMagFiltered += (int16_t)(buffer[4] << 8 | buffer[5]);
} }
// Calculate the moving average // Calculate the moving average
@ -142,7 +143,8 @@ read_magnetometer(int16_t magnetometer[3]) {
* @param temperature Temperature Data in Degrees Celsius * @param temperature Temperature Data in Degrees Celsius
*/ */
static inline void static inline void
read_temperature(int16_t temperature[1]) { read_temperature(int16_t temperature[1])
{
uint8_t buffer[2]; uint8_t buffer[2];
buffer[0] = read_data(MAG_ADDR, LSM303_TEMP_OUT_H_M); 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 * Source: https://electronics.stackexchange.com/a/356964
*/ */
int16_t raw_temperature = int16_t raw_temperature
(20 << 3) + (((int16_t) buffer[0] << 8 | buffer[1]) >> 4); = (20 << 3) + (((int16_t)buffer[0] << 8 | buffer[1]) >> 4);
// Convert the raw temperature data to degrees Celsius // 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 // 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 accelerometer[3];
int16_t magnetometer[3]; int16_t magnetometer[3];
int16_t accelerometerMin[3] = {0, 0, 0}; int16_t accelerometerMin[3] = { 0, 0, 0 };
int16_t accelerometerMax[3] = {0, 0, 0}; int16_t accelerometerMax[3] = { 0, 0, 0 };
int16_t magnetometerMin[3] = {0, 0, 0}; int16_t magnetometerMin[3] = { 0, 0, 0 };
int16_t magnetometerMax[3] = {0, 0, 0}; int16_t magnetometerMax[3] = { 0, 0, 0 };
printf("Initial Calibration\n"); printf("Initial Calibration\n");
for (int i = 0; i < 100; i ++) for (int i = 0; i < 100; i++)
{ {
printf("Calibrating... %d\n", i); printf("Calibrating... %d\n", i);
read_accelerometer(accelerometer); read_accelerometer(accelerometer);
read_magnetometer(magnetometer); read_magnetometer(magnetometer);
for (int j = 0; j < 3; j ++) for (int j = 0; j < 3; j++)
{ {
if (accelerometer[j] > accelerometerMax[j]) if (accelerometer[j] > accelerometerMax[j])
{ {
@ -205,19 +209,19 @@ static void initial_calibration() {
sleep_ms(10); sleep_ms(10);
} }
g_calibration_data.accelerometerBias[0] = g_calibration_data.accelerometerBias[0]
(accelerometerMax[0] + accelerometerMin[0]) / 2; = (accelerometerMax[0] + accelerometerMin[0]) / 2;
g_calibration_data.accelerometerBias[1] = g_calibration_data.accelerometerBias[1]
(accelerometerMax[1] + accelerometerMin[1]) / 2; = (accelerometerMax[1] + accelerometerMin[1]) / 2;
g_calibration_data.accelerometerBias[2] = g_calibration_data.accelerometerBias[2]
(accelerometerMax[2] + accelerometerMin[2]) / 2; = (accelerometerMax[2] + accelerometerMin[2]) / 2;
g_calibration_data.magnetometerBias[0] = g_calibration_data.magnetometerBias[0]
(magnetometerMax[0] + magnetometerMin[0]) / 2; = (magnetometerMax[0] + magnetometerMin[0]) / 2;
g_calibration_data.magnetometerBias[1] = g_calibration_data.magnetometerBias[1]
(magnetometerMax[1] + magnetometerMin[1]) / 2; = (magnetometerMax[1] + magnetometerMin[1]) / 2;
g_calibration_data.magnetometerBias[2] = g_calibration_data.magnetometerBias[2]
(magnetometerMax[2] + magnetometerMin[2]) / 2; = (magnetometerMax[2] + magnetometerMin[2]) / 2;
printf("Accelerometer Bias: %d, %d, %d\n", printf("Accelerometer Bias: %d, %d, %d\n",
g_calibration_data.accelerometerBias[0], g_calibration_data.accelerometerBias[0],
@ -242,14 +246,15 @@ static void initial_calibration() {
* @return None * @return None
*/ */
static void static void
LSM303DLHC_init() { LSM303DLHC_init()
{
/** /**
* Accelerometer Setup * Accelerometer Setup
*/ */
// 0x20 = CTRL_REG1_A // 0x20 = CTRL_REG1_A
// Normal power mode, all axes enabled, 10 Hz // 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); i2c_write_blocking(i2c_default, ACCEL_ADDR, buf, 2, false);
// Reboot memory content (0x40 = CTRL_REG4_A) // 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 * @details Initialise the I2C Port, SDA and SCL Pins, and the LSM303DLHC Sensor
*/ */
void 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); i2c_init(I2C_PORT, 400 * 1000);
gpio_set_function(I2C_SDA, GPIO_FUNC_I2C); gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
@ -296,12 +308,9 @@ magnetometer_init()
LSM303DLHC_init(); LSM303DLHC_init();
// initial_calibration(); // initial_calibration();
// sleep_ms(3000);
printf("Magnetometer Initialised\n"); printf("Magnetometer Initialised\n");
// Semaphore
// g_direction_sem = xSemaphoreCreateBinary();
} }
/** /**
@ -310,11 +319,11 @@ magnetometer_init()
* @return True (To keep the timer running) * @return True (To keep the timer running)
*/ */
bool bool
h_direction_timer_handler(repeating_timer_t *repeatingTimer) { h_direction_timer_handler(repeating_timer_t *repeatingTimer)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_direction_sem, xSemaphoreGiveFromISR(g_direction_sem, &xHigherPriorityTaskWoken);
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true; 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_init.h"
#include "magnetometer_read.h"
#include "magnetometer_direction.h" #include "magnetometer_direction.h"
#include "map.h" #include "map.h"
@ -9,11 +8,6 @@
void void
launch() 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; TaskHandle_t h_monitor_direction_task = NULL;
xTaskCreate(monitor_direction_task, xTaskCreate(monitor_direction_task,
@ -31,14 +25,27 @@ main (void)
{ {
stdio_usb_init(); 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_rows = 10; // Define the number of rows in your grid
int grid_cols = 10; // Define the number of columns in your grid int grid_cols = 10; // Define the number of columns in your grid
car_path_grid = create_grid(grid_rows, grid_cols); 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); 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 (;;) for (;;)
{ {
updateDirection(); updateDirection(pp_car_struct->p_direction);
if (check_direction(g_direction.yaw, target_yaw, 1)) if (check_direction(pp_car_struct->p_direction->yaw, target_yaw, 1))
{ {
set_wheel_direction(DIRECTION_MASK); set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u, pp_car_struct); 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); set_wheel_direction(DIRECTION_MASK);
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
updateDirection(); updateDirection(pp_car_struct->p_direction);
float initial_yaw = g_direction.yaw; float initial_yaw = pp_car_struct->p_direction->yaw;
float target_yaw = adjust_yaw(initial_yaw + degree); float target_yaw = adjust_yaw(initial_yaw + degree);
spin_to_yaw(DIRECTION_RIGHT, target_yaw, pp_car_struct); 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); set_wheel_direction(DIRECTION_MASK);
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
updateDirection(); updateDirection(pp_car_struct->p_direction);
float initial_yaw = g_direction.yaw; float initial_yaw = pp_car_struct->p_direction->yaw;
float target_yaw = adjust_yaw(initial_yaw - degree); float target_yaw = adjust_yaw(initial_yaw - degree);
spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct); spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct);