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

fix(Magnetometer): Added option for calibration

This commit is contained in:
Devoalda 2023-11-08 14:09:54 +08:00
parent b60170a526
commit f65c0ab189
3 changed files with 198 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
frtos/magnetometer/magnetometer_direction.h
View File

@ -377,7 +377,7 @@ void updateDirection() {
// update_map(g_direction.orientation, cur_x, cur_y); // update_map(g_direction.orientation, cur_x, cur_y);
// printf("Current Position: (%d, %d)\n", cur_x, cur_y); // printf("Current Position: (%d, %d)\n", cur_x, cur_y);
// print_map(); // print_map();
// print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle); // print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle);
} }

197
frtos/magnetometer/magnetometer_init.h
View File

@ -38,6 +38,198 @@ direction_t g_direction = {
.pitch_angle = UP .pitch_angle = UP
}; };
struct s_calibration_data {
int16_t accelerometerBias[3];
int16_t magnetometerBias[3];
};
struct s_calibration_data g_calibration_data = {
.accelerometerBias = {0, 0, 0},
.magnetometerBias = {0, 0, 0}
};
/**
* @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, &reg, 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]);
// Apply the calibration data
accelerometer[0] -= g_calibration_data.accelerometerBias[0];
accelerometer[1] -= g_calibration_data.accelerometerBias[1];
accelerometer[2] -= g_calibration_data.accelerometerBias[2];
}
/**
* @brief Read Magnetometer Data with Moving Average
* @param magnetometer Magnetometer Data
*/
static inline void
read_magnetometer(int16_t magnetometer[3]) {
uint8_t buffer[6];
int32_t xMagFiltered = 0;
int32_t yMagFiltered = 0;
int32_t zMagFiltered = 0;
for (int i = 0; i < NUM_READINGS; i ++)
{
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);
// Update the cumulative sum of the magnetometer data
xMagFiltered += (int16_t) (buffer[0] << 8 | buffer[1]);
yMagFiltered += (int16_t) (buffer[2] << 8 | buffer[3]);
zMagFiltered += (int16_t) (buffer[4] << 8 | buffer[5]);
}
// Calculate the moving average
magnetometer[0] = xMagFiltered / NUM_READINGS;
magnetometer[1] = yMagFiltered / NUM_READINGS;
magnetometer[2] = zMagFiltered / NUM_READINGS;
// Apply the calibration data
magnetometer[0] -= g_calibration_data.magnetometerBias[0];
magnetometer[1] -= g_calibration_data.magnetometerBias[1];
magnetometer[2] -= g_calibration_data.magnetometerBias[2];
}
/**
* @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;
}
static void initial_calibration() {
int16_t accelerometer[3];
int16_t magnetometer[3];
int16_t accelerometerMin[3] = {0, 0, 0};
int16_t accelerometerMax[3] = {0, 0, 0};
int16_t magnetometerMin[3] = {0, 0, 0};
int16_t magnetometerMax[3] = {0, 0, 0};
printf("Initial Calibration\n");
for (int i = 0; i < 100; i ++)
{
printf("Calibrating... %d\n", i);
read_accelerometer(accelerometer);
read_magnetometer(magnetometer);
for (int j = 0; j < 3; j ++)
{
if (accelerometer[j] > accelerometerMax[j])
{
accelerometerMax[j] = accelerometer[j];
}
if (accelerometer[j] < accelerometerMin[j])
{
accelerometerMin[j] = accelerometer[j];
}
if (magnetometer[j] > magnetometerMax[j])
{
magnetometerMax[j] = magnetometer[j];
}
if (magnetometer[j] < magnetometerMin[j])
{
magnetometerMin[j] = magnetometer[j];
}
}
sleep_ms(10);
}
g_calibration_data.accelerometerBias[0] =
(accelerometerMax[0] + accelerometerMin[0]) / 2;
g_calibration_data.accelerometerBias[1] =
(accelerometerMax[1] + accelerometerMin[1]) / 2;
g_calibration_data.accelerometerBias[2] =
(accelerometerMax[2] + accelerometerMin[2]) / 2;
g_calibration_data.magnetometerBias[0] =
(magnetometerMax[0] + magnetometerMin[0]) / 2;
g_calibration_data.magnetometerBias[1] =
(magnetometerMax[1] + magnetometerMin[1]) / 2;
g_calibration_data.magnetometerBias[2] =
(magnetometerMax[2] + magnetometerMin[2]) / 2;
printf("Accelerometer Bias: %d, %d, %d\n",
g_calibration_data.accelerometerBias[0],
g_calibration_data.accelerometerBias[1],
g_calibration_data.accelerometerBias[2]);
printf("Magnetometer Bias: %d, %d, %d\n",
g_calibration_data.magnetometerBias[0],
g_calibration_data.magnetometerBias[1],
g_calibration_data.magnetometerBias[2]);
}
/** /**
* @brief Initialise the LSM303DLHC sensor (Accelerometer and Magnetometer) * @brief Initialise the LSM303DLHC sensor (Accelerometer and Magnetometer)
* @details * @details
@ -104,6 +296,10 @@ magnetometer_init()
LSM303DLHC_init(); LSM303DLHC_init();
// initial_calibration();
// sleep_ms(3000);
// printf("Magnetometer Initialised\n");
// Semaphore // Semaphore
g_direction_sem = xSemaphoreCreateBinary(); g_direction_sem = xSemaphoreCreateBinary();
} }
@ -122,4 +318,5 @@ h_direction_timer_handler(repeating_timer_t *repeatingTimer) {
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true; return true;
} }
#endif #endif

105
frtos/magnetometer/magnetometer_read.h
View File

@ -10,110 +10,5 @@
#include "magnetometer_init.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, &reg, 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 with Moving Average
* @param magnetometer Magnetometer Data
*/
static inline void
read_magnetometer(int16_t magnetometer[3]) {
uint8_t buffer[6];
int32_t xMagFiltered = 0;
int32_t yMagFiltered = 0;
int32_t zMagFiltered = 0;
for (int i = 0; i < NUM_READINGS; i++) {
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);
// Update the cumulative sum of the magnetometer data
xMagFiltered += (int16_t)(buffer[0] << 8 | buffer[1]);
yMagFiltered += (int16_t)(buffer[2] << 8 | buffer[3]);
zMagFiltered += (int16_t)(buffer[4] << 8 | buffer[5]);
}
// Calculate the moving average
magnetometer[0] = xMagFiltered / NUM_READINGS;
magnetometer[1] = yMagFiltered / NUM_READINGS;
magnetometer[2] = zMagFiltered / NUM_READINGS;
}
/**
* @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 #endif