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

Merge pull request #6 from Devoalda/main

This commit is contained in:
Richie Wang 2023-10-26 08:08:02 +08:00 committed by GitHub
parent 3b7831297a 237e370875
commit 1e34a8259f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 682 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
frtos/CMakeLists.txt
View File

@ -4,6 +4,7 @@ add_subdirectory(motor)
add_subdirectory(line_sensor) add_subdirectory(line_sensor)
add_subdirectory(car) add_subdirectory(car)
add_subdirectory(ultrasonic_sensor) add_subdirectory(ultrasonic_sensor)
add_subdirectory(magnetometer)
add_executable(rtos_car rtos_car.c) add_executable(rtos_car rtos_car.c)
@ -27,6 +28,7 @@ target_link_libraries(rtos_car
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
hardware_adc hardware_adc
hardware_pwm hardware_pwm
hardware_i2c
) )
pico_enable_stdio_usb(rtos_car 1) pico_enable_stdio_usb(rtos_car 1)
pico_add_extra_outputs(rtos_car) pico_add_extra_outputs(rtos_car)

54
frtos/config/magnetometer_config.h Normal file
View File

@ -0,0 +1,54 @@
#ifndef MAGNETOMETER_CONFIG_H
#define MAGNETOMETER_CONFIG_H
#define I2C_PORT i2c0
#define I2C_SDA ( 8 )
#define I2C_SCL ( 9 )
#define DIRECTION_READ_DELAY ( 100 )
#define ALPHA ( 0.01f ) // Complementary
// Filter Constant
/**
* @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

19
frtos/magnetometer/CMakeLists.txt Normal file
View File

@ -0,0 +1,19 @@
add_executable(
magnetometer_test
magnetometer_test.c
)
target_link_libraries(
magnetometer_test
hardware_i2c
pico_stdlib
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
)
target_include_directories(
magnetometer_test
PRIVATE ../config
)
pico_enable_stdio_usb(magnetometer_test 1)
pico_add_extra_outputs(magnetometer_test)

44
frtos/magnetometer/LSM303DLHC_register.h Normal file
View File

@ -0,0 +1,44 @@
#ifndef MAGNETOMETER_REGISTER_H
#define MAGNETOMETER_REGISTER_H
// Accelerometer registers
#define LSM303_CTRL_REG1_A 0x20
#define LSM303_CTRL_REG4_A 0x23
#define LSM303_CTRL_REG5_A 0x24
#define LSM303_OUT_X_L_A 0x28
#define LSM303_OUT_X_H_A 0x29
#define LSM303_OUT_Y_L_A 0x2A
#define LSM303_OUT_Y_H_A 0x2B
#define LSM303_OUT_Z_L_A 0x2C
#define LSM303_OUT_Z_H_A 0x2D
// Magnetometer registers
#define LSM303_CRA_REG_M 0x00
#define LSM303_CRB_REG_M 0x01
#define LSM303_MR_REG_M 0x02
#define LSM303_OUT_X_H_M 0x03
#define LSM303_OUT_X_L_M 0x04
#define LSM303_OUT_Z_H_M 0x05
#define LSM303_OUT_Z_L_M 0x06
#define LSM303_OUT_Y_H_M 0x07
#define LSM303_OUT_Y_L_M 0x08
#define LSM303_SR_REG_M 0x09
// Temperature registers
#define LSM303_TEMP_OUT_H_M 0x31
#define LSM303_TEMP_OUT_L_M 0x32
// LSM303DLHC temperature compensation coefficients
#define SCALE_Z 0.9 // Scale factor for Z-axis
#define OFFSET_Z 5.0 // Offset for Z-axis
#define TEMPERATURE_OFFSET 25.0 // Reference temperature for calibration
#define TEMPERATURE_COEFFICIENT_Z 0.33
#define OFFSET_Z 5.0
#define SCALE_Z 0.9
#define ACCEL_ADDR 0x19
#define MAG_ADDR 0x1E
#endif

286
frtos/magnetometer/magnetometer_direction.h Normal file
View File

@ -0,0 +1,286 @@
/**
* @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
*
* Source:
* https://www.nxp.com/docs/en/application-note/AN3461.pdf
* https://ahrs.readthedocs.io/en/latest/filters/complementary.html
*
*/
#ifndef MAGNETOMETER_DIRECTION_H
#define MAGNETOMETER_DIRECTION_H
#include "magnetometer_init.h"
/**
* @brief Roll Calculation with Accelerometer Data
* @param acceleration Accelerometer Data
* @return
*/
static inline float
calculate_roll(int16_t acceleration[3]) {
return atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
}
/**
* @brief Pitch Calculation with Accelerometer Data
* @param acceleration Accelerometer Data
* @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);
}
/**
* @brief Yaw Calculation with Magnetometer Data
* @param magnetometer Magnetometer Data
* @return
*/
static inline float
calculate_yaw_magnetometer(int16_t magnetometer[3]) {
return atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
}
/**
* @brief Complementary Filter for Yaw
* @param yaw_acc Yaw calculated from Accelerometer Data
* @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;
}
/**
* @brief Compensate the magnetometer readings for temperature
* @param yaw_mag Yaw calculated from Magnetometer Data
* @param temperature Temperature in degrees Celsius
* @return Compensated Yaw
*/
float
compensate_magnetometer(float yaw_mag, int16_t temperature) {
// Calculate temperature difference from the reference temperature
float delta_temp = (float) (temperature - TEMPERATURE_OFFSET);
// Apply temperature compensation to each axis using macros
float compensated_yaw_mag =
yaw_mag - (delta_temp * TEMPERATURE_COEFFICIENT_Z);
// Apply scale and offset corrections using macros
compensated_yaw_mag = (compensated_yaw_mag - OFFSET_Z) * SCALE_Z;
return compensated_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
*/
static inline 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
* 45.0 = 360 / 8, used to calculate the compass direction from
* the orientation (0 - 7)
* @param yaw Yaw calculated from Complementary Filter
* @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)
{
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;
}
}
/**
* @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
*/
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;
}
/**
* @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],
int16_t temperature[1]) {
float roll = calculate_roll(acceleration);
float pitch = calculate_pitch(acceleration);
float yaw_mag = calculate_yaw_magnetometer(magnetometer);
// Apply temperature compensation to the magnetometer data
float compensated_mag_yaw = compensate_magnetometer(yaw_mag,
temperature[0]);
float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI);
float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw);
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 print_orientation_data() {
printf("Roll: %f, Pitch: %f, Yaw: %f\n",
g_direction.roll,
g_direction.pitch,
g_direction.yaw
);
}
void print_direction(compass_direction_t direction) {
switch (direction)
{
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;
}
}
void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) {
switch (roll_angle)
{
case LEFT:
printf("Your left wheel is in the air!\n");
break;
case RIGHT:
printf("Your right wheel is in the air!\n");
break;
}
switch (pitch_angle)
{
case UP:
printf("You're Flying!\n");
break;
case DOWN:
printf("You're Plunging!\n");
break;
}
}
void monitor_direction_task(__unused void *params) {
for (;;)
{
if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
{
int16_t magnetometer[3];
int16_t accelerometer[3];
int16_t temperature[1];
read_magnetometer(magnetometer);
read_accelerometer(accelerometer);
read_temperature(temperature);
read_direction(accelerometer, magnetometer, temperature);
// Temperature in degrees Celsius
printf("Temperature: %d\n", temperature[0]);
print_orientation_data();
printf("Direction: ");
print_direction(g_direction.orientation);
print_roll_and_pitch(g_direction.roll_angle,
g_direction.pitch_angle);
}
}
}
#endif

126
frtos/magnetometer/magnetometer_init.h Normal file
View File

@ -0,0 +1,126 @@
/**
* @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
#define MAGNETOMETER_INIT_H
#include <stdio.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include "pico/binary_info.h"
#include "FreeRTOS.h"
#include "task.h"
#include "message_buffer.h"
#include "semphr.h"
#include "magnetometer_config.h"
#include "LSM303DLHC_register.h"
// Semaphores
SemaphoreHandle_t g_direction_sem = 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
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};
i2c_write_blocking(i2c_default, ACCEL_ADDR, buf, 2, false);
// Reboot memory content (0x40 = CTRL_REG4_A)
// Full Scale +-2g, continuous update (0x00 = 0b0000 0000)
buf[0] = LSM303_CTRL_REG4_A;
buf[1] = 0x00;
i2c_write_blocking(i2c_default, ACCEL_ADDR, buf, 2, false);
/**
* Magnetometer Setup
*/
// MR_REG_M (0x02) - Continuous-conversion mode (0x00 -> 00000000)
buf[0] = LSM303_MR_REG_M;
buf[1] = 0x00;
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()
{
i2c_init(I2C_PORT, 400 * 1000);
gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
gpio_pull_up(I2C_SDA);
gpio_pull_up(I2C_SCL);
LSM303DLHC_init();
// Semaphore
g_direction_sem = xSemaphoreCreateBinary();
}
/**
* @brief Timer Interrupt Handler To calculate the direction of the car
* @param repeatingTimer The timer handler
* @return True (To keep the timer running)
*/
bool
h_direction_timer_handler(repeating_timer_t *repeatingTimer) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_direction_sem,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true;
}
#endif

