From fa8e561490c51bd58d4e51c3f835abcc773eded8 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Fri, 10 Nov 2023 18:37:50 +0800 Subject: [PATCH] Updated Magnetometer Calibration Struct --- frtos/config/magnetometer_config.h | 16 +++-- frtos/magnetometer/magnetometer_direction.h | 79 +++++++++------------ frtos/magnetometer/magnetometer_init.h | 72 ++++++++----------- 3 files changed, 78 insertions(+), 89 deletions(-) diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 8da5c41..6cbc5aa 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,10 +7,11 @@ #define DIRECTION_READ_DELAY (200) -#define NUM_READINGS ( 10 ) // Number of readings to - // take before - // calculating - // direction +#define NUM_READINGS \ + (10) // Number of readings to + // take before + // calculating + // direction // #define ALPHA ( 0.1f ) // Low Pass Filter // Coefficient @@ -56,6 +57,12 @@ typedef enum RIGHT = 3 } angle_t; +typedef struct s_calibration_data +{ + int16_t accelerometerBias[3]; + int16_t magnetometerBias[3]; +} calibration_data_t; + /** * @brief The direction of the car * roll = angle of the car (left or right) @@ -71,6 +78,7 @@ typedef struct compass_direction_t orientation; angle_t roll_angle; angle_t pitch_angle; + calibration_data_t *calibration_data; } direction_t; #endif diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index bf5e1dc..1b7de60 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -310,54 +310,45 @@ updateDirection(volatile direction_t * g_direction) static int cur_x = 0; static int cur_y = 0; - read_magnetometer(magnetometer); - read_accelerometer(accelerometer); + read_magnetometer(magnetometer, g_direction); + read_accelerometer(accelerometer, g_direction); read_temperature(temperature); read_direction(accelerometer, magnetometer, g_direction); - print_orientation_data(*g_direction); - - // Temperature in degrees Celsius - // printf("Temperature: %d\n", temperature[0]); - - // print_orientation_data(); - - // printf("Direction: "); - - // print_direction(g_direction.orientation); - - switch (g_direction->orientation) - { - case NORTH: - cur_y++; - break; - case EAST: - cur_x++; - break; - case SOUTH: - cur_y--; - break; - case WEST: - cur_x--; - break; - case NORTH_EAST: - cur_x++; - cur_y++; - break; - case SOUTH_EAST: - cur_x++; - cur_y--; - break; - case SOUTH_WEST: - cur_x--; - cur_y--; - break; - case NORTH_WEST: - cur_x--; - cur_y++; - break; - } +// print_orientation_data(*g_direction); +// +// switch (g_direction->orientation) +// { +// case NORTH: +// cur_y++; +// break; +// case EAST: +// cur_x++; +// break; +// case SOUTH: +// cur_y--; +// break; +// case WEST: +// cur_x--; +// break; +// case NORTH_EAST: +// cur_x++; +// cur_y++; +// break; +// case SOUTH_EAST: +// cur_x++; +// cur_y--; +// break; +// case SOUTH_WEST: +// cur_x--; +// cur_y--; +// break; +// case NORTH_WEST: +// cur_x--; +// cur_y++; +// break; +// } // Update the map based on the direction of the car (N, E, S, W) // update_map(g_direction.orientation, cur_x, cur_y); diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index ad79f18..ef7a89d 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -29,24 +29,6 @@ // Semaphores SemaphoreHandle_t g_direction_sem = NULL; -// direction_t g_direction = { -// .roll = 0, -// .pitch = 0, -// .yaw = 0, -// .orientation = NORTH, -// .roll_angle = LEFT, -// .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 @@ -72,7 +54,7 @@ read_data(uint8_t addr, uint8_t reg) * @param accelerometer Accelerometer Data */ static inline void -read_accelerometer(int16_t accelerometer[3]) +read_accelerometer(int16_t accelerometer[3], volatile direction_t *p_direction) { uint8_t buffer[6]; @@ -95,9 +77,9 @@ read_accelerometer(int16_t accelerometer[3]) 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]; + accelerometer[0] -= p_direction->calibration_data->accelerometerBias[0]; + accelerometer[1] -= p_direction->calibration_data->accelerometerBias[1]; + accelerometer[2] -= p_direction->calibration_data->accelerometerBias[2]; } /** @@ -105,7 +87,7 @@ read_accelerometer(int16_t accelerometer[3]) * @param magnetometer Magnetometer Data */ static inline void -read_magnetometer(int16_t magnetometer[3]) +read_magnetometer(int16_t magnetometer[3], volatile direction_t *p_direction) { uint8_t buffer[6]; int32_t xMagFiltered = 0; @@ -133,9 +115,9 @@ read_magnetometer(int16_t magnetometer[3]) 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]; + magnetometer[0] -= p_direction->calibration_data->magnetometerBias[0]; + magnetometer[1] -= p_direction->calibration_data->magnetometerBias[1]; + magnetometer[2] -= p_direction->calibration_data->magnetometerBias[2]; } /** @@ -167,8 +149,13 @@ read_temperature(int16_t temperature[1]) temperature[0] = (int16_t)temperature_celsius; } +/** + * @brief Calibrate the Magnetometer Sensor via bias values + * + * @return None + */ void -initial_calibration() +initial_calibration(direction_t *p_direction) { int16_t accelerometer[3]; int16_t magnetometer[3]; @@ -184,8 +171,8 @@ initial_calibration() { printf("Calibrating... %d\n", i); - read_accelerometer(accelerometer); - read_magnetometer(magnetometer); + read_accelerometer(accelerometer, p_direction); + read_magnetometer(magnetometer, p_direction); for (int j = 0; j < 3; j++) { @@ -209,29 +196,29 @@ initial_calibration() sleep_ms(10); } - g_calibration_data.accelerometerBias[0] + p_direction->calibration_data->accelerometerBias[0] = (accelerometerMax[0] + accelerometerMin[0]) / 2; - g_calibration_data.accelerometerBias[1] + p_direction->calibration_data->accelerometerBias[1] = (accelerometerMax[1] + accelerometerMin[1]) / 2; - g_calibration_data.accelerometerBias[2] + p_direction->calibration_data->accelerometerBias[2] = (accelerometerMax[2] + accelerometerMin[2]) / 2; - g_calibration_data.magnetometerBias[0] + p_direction->calibration_data->magnetometerBias[0] = (magnetometerMax[0] + magnetometerMin[0]) / 2; - g_calibration_data.magnetometerBias[1] + p_direction->calibration_data->magnetometerBias[1] = (magnetometerMax[1] + magnetometerMin[1]) / 2; - g_calibration_data.magnetometerBias[2] + p_direction->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]); + p_direction->calibration_data->accelerometerBias[0], + p_direction->calibration_data->accelerometerBias[1], + p_direction->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]); + p_direction->calibration_data->magnetometerBias[0], + p_direction->calibration_data->magnetometerBias[1], + p_direction->calibration_data->magnetometerBias[2]); } /** @@ -298,6 +285,9 @@ magnetometer_init(car_struct_t *p_car_struct) p_car_struct->p_direction->orientation = NORTH; p_car_struct->p_direction->roll_angle = LEFT; p_car_struct->p_direction->pitch_angle = UP; + calibration_data_t g_calibration_data + = { .accelerometerBias = { 0, 0, 0 }, .magnetometerBias = { 0, 0, 0 } }; + p_car_struct->p_direction->calibration_data = &g_calibration_data; printf("Magnetometer Initialising\n"); i2c_init(I2C_PORT, 400 * 1000);