Th3maz1ng/W800_Smart_Watch
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

QMC5883L magnetometer driver done

Browse Source
This commit is contained in:
anschrammh 2022-12-15 08:19:30 +01:00
parent 29d5f6ce74
commit 4667ece5c8
2 changed files with 103 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.c
View File

@ -1,11 +1,34 @@
#include <math.h>
#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;

17
src/W800 SDK v1.00.08/app/drivers/i2c/QMC5883L.h
View File

@ -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