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 filter for magnetometer outliers
This commit is contained in:
Devoalda 2023-11-07 10:02:09 +08:00
parent f933d8ea91
commit d0553b6ddb
3 changed files with 38 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
frtos/config/magnetometer_config.h
View File

@ -7,8 +7,13 @@
#define DIRECTION_READ_DELAY ( 100 )
#define ALPHA ( 0.0f ) // Complementary
// Filter Constant
#define NUM_READINGS ( 10 ) // Number of readings to
// take before
// calculating
// direction
//#define ALPHA ( 0.1f ) // Low Pass Filter
// Coefficient
// LSM303DLHC temperature compensation coefficients
#define SCALE_Z ( 1.0f ) // Scale for Z-axis

23
frtos/magnetometer/magnetometer_direction.h
View File

@ -65,10 +65,10 @@ calculate_yaw_magnetometer(int16_t magnetometer[3]) {
* @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;
}
//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
@ -220,9 +220,7 @@ 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],
int16_t temperature[1]) {
read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
float roll = calculate_roll(acceleration);
float pitch = calculate_pitch(acceleration);
@ -260,7 +258,8 @@ read_direction(int16_t acceleration[3],
* @param params
*/
void print_orientation_data() {
printf("Roll: %f, Pitch: %f, Yaw: %f\n",
// printf("Roll: %f, Pitch: %f, Yaw: %f\n",
printf("%f %f %f\n",
g_direction.roll,
g_direction.pitch,
g_direction.yaw
@ -331,16 +330,16 @@ void updateDirection() {
read_accelerometer(accelerometer);
read_temperature(temperature);
read_direction(accelerometer, magnetometer, temperature);
read_direction(accelerometer, magnetometer);
// Temperature in degrees Celsius
printf("Temperature: %d\n", temperature[0]);
// printf("Temperature: %d\n", temperature[0]);
print_orientation_data();
printf("Direction: ");
// printf("Direction: ");
print_direction(g_direction.orientation);
// print_direction(g_direction.orientation);
switch (g_direction.orientation)
{

19
frtos/magnetometer/magnetometer_read.h
View File

@ -58,13 +58,17 @@ read_accelerometer(int16_t accelerometer[3]) {
}
/**
* @brief Read Magnetometer Data
* @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);
@ -72,11 +76,16 @@ read_magnetometer(int16_t magnetometer[3]) {
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
// 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]);
}
magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag
magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
// Calculate the moving average
magnetometer[0] = xMagFiltered / NUM_READINGS;
magnetometer[1] = yMagFiltered / NUM_READINGS;
magnetometer[2] = zMagFiltered / NUM_READINGS;
}
/**