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

feature(LSM303DLHC): Added Temperature Compensation 

Added:
- Temperature reading
- Temperature compensation for yaw
- Modified Alpha to put more weight on magnetometer
This commit is contained in:
Devoalda 2023-10-26 07:55:43 +08:00
parent 0f0b0a9886
commit 237e370875
4 changed files with 178 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
frtos/config/magnetometer_config.h
View File

@ -7,7 +7,7 @@
#define DIRECTION_READ_DELAY ( 100 )
#define ALPHA ( 0.98f ) // Complementary
#define ALPHA ( 0.01f ) // Complementary
// Filter Constant
/**

14
frtos/magnetometer/LSM303DLHC_register.h
View File

@ -24,6 +24,20 @@
#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

127
frtos/magnetometer/magnetometer_direction.h
View File

@ -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,27 +193,16 @@ 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) {
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);
read_direction(accelerometer, magnetometer);
void print_orientation_data() {
printf("Roll: %f, Pitch: %f, Yaw: %f\n",
g_direction.roll, g_direction.pitch, g_direction.yaw);
g_direction.roll,
g_direction.pitch,
g_direction.yaw
);
}
printf("Direction:");
switch (g_direction.orientation)
void print_direction(compass_direction_t direction) {
switch (direction)
{
case NORTH:
printf("North\n");
@ -203,27 +229,56 @@ monitor_direction_task(__unused void *params) {
printf("North West\n");
break;
}
}
switch (g_direction.roll_angle)
void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) {
switch (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:
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);
}
}
}

29
frtos/magnetometer/magnetometer_read.h
View File

@ -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