From 32bad315644823dc51d486e0ca412263f98fbee0 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Wed, 25 Oct 2023 17:02:38 +0800 Subject: [PATCH] feature(LSM303DLHC Sensor): Added: - Accelerometer Sensor Data - Magnetometer Sensor Data - Compass Direction --- frtos/CMakeLists.txt | 2 + frtos/config/magnetometer_config.h | 11 ++ frtos/magnetometer/CMakeLists.txt | 19 +++ frtos/magnetometer/LSM303DLHC_register.h | 30 ++++ frtos/magnetometer/magnetometer_direction.h | 119 ++++++++++++++++ frtos/magnetometer/magnetometer_init.h | 146 ++++++++++++++++++++ frtos/magnetometer/magnetometer_read.h | 106 ++++++++++++++ frtos/magnetometer/magnetometer_test.c | 71 ++++++++++ 8 files changed, 504 insertions(+) create mode 100644 frtos/config/magnetometer_config.h create mode 100644 frtos/magnetometer/CMakeLists.txt create mode 100644 frtos/magnetometer/LSM303DLHC_register.h create mode 100644 frtos/magnetometer/magnetometer_direction.h create mode 100644 frtos/magnetometer/magnetometer_init.h create mode 100644 frtos/magnetometer/magnetometer_read.h create mode 100644 frtos/magnetometer/magnetometer_test.c diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index d6014bf..be93bcf 100644 --- a/frtos/CMakeLists.txt +++ b/frtos/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(motor) add_subdirectory(line_sensor) add_subdirectory(car) add_subdirectory(ultrasonic_sensor) +add_subdirectory(magnetometer) add_executable(rtos_car rtos_car.c) @@ -27,6 +28,7 @@ target_link_libraries(rtos_car FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap hardware_adc hardware_pwm + hardware_i2c ) pico_enable_stdio_usb(rtos_car 1) pico_add_extra_outputs(rtos_car) diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h new file mode 100644 index 0000000..f3fbefa --- /dev/null +++ b/frtos/config/magnetometer_config.h @@ -0,0 +1,11 @@ +#ifndef MAGNETOMETER_CONFIG_H +#define MAGNETOMETER_CONFIG_H + +#define I2C_PORT i2c0 +#define I2C_SDA ( 8 ) +#define I2C_SCL ( 9 ) + +#define MAGNETOMETER_READ_DELAY ( 1000 ) +#define ACCELEROMETER_READ_DELAY ( 1000 ) + +#endif diff --git a/frtos/magnetometer/CMakeLists.txt b/frtos/magnetometer/CMakeLists.txt new file mode 100644 index 0000000..2e95c36 --- /dev/null +++ b/frtos/magnetometer/CMakeLists.txt @@ -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) \ No newline at end of file diff --git a/frtos/magnetometer/LSM303DLHC_register.h b/frtos/magnetometer/LSM303DLHC_register.h new file mode 100644 index 0000000..c742a50 --- /dev/null +++ b/frtos/magnetometer/LSM303DLHC_register.h @@ -0,0 +1,30 @@ +#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 + +#define ACCEL_ADDR 0x19 +#define MAG_ADDR 0x1E + +#endif \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h new file mode 100644 index 0000000..9b8ca73 --- /dev/null +++ b/frtos/magnetometer/magnetometer_direction.h @@ -0,0 +1,119 @@ +#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); + + // Calculate the heading from magnetometer data + float heading = atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI); + + // Adjust the heading for negative values + if (heading < 0) + { + heading += 360.0; + } + + // Determine the direction based on the heading + // TODO: Optimize this + char *dir; + + 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"; + } + } + } + + printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading); + + return dir; +} + +/** + * FreeRTOS Tasks + */ + +void +monitor_direction_task(__unused void *params) { + for (;;) + { + if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE) + { + // Read from message buffer + int16_t magnetometer[3]; + int16_t accelerometer[3]; + + 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); + + // Calculate the heading from magnetometer data + float heading = + atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI); + + // Adjust the heading for negative values + if (heading < 0) + { + heading += 360.0; + } + + // Determine the direction based on the heading + char *dir; + + 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"; + } + } + } + + printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading); + + printf("Direction: %s\n", dir); + } + } +} diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h new file mode 100644 index 0000000..bf7d928 --- /dev/null +++ b/frtos/magnetometer/magnetometer_init.h @@ -0,0 +1,146 @@ +/** + * @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 + */ + +#ifndef MAGNETOMETER_INIT_H +#define MAGNETOMETER_INIT_H + +#include +#include +#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_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; + +static void +lsm303_reset() { + /** + * 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); + + // 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 + buf[0] = LSM303_CRA_REG_M; + buf[1] = 0x9C; + i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false); +} + +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); + + lsm303_reset(); + + // 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 + * @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 \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h new file mode 100644 index 0000000..fe7a293 --- /dev/null +++ b/frtos/magnetometer/magnetometer_read.h @@ -0,0 +1,106 @@ +#include "magnetometer_init.h" + +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, ®, 1, true); + + // Read the data + i2c_read_blocking(i2c_default, addr, data, 1, false); + + return data[0]; +} + +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]); + +} + +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 +} + +/** + * 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]); +// } +// } +//} diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c new file mode 100644 index 0000000..cc8b0ee --- /dev/null +++ b/frtos/magnetometer/magnetometer_test.c @@ -0,0 +1,71 @@ + +#include "magnetometer_init.h" +#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, + 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", + 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); +} \ No newline at end of file