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

fix(Magnetometer): 

Removed Com Filter
This commit is contained in:
Devoalda 2023-11-04 13:38:47 +08:00
parent 4137d4f350
commit f933d8ea91
5 changed files with 303 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
frtos/config/magnetometer_config.h
View File

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

190
frtos/magnetometer/magnetometer_direction.h
View File

@ -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;
}
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,15 +319,14 @@ void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) {
}
}
void monitor_direction_task(__unused void *params) {
for (;;)
{
if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
{
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);
@ -275,10 +342,57 @@ void monitor_direction_task(__unused void *params) {
print_direction(g_direction.orientation);
print_roll_and_pitch(g_direction.roll_angle,
g_direction.pitch_angle);
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)
{
updateDirection();
}
}
}
#endif

1
frtos/magnetometer/magnetometer_init.h
View File

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

7
frtos/magnetometer/magnetometer_test.c
View File

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

128
frtos/magnetometer/map.h Normal file
View File

@ -0,0 +1,128 @@
//
// Created by junwei on 31/10/23.
//
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#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