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

feature(LSM303DLHC Sensor): 

Using both Accelerometer and magnetometer to do a complementary filter to calculate the compass direction.
Added more fine-grained directions (NW, NE, SE, SW)
This commit is contained in:
Devoalda 2023-10-25 20:46:13 +08:00
parent 972cd4240d
commit e0c85ce2ce
5 changed files with 285 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
frtos/config/magnetometer_config.h
View File

@ -5,7 +5,47 @@
#define I2C_SDA ( 8 )
#define I2C_SCL ( 9 )
#define MAGNETOMETER_READ_DELAY ( 1000 )
#define ACCELEROMETER_READ_DELAY ( 1000 )
#define DIRECTION_READ_DELAY ( 100 )
/**
* @brief The orientation of the car
*/
typedef enum {
NORTH,
NORTH_EAST,
EAST,
SOUTH_EAST,
SOUTH,
SOUTH_WEST,
WEST,
NORTH_WEST
} compass_direction_t;
/**
* Angle of the car
*/
typedef enum {
UP = 0,
DOWN = 1,
LEFT = 2,
RIGHT = 3
} angle_t;
/**
* @brief The direction of the car
* roll = angle of the car (left or right)
* pitch = angle of the car (up or down)
* 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;
compass_direction_t orientation;
angle_t roll_angle;
angle_t pitch_angle;
} direction_t;
#endif

260
frtos/magnetometer/magnetometer_direction.h
View File

@ -1,59 +1,153 @@
/**
* @file magnetometer_direction.h
* @author Woon Jun Wei
* @brief This file contains the functions to calculate the direction of
* the car using the accelerometer and magnetometer data
*
* @details The direction of the car is calculated using the roll, pitch and yaw
* The roll and pitch are calculated using the accelerometer data
* The yaw is calculated using the magnetometer data
* The roll, pitch and yaw are combined to calculate the direction
* of the car
*
* The direction of the car is calculated using the complementary filter
* The complementary filter is used to combine the accelerometer
* and magnetometer data to calculate the direction of the car
*
*/
#ifndef MAGNETOMETER_DIRECTION_H
#define MAGNETOMETER_DIRECTION_H
#include "magnetometer_init.h"
static char
*direction(int16_t acceleration[3], int16_t magnetometer[3]) {
// Calculate the angle from accelerometer data
float roll = atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
float pitch = atan2(- acceleration[0],
sqrt(acceleration[1] * acceleration[1] +
acceleration[2] * acceleration[2])) *
(180.0 / M_PI);
/**
* @brief Roll Calculation with Accelerometer Data
* @param acceleration Accelerometer Data
* @return
*/
float calculate_roll(int16_t acceleration[3]) {
return atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
}
// Calculate the heading from magnetometer data
float heading = atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
/**
* @brief Pitch Calculation with Accelerometer Data
* @param acceleration Accelerometer Data
* @return
*/
float calculate_pitch(int16_t acceleration[3]) {
return atan2(- acceleration[0], sqrt(acceleration[1] * acceleration[1] +
acceleration[2] * acceleration[2])) *
(180.0 / M_PI);
}
// Adjust the heading for negative values
if (heading < 0)
/**
* @brief Yaw Calculation with Magnetometer Data
* @param magnetometer Magnetometer Data
* @return
*/
float calculate_yaw_magnetometer(int16_t magnetometer[3]) {
return atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
}
/**
* @brief Complementary Filter for Yaw
* @param alpha Complementary Filter Constant
* @param yaw_acc Yaw calculated from Accelerometer Data
* @param yaw_mag Yaw calculated from Magnetometer Data
* @return yaw Yaw calculated from Complementary Filter
*/
float calculate_yaw_complementary(float alpha, float yaw_acc, float yaw_mag) {
return alpha * yaw_acc + (1 - alpha) * yaw_mag;
}
/**
* @brief Adjust Yaw to be between 0 and 360 degrees
* @param yaw Yaw calculated from Complementary Filter
* @return yaw Yaw adjusted to be between 0 and 360 degrees
*/
float adjust_yaw(float yaw) {
return (yaw < 0) ? yaw + 360.0f : yaw;
}
/**
* @brief Calculate the Compass Direction (N, NE, E, SE, S, SW, W, NW)
* 22.5 = 360 / 16, used to calculate the compass direction from
* the compass direction enum
* @param yaw Yaw calculated from Complementary Filter
* @return Compass Direction
*/
compass_direction_t calculate_compass_direction(float yaw) {
int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions
switch (orientation)
{
heading += 360.0;
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;
}
}
// Determine the direction based on the heading
// TODO: Optimize this
char *dir;
/**
* @brief Update the Orientation Data
* @param roll Roll calculated from Accelerometer Data
* @param pitch Pitch calculated from Accelerometer Data
* @param yaw Yaw calculated from Complementary Filter
* @param compass_direction Compass Direction
*/
void update_orientation_data(float roll, float pitch, float yaw,
compass_direction_t compass_direction) {
g_direction.roll = roll;
g_direction.pitch = pitch;
g_direction.yaw = yaw;
g_direction.orientation = compass_direction;
}
if (heading >= 315 || heading < 45)
{
dir = "North";
}
else
{
if (heading >= 45 && heading < 135)
{
dir = "East";
}
else
{
if (heading >= 135 && heading < 225)
{
dir = "South";
}
else
{
dir = "West";
}
}
}
/**
* @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]) {
float roll = calculate_roll(acceleration);
float pitch = calculate_pitch(acceleration);
float yaw_mag = calculate_yaw_magnetometer(magnetometer);
printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading);
float alpha = 0.98;
float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI);
float yaw = calculate_yaw_complementary(alpha, yaw_acc, yaw_mag);
return dir;
yaw = adjust_yaw(yaw);
compass_direction_t compass_direction = calculate_compass_direction(yaw);
update_orientation_data(roll, pitch, yaw, compass_direction);
}
/**
* FreeRTOS Tasks
*/
/**
* @brief Task to Monitor the Direction of the Car
* @param params
*/
void
monitor_direction_task(__unused void *params) {
for (;;)
@ -67,53 +161,63 @@ monitor_direction_task(__unused void *params) {
read_magnetometer(magnetometer);
read_accelerometer(accelerometer);
// Calculate the angle from accelerometer data
float roll =
atan2(accelerometer[1], accelerometer[2]) * (180.0 / M_PI);
float pitch = atan2(- accelerometer[0],
sqrt(accelerometer[1] * accelerometer[1] +
accelerometer[2] * accelerometer[2])) *
(180.0 / M_PI);
read_direction(accelerometer, magnetometer);
// Calculate the heading from magnetometer data
float heading =
atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
printf("Roll: %f, Pitch: %f, Yaw: %f\n",
g_direction.roll, g_direction.pitch, g_direction.yaw);
// Adjust the heading for negative values
if (heading < 0)
printf("Direction:");
switch (g_direction.orientation)
{
heading += 360.0;
case NORTH:
printf("North\n");
break;
case NORTH_EAST:
printf("North East\n");
break;
case EAST:
printf("East\n");
break;
case SOUTH_EAST:
printf("South East\n");
break;
case SOUTH:
printf("South\n");
break;
case SOUTH_WEST:
printf("South West\n");
break;
case WEST:
printf("West\n");
break;
case NORTH_WEST:
printf("North West\n");
break;
}
// Determine the direction based on the heading
char *dir;
if (heading >= 315 || heading < 45)
switch (g_direction.roll_angle)
{
dir = "North";
}
else
{
if (heading >= 45 && heading < 135)
{
dir = "East";
}
else
{
if (heading >= 135 && heading < 225)
{
dir = "South";
}
else
{
dir = "West";
}
}
case LEFT:
printf("You're Flying!\n");
break;
case RIGHT:
printf("You're Plunging!\n");
break;
}
printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading);
switch (g_direction.pitch_angle)
{
case UP:
printf("Your left wheel is in the air!\n");
break;
case DOWN:
printf("Your right wheel is in the air!\n");
break;
}
printf("Direction: %s\n", dir);
}
}
}
#endif

