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 DIRECTION_READ_DELAY ( 100 )
#define ALPHA ( 0.0f ) // Complementary #define NUM_READINGS ( 10 ) // Number of readings to
// Filter Constant // take before
// calculating
// direction
//#define ALPHA ( 0.1f ) // Low Pass Filter
// Coefficient
// LSM303DLHC temperature compensation coefficients // LSM303DLHC temperature compensation coefficients
#define SCALE_Z ( 1.0f ) // Scale for Z-axis #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 * @param yaw_mag Yaw calculated from Magnetometer Data
* @return yaw Yaw calculated from Complementary Filter * @return yaw Yaw calculated from Complementary Filter
*/ */
static inline float //static inline float
calculate_yaw_complementary(float yaw_acc, float yaw_mag) { //calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag; // return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
} //}
/** /**
* @brief Compensate the magnetometer readings for temperature * @brief Compensate the magnetometer readings for temperature
@ -220,9 +220,7 @@ update_orientation_data(float roll, float pitch, float yaw,
* @param magnetometer Magnetometer Data * @param magnetometer Magnetometer Data
*/ */
static void static void
read_direction(int16_t acceleration[3], read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
int16_t magnetometer[3],
int16_t temperature[1]) {
float roll = calculate_roll(acceleration); float roll = calculate_roll(acceleration);
float pitch = calculate_pitch(acceleration); float pitch = calculate_pitch(acceleration);
@ -260,7 +258,8 @@ read_direction(int16_t acceleration[3],
* @param params * @param params
*/ */
void print_orientation_data() { 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.roll,
g_direction.pitch, g_direction.pitch,
g_direction.yaw g_direction.yaw
@ -331,16 +330,16 @@ void updateDirection() {
read_accelerometer(accelerometer); read_accelerometer(accelerometer);
read_temperature(temperature); read_temperature(temperature);
read_direction(accelerometer, magnetometer, temperature); read_direction(accelerometer, magnetometer);
// Temperature in degrees Celsius // Temperature in degrees Celsius
printf("Temperature: %d\n", temperature[0]); // printf("Temperature: %d\n", temperature[0]);
print_orientation_data(); print_orientation_data();
printf("Direction: "); // printf("Direction: ");
print_direction(g_direction.orientation); // print_direction(g_direction.orientation);
switch (g_direction.orientation) switch (g_direction.orientation)
{ {

31
frtos/magnetometer/magnetometer_read.h
View File

@ -58,25 +58,34 @@ read_accelerometer(int16_t accelerometer[3]) {
} }
/** /**
* @brief Read Magnetometer Data * @brief Read Magnetometer Data with Moving Average
* @param magnetometer Magnetometer Data * @param magnetometer Magnetometer Data
*/ */
static inline void static inline void
read_magnetometer(int16_t magnetometer[3]) { read_magnetometer(int16_t magnetometer[3]) {
uint8_t buffer[6]; uint8_t buffer[6];
int32_t xMagFiltered = 0;
int32_t yMagFiltered = 0;
int32_t zMagFiltered = 0;
buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M); for (int i = 0; i < NUM_READINGS; i++) {
buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M); buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M); buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M);
buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M); buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M);
buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M); buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M);
buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_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 // 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 // Calculate the moving average
magnetometer[0] = xMagFiltered / NUM_READINGS;
magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag magnetometer[1] = yMagFiltered / NUM_READINGS;
magnetometer[2] = zMagFiltered / NUM_READINGS;
} }
/** /**