From e0c85ce2cec192734762bb366c71fc586f45e42f Mon Sep 17 00:00:00 2001 From: Devoalda Date: Wed, 25 Oct 2023 20:46:13 +0800 Subject: [PATCH] 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) --- frtos/config/magnetometer_config.h | 44 +++- frtos/magnetometer/magnetometer_direction.h | 260 ++++++++++++++------ frtos/magnetometer/magnetometer_init.h | 92 +++---- frtos/magnetometer/magnetometer_read.h | 73 ++---- frtos/magnetometer/magnetometer_test.c | 32 +-- 5 files changed, 285 insertions(+), 216 deletions(-) diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index f3fbefa..f52a8f1 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -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 diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index 9b8ca73..089a809 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -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 \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index bf7d928..96ab986 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -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 diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h index fe7a293..49d131d 100644 --- a/frtos/magnetometer/magnetometer_read.h +++ b/frtos/magnetometer/magnetometer_read.h @@ -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 \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c index cc8b0ee..18af527 100644 --- a/frtos/magnetometer/magnetometer_test.c +++ b/frtos/magnetometer/magnetometer_test.c @@ -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",