From 237e37087519c39df56c15a534a5ec858acb7cc9 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Thu, 26 Oct 2023 07:55:43 +0800 Subject: [PATCH] feature(LSM303DLHC): Added Temperature Compensation Added: - Temperature reading - Temperature compensation for yaw - Modified Alpha to put more weight on magnetometer --- frtos/config/magnetometer_config.h | 2 +- frtos/magnetometer/LSM303DLHC_register.h | 56 ++++--- frtos/magnetometer/magnetometer_direction.h | 171 +++++++++++++------- frtos/magnetometer/magnetometer_read.h | 29 ++++ 4 files changed, 178 insertions(+), 80 deletions(-) diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 7add6ab..1eed5f3 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,7 +7,7 @@ #define DIRECTION_READ_DELAY ( 100 ) -#define ALPHA ( 0.98f ) // Complementary +#define ALPHA ( 0.01f ) // Complementary // Filter Constant /** diff --git a/frtos/magnetometer/LSM303DLHC_register.h b/frtos/magnetometer/LSM303DLHC_register.h index c742a50..5b5940f 100644 --- a/frtos/magnetometer/LSM303DLHC_register.h +++ b/frtos/magnetometer/LSM303DLHC_register.h @@ -2,29 +2,43 @@ #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 +#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 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 +// 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 index d83ffa3..441e0cb 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -16,6 +16,10 @@ * 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 @@ -40,9 +44,10 @@ calculate_roll(int16_t acceleration[3]) { */ 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); + return atan2(- acceleration[0], + sqrt((acceleration[1] * acceleration[1]) + + (acceleration[2] * acceleration[2])) + ) * (180.0 / M_PI); } /** @@ -66,6 +71,27 @@ 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 @@ -80,6 +106,8 @@ adjust_yaw(float 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 */ @@ -120,7 +148,9 @@ 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; } @@ -133,13 +163,20 @@ update_orientation_data(float roll, float pitch, float yaw, * @param magnetometer Magnetometer Data */ static void -read_direction(int16_t acceleration[3], int16_t magnetometer[3]) { +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, yaw_mag); + float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw); yaw = adjust_yaw(yaw); @@ -156,74 +193,92 @@ read_direction(int16_t acceleration[3], int16_t magnetometer[3]) { * @brief Task to Monitor the Direction of the Car * @param params */ -void -monitor_direction_task(__unused void *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) { - // Read from message buffer 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); + read_direction(accelerometer, magnetometer, temperature); - printf("Roll: %f, Pitch: %f, Yaw: %f\n", - g_direction.roll, g_direction.pitch, g_direction.yaw); + // Temperature in degrees Celsius + printf("Temperature: %d\n", temperature[0]); - printf("Direction:"); + print_orientation_data(); - switch (g_direction.orientation) - { - 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; - } + printf("Direction: "); - switch (g_direction.roll_angle) - { - case LEFT: - printf("You're Flying!\n"); - break; - case RIGHT: - printf("You're Plunging!\n"); - break; - } - - 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; - } + print_direction(g_direction.orientation); + print_roll_and_pitch(g_direction.roll_angle, + g_direction.pitch_angle); } } } diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h index 49d131d..fde4b0b 100644 --- a/frtos/magnetometer/magnetometer_read.h +++ b/frtos/magnetometer/magnetometer_read.h @@ -7,6 +7,7 @@ #ifndef MAGNETOMETER_READ_H #define MAGNETOMETER_READ_H + #include "magnetometer_init.h" /** @@ -78,4 +79,32 @@ read_magnetometer(int16_t magnetometer[3]) { 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