From 4667ece5c8c946a92daa0ba97c4337f869476ba5 Mon Sep 17 00:00:00 2001 From: anschrammh Date: Thu, 15 Dec 2022 08:19:30 +0100 Subject: [PATCH] QMC5883L magnetometer driver done --- .../app/drivers/i2c/QMC5883L.c | 90 ++++++++++++++++++- .../app/drivers/i2c/QMC5883L.h | 17 +++- 2 files changed, 103 insertions(+), 4 deletions(-) diff --git a/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.c b/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.c index d1e356f..07ab202 100644 --- a/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.c +++ b/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.c @@ -1,11 +1,34 @@ +#include #include "i2c.h" #include "QMC5883L.h" +#ifndef PI +#define PI 3.14159265358979f +#endif + +typedef struct +{ + int16_t x_min; + int16_t x_max; + int16_t y_min; + int16_t y_max; + int16_t z_min; + int16_t z_max; +} _QMC5883L_calibration_data; + +static _QMC5883L_calibration_data calibration_data = {0}; + + bool QMC5883L_init(void) { return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_SETRES_REG, 0x01); } +bool QMC5883L_software_reset(void) +{ + return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL2_REG, 0x80); +} + float QMC5883L_get_temperature(void) { uint8_t data; @@ -31,7 +54,17 @@ bool QMC5883L_is_data_available(void) return data & QMC5883L_DRDY_BIT ? true : false; } -QMC5883L_MData_t QMC5883L_get_MFields(void) +void QMC5883L_set_calibration_data(int16_t x_min, int16_t x_max, int16_t y_min, int16_t y_max, int16_t z_min, int16_t z_max) +{ + calibration_data.x_min = x_min; + calibration_data.x_max = x_max; + calibration_data.y_min = y_min; + calibration_data.y_max = y_max; + calibration_data.z_min = z_min; + calibration_data.z_max = z_max; +} + +QMC5883L_MData_t QMC5883L_get_MFields_raw(void) { uint8_t data[6]; QMC5883L_MData_t Mdata = {.MFieldX = -1, .MFieldY = -1, .MFieldZ = -1}; @@ -46,6 +79,61 @@ QMC5883L_MData_t QMC5883L_get_MFields(void) return Mdata; } +QMC5883L_MData_calibrated_t QMC5883L_get_MFields_calibrated(void) +{ + uint8_t data[6]; + QMC5883L_MData_calibrated_t Mdata = {.MFieldX = -1, .MFieldY = -1, .MFieldZ = -1}; + + if(i2c_read(QMC5883L_I2C_ADDR, QMC5883L_DATA_X_LSB_REG, data, sizeof data)) + { + Mdata.MFieldX = (data[1] << 8) | data[0]; + Mdata.MFieldY = (data[3] << 8) | data[2]; + Mdata.MFieldZ = (data[5] << 8) | data[4]; + } + + int x_offset = (calibration_data.x_min + calibration_data.x_max) / 2; + int y_offset = (calibration_data.y_min + calibration_data.y_max) / 2; + int z_offset = (calibration_data.z_min + calibration_data.z_max) / 2; + int x_avg_delta = (calibration_data.x_max - calibration_data.x_min) / 2; + int y_avg_delta = (calibration_data.y_max - calibration_data.y_min) / 2; + int z_avg_delta = (calibration_data.z_max - calibration_data.z_min) / 2; + + int avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta) / 3; + + float x_scale = (float)avg_delta / x_avg_delta; + float y_scale = (float)avg_delta / y_avg_delta; + float z_scale = (float)avg_delta / z_avg_delta; + + Mdata.MFieldX = (Mdata.MFieldX - x_offset) * x_scale; + Mdata.MFieldY = (Mdata.MFieldY - y_offset) * y_scale; + Mdata.MFieldZ = (Mdata.MFieldZ - z_offset) * z_scale; + + return Mdata; +} + +int16_t QMC5883L_get_azimuth(const QMC5883L_MData_calibrated_t data) +{ + int16_t angle = atan2(data.MFieldY, data.MFieldX) * 180.0 / PI; + return angle < 0 ? 360 + angle : angle; +} + +bool QMC5883L_configure_1(QMC5883L_Output_Data_Rate_e ODR, QMC5883L_Full_Scale_e RNG, QMC5883L_Over_Sample_Ratio_e OSR) +{ + uint8_t data; + + if(!i2c_read_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL1_REG, &data))return false; + + data &= 0x03; + data |= OSR << 6 | RNG << 4 | ODR << 2; + + return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL1_REG, data); +} + +bool QMC5883L_configure_2(bool rolling_ptr, bool data_ready_int) +{ + return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL2_REG, rolling_ptr << 6 | data_ready_int); +} + bool QMC5883L_set_power_mode(QMC5883L_Mode_Control_e MC) { uint8_t data; diff --git a/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.h b/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.h index e7004a7..f52ca7c 100644 --- a/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.h +++ b/src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.h @@ -55,9 +55,15 @@ typedef struct int16_t MFieldZ; } QMC5883L_MData_t; +typedef struct +{ + int16_t MFieldX; + int16_t MFieldY; + int16_t MFieldZ; +} QMC5883L_MData_calibrated_t; + /* QMC5883L Driver API */ - bool QMC5883L_init(void); bool QMC5883L_software_reset(void); @@ -66,7 +72,13 @@ float QMC5883L_get_temperature(void); bool QMC5883L_is_data_available(void); -QMC5883L_MData_t QMC5883L_get_MFields(void); +void QMC5883L_set_calibration_data(int16_t x_min, int16_t x_max, int16_t y_min, int16_t y_max, int16_t z_min, int16_t z_max); + +QMC5883L_MData_t QMC5883L_get_MFields_raw(void); + +QMC5883L_MData_calibrated_t QMC5883L_get_MFields_calibrated(void); + +int16_t QMC5883L_get_azimuth(const QMC5883L_MData_calibrated_t data); bool QMC5883L_configure_1(QMC5883L_Output_Data_Rate_e ODR, QMC5883L_Full_Scale_e RNG, QMC5883L_Over_Sample_Ratio_e OSR); @@ -74,5 +86,4 @@ bool QMC5883L_configure_2(bool rolling_ptr, bool data_ready_int); bool QMC5883L_set_power_mode(QMC5883L_Mode_Control_e MC); - #endif //QMC5883L_H \ No newline at end of file