From f65c0ab1896214c87d04b8241aa259f048772b5f Mon Sep 17 00:00:00 2001 From: Devoalda Date: Wed, 8 Nov 2023 14:09:54 +0800 Subject: [PATCH] fix(Magnetometer): Added option for calibration --- frtos/magnetometer/magnetometer_direction.h | 2 +- frtos/magnetometer/magnetometer_init.h | 197 ++++++++++++++++++++ frtos/magnetometer/magnetometer_read.h | 105 ----------- 3 files changed, 198 insertions(+), 106 deletions(-) diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index 2cabc97..d9dd5c8 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -377,7 +377,7 @@ void updateDirection() { // update_map(g_direction.orientation, 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); } diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index 1de1432..fdb1b0b 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -38,6 +38,198 @@ direction_t g_direction = { .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, ®, 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) * @details @@ -104,6 +296,10 @@ magnetometer_init() LSM303DLHC_init(); +// initial_calibration(); + +// sleep_ms(3000); +// printf("Magnetometer Initialised\n"); // Semaphore g_direction_sem = xSemaphoreCreateBinary(); } @@ -122,4 +318,5 @@ h_direction_timer_handler(repeating_timer_t *repeatingTimer) { portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return true; } + #endif \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h index 36f3f35..8f8911b 100644 --- a/frtos/magnetometer/magnetometer_read.h +++ b/frtos/magnetometer/magnetometer_read.h @@ -10,110 +10,5 @@ #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, ®, 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 \ No newline at end of file