110
frtos/magnetometer/magnetometer_read.h Normal file
View File

@ -0,0 +1,110 @@
/**
* @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];
// Send the register address to read from
i2c_write_blocking(i2c_default, addr, &reg, 1, true);
// Read the data
i2c_read_blocking(i2c_default, addr, data, 1, false);
return data[0];
}
/**
* @brief Read Accelerometer Data
* @param accelerometer Accelerometer Data
*/
static inline void
read_accelerometer(int16_t accelerometer[3]) {
uint8_t buffer[6];
buffer[0] = read_data(ACCEL_ADDR, LSM303_OUT_X_L_A);
buffer[1] = read_data(ACCEL_ADDR, LSM303_OUT_X_H_A);
buffer[2] = read_data(ACCEL_ADDR, LSM303_OUT_Y_L_A);
buffer[3] = read_data(ACCEL_ADDR, LSM303_OUT_Y_H_A);
buffer[4] = read_data(ACCEL_ADDR, LSM303_OUT_Z_L_A);
buffer[5] = read_data(ACCEL_ADDR, LSM303_OUT_Z_H_A);
// Combine high and low bytes
// xAcceleration
accelerometer[0] = (int16_t) ((buffer[1] << 8) | buffer[0]);
// yAcceleration
accelerometer[1] = (int16_t) ((buffer[3] << 8) | buffer[2]);
// zAcceleration
accelerometer[2] = (int16_t) ((buffer[5] << 8) | buffer[4]);
}
/**
* @brief Read Magnetometer Data
* @param magnetometer Magnetometer Data
*/
static inline void
read_magnetometer(int16_t magnetometer[3]) {
uint8_t buffer[6];
buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M);
buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M);
buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M);
buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M);
buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M);
magnetometer[0] = (int16_t) (buffer[0] << 8 | buffer[1]); //xMag
magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag
magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
}
/**
* @brief Read Temperature Data in Degrees Celsius
* @param temperature Temperature Data in Degrees Celsius
*/
static inline void
read_temperature(int16_t temperature[1]) {
uint8_t buffer[2];
buffer[0] = read_data(MAG_ADDR, LSM303_TEMP_OUT_H_M);
buffer[1] = read_data(MAG_ADDR, LSM303_TEMP_OUT_L_M);
/**
* Normalize temperature; it is big-endian, fixed-point
* 9 bits signed integer, 3 bits fractional part, 4 bits zeros
* and is relative to 20 degrees Celsius
* Source: https://electronics.stackexchange.com/a/356964
*/
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;
// Store the result in the temperature array
temperature[0] = (int16_t) temperature_celsius;
}
#endif

41
frtos/magnetometer/magnetometer_test.c Normal file
View File

@ -0,0 +1,41 @@
#include "magnetometer_init.h"
#include "magnetometer_read.h"
#include "magnetometer_direction.h"
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL)
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,
"Monitor Direction Task",
configMINIMAL_STACK_SIZE,
NULL,
DIRECTION_TASK_PRIORITY,
&h_monitor_direction_task);
vTaskStartScheduler();
}
int
main (void)
{
stdio_usb_init();
sleep_ms(2000);
printf("Test started!\n");
magnetometer_init();
launch();
return(0);
}