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..1eed5f3 --- /dev/null +++ b/frtos/config/magnetometer_config.h @@ -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 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..5b5940f --- /dev/null +++ b/frtos/magnetometer/LSM303DLHC_register.h @@ -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 \ 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..441e0cb --- /dev/null +++ b/frtos/magnetometer/magnetometer_direction.h @@ -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 \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h new file mode 100644 index 0000000..96ab986 --- /dev/null +++ b/frtos/magnetometer/magnetometer_init.h @@ -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 +#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_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 \ 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..fde4b0b --- /dev/null +++ b/frtos/magnetometer/magnetometer_read.h @@ -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, ®, 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 \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c new file mode 100644 index 0000000..18af527 --- /dev/null +++ b/frtos/magnetometer/magnetometer_test.c @@ -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); +} \ No newline at end of file