92
frtos/magnetometer/magnetometer_init.h
View File

@ -1,10 +1,12 @@
/**
* @file magnetometer_init.h
* @brief Initialise the magnetometer sensor and calculate the direction of the car
* @details This file contains the function prototypes for the magnetometer sensor
* and the function to calculate the direction of the car
* based on the magnetometer sensor data
* @author Woon Jun Wei
* @file magnetometer_init.h
* @author Woon Jun Wei
* @brief Initialise the magnetometer sensor and
* calculate the direction of the car
*
* @details This file contains the function prototypes for the
* magnetometer sensor and the function to calculate
* the direction of the car based on the magnetometer sensor data
*/
#ifndef MAGNETOMETER_INIT_H
@ -25,16 +27,31 @@
#include "LSM303DLHC_register.h"
// Semaphores
//SemaphoreHandle_t g_magnetometer_sem = NULL;
//SemaphoreHandle_t g_accelerometer_sem = NULL;
SemaphoreHandle_t g_direction_sem = NULL;
// Message Buffers
//MessageBufferHandle_t g_magnetometer_buffer = NULL;
//MessageBufferHandle_t g_accelerometer_buffer = NULL;
direction_t g_direction = {
.roll = 0,
.pitch = 0,
// .heading = 0,
.yaw = 0,
.orientation = NORTH,
.roll_angle = LEFT,
.pitch_angle = UP
};
/**
* @brief Initialise the LSM303DLHC sensor (Accelerometer and Magnetometer)
* @details
* Accelerometer - Normal power mode, all axes enabled, 10 Hz,
* Full Scale +-2g, continuous update
*
* Magnetometer - Continuous-conversion mode, Gain = +/- 1.3,
* Enable temperature sensor, 220 Hz
*
* @return None
*/
static void
lsm303_reset() {
LSM303DLHC_init() {
/**
* Accelerometer Setup
*/
@ -59,22 +76,23 @@ lsm303_reset() {
buf[1] = 0x00;
i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
// CRA_REG_M (0x00), 15 Hz (0x10 -> 00010000)
// buf[0] = LSM303_CRA_REG_M;
// buf[1] = 0x10;
// i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
// CRB_REG_M (0x01) - Gain = +/- 1.3 (0x20 -> 00100000)
buf[0] = LSM303_CRB_REG_M;
buf[1] = 0x20;
i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
// CRA_REG_M (0x00), 0x9C = 0b1001 1100
// Enable temperature sensor (0x80 -> 1000 0000)
// 220 Hz (0x1C -> 0001 1100)
buf[0] = LSM303_CRA_REG_M;
buf[1] = 0x9C;
i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
}
/**
* @brief Initialise the Magnetometer Sensor
* @details Initialise the I2C Port, SDA and SCL Pins, and the LSM303DLHC Sensor
*/
void
magnetometer_init()
{
@ -85,50 +103,12 @@ magnetometer_init()
gpio_pull_up(I2C_SDA);
gpio_pull_up(I2C_SCL);
lsm303_reset();
LSM303DLHC_init();
// Semaphore
// g_magnetometer_sem = xSemaphoreCreateBinary();
// g_accelerometer_sem = xSemaphoreCreateBinary();
g_direction_sem = xSemaphoreCreateBinary();
// Message Buffers
// g_magnetometer_buffer = xMessageBufferCreate(sizeof(int16_t) * 3);
// g_accelerometer_buffer = xMessageBufferCreate(sizeof(int16_t) * 3);
printf("Magnetometer initialised\n");
}
/**
* @brief Timer Interrupt Handler for the magnetometer
* @param repeatingTimer The timer handler
* @return True (To keep the timer running)
*/
//bool
//h_magnetometer_timer_handler(repeating_timer_t *repeatingTimer) {
//
// BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// xSemaphoreGiveFromISR(g_magnetometer_sem,
// &xHigherPriorityTaskWoken);
// portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
// return true;
//}
/**
* @brief Timer Interrupt Handler for the accelerometer
* @param repeatingTimer The timer handler
* @return True (To keep the timer running)
*/
//bool
//h_accelerometer_timer_handler(repeating_timer_t *repeatingTimer) {
//
// BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// xSemaphoreGiveFromISR(g_accelerometer_sem,
// &xHigherPriorityTaskWoken);
// portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
// return true;
//}
/**
* @brief Timer Interrupt Handler To calculate the direction of the car
* @param repeatingTimer The timer handler

73
frtos/magnetometer/magnetometer_read.h
View File

@ -1,5 +1,20 @@
/**
* @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"
/**
* @brief Read Data with I2C, given the address and register
* @param addr Address of the device
* @param reg Register to read from
* @return 1 piece of data read from the register
*/
static inline int
read_data(uint8_t addr, uint8_t reg) {
uint8_t data[1];
@ -13,6 +28,10 @@ read_data(uint8_t addr, uint8_t reg) {
return data[0];
}
/**
* @brief Read Accelerometer Data
* @param accelerometer Accelerometer Data
*/
static inline void
read_accelerometer(int16_t accelerometer[3]) {
uint8_t buffer[6];
@ -37,6 +56,10 @@ read_accelerometer(int16_t accelerometer[3]) {
}
/**
* @brief Read Magnetometer Data
* @param magnetometer Magnetometer Data
*/
static inline void
read_magnetometer(int16_t magnetometer[3]) {
uint8_t buffer[6];
@ -55,52 +78,4 @@ read_magnetometer(int16_t magnetometer[3]) {
magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
}
/**
* FreeRTOS Tasks
*/
//void
//monitor_magnetometer_task(__unused void *params) {
// for (;;)
// {
// if (xSemaphoreTake(g_magnetometer_sem, portMAX_DELAY) == pdTRUE)
// {
//// printf("Magnetometer Task");
// int16_t magnetometer[3];
// read_magnetometer(magnetometer);
//
// // Send to message buffer
// xMessageBufferSend(g_magnetometer_buffer,
// &magnetometer,
// sizeof(magnetometer),
// 0
// );
//
// printf("Magnetometer: %d, %d, %d\n", magnetometer[0],
// magnetometer[1], magnetometer[2]);
// }
// }
//}
//void
//monitor_accelerometer_task(__unused void *params) {
// for (;;)
// {
// if (xSemaphoreTake(g_accelerometer_sem, portMAX_DELAY) == pdTRUE)
// {
// int16_t accelerometer[3];
// read_accelerometer(accelerometer);
//
// // Send to message buffer
// xMessageBufferSend(g_accelerometer_buffer,
// &accelerometer,
// sizeof(accelerometer),
// 0
// );
//
// printf("Accelerometer: %d, %d, %d\n", accelerometer[0],
// accelerometer[1], accelerometer[2]);
// }
// }
//}
#endif

32
frtos/magnetometer/magnetometer_test.c
View File

@ -3,47 +3,17 @@
#include "magnetometer_read.h"
#include "magnetometer_direction.h"
//#define READ_MAGNETOMETER_PRIORITY (tskIDLE_PRIORITY + 2UL)
//#define READ_ACCELEROMETER_PRIORITY (tskIDLE_PRIORITY + 3UL)
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL)
void
launch()
{
struct repeating_timer g_direction_timer;
add_repeating_timer_ms(1000,
add_repeating_timer_ms(DIRECTION_READ_DELAY,
h_direction_timer_handler,
NULL,
&g_direction_timer);
// struct repeating_timer g_magnetometer_timer;
// add_repeating_timer_ms(MAGNETOMETER_READ_DELAY,
// h_magnetometer_timer_handler,
// NULL,
// &g_magnetometer_timer);
//
// struct repeating_timer g_accelerometer_timer;
// add_repeating_timer_ms(ACCELEROMETER_READ_DELAY,
// h_accelerometer_timer_handler,
// NULL,
// &g_accelerometer_timer);
// TaskHandle_t h_monitor_magnetometer_task = NULL;
// xTaskCreate(monitor_magnetometer_task,
// "Monitor Magnetometer Task",
// configMINIMAL_STACK_SIZE,
// NULL,
// READ_MAGNETOMETER_PRIORITY,
// &h_monitor_magnetometer_task);
// TaskHandle_t h_monitor_accelerometer_task = NULL;
// xTaskCreate(monitor_accelerometer_task,
// "Monitor Accelerometer Task",
// configMINIMAL_STACK_SIZE,
// NULL,
// READ_ACCELEROMETER_PRIORITY,
// &h_monitor_accelerometer_task);
TaskHandle_t h_monitor_direction_task = NULL;
xTaskCreate(monitor_direction_task,
"Monitor Direction Task",