You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
271 lines
8.4 KiB
271 lines
8.4 KiB
/* |
|
* lis2mdl.c |
|
* The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 10 DoF |
|
* absolute orientation solution. |
|
* |
|
* Created on: Jan 18, 2021 |
|
* Author: Daniel Peter Chokola |
|
* |
|
* Adapted From: |
|
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
|
* by: Kris Winer |
|
* 09/23/2017 Copyright Tlera Corporation |
|
* |
|
* Library may be used freely and without limit with attribution. |
|
*/ |
|
|
|
/* Includes */ |
|
#include <imu_common.h> |
|
#include <lis2mdl.h> |
|
#include <stddef.h> |
|
|
|
/* Definitions */ |
|
/* |
|
* Register map for LIS2MDL |
|
* http://www.st.com/content/ccc/resource/technical/document/datasheet/group3/29/13/d1/e0/9a/4d/4f/30/DM00395193/files/DM00395193.pdf/jcr:content/translations/en.DM00395193.pdf |
|
*/ |
|
#define LIS2MDL_OFFSET_X_REG_L 0x45 |
|
#define LIS2MDL_OFFSET_X_REG_H 0x46 |
|
#define LIS2MDL_OFFSET_Y_REG_L 0x47 |
|
#define LIS2MDL_OFFSET_Y_REG_H 0x48 |
|
#define LIS2MDL_OFFSET_Z_REG_L 0x49 |
|
#define LIS2MDL_OFFSET_Z_REG_H 0x4A |
|
#define LIS2MDL_WHO_AM_I 0x4F |
|
#define LIS2MDL_CFG_REG_A 0x60 |
|
#define LIS2MDL_CFG_REG_B 0x61 |
|
#define LIS2MDL_CFG_REG_C 0x62 |
|
#define LIS2MDL_INT_CTRL_REG 0x63 |
|
#define LIS2MDL_INT_SOURCE_REG 0x64 |
|
#define LIS2MDL_INT_THS_L_REG 0x65 |
|
#define LIS2MDL_INT_THS_H_REG 0x66 |
|
#define LIS2MDL_STATUS_REG 0x67 |
|
#define LIS2MDL_OUTX_L_REG 0x68 |
|
#define LIS2MDL_OUTX_H_REG 0x69 |
|
#define LIS2MDL_OUTY_L_REG 0x6A |
|
#define LIS2MDL_OUTY_H_REG 0x6B |
|
#define LIS2MDL_OUTZ_L_REG 0x6C |
|
#define LIS2MDL_OUTZ_H_REG 0x6D |
|
#define LIS2MDL_TEMP_OUT_L_REG 0x6E |
|
#define LIS2MDL_TEMP_OUT_H_REG 0x6F |
|
|
|
/* Macros */ |
|
#define lis2mdl_read_byte(addr, byte) i2c_read_byte((lis2mdl->i2c_read_func), (lis2mdl->i2c_addr), (addr), (byte)) |
|
#define lis2mdl_write_byte(addr, byte) i2c_write_byte((lis2mdl->i2c_write_func), (lis2mdl->i2c_addr), (addr), (byte)) |
|
#define lis2mdl_read(addr, data, len) i2c_read((lis2mdl->i2c_read_func), (lis2mdl->i2c_addr), (addr), (data), (len)) |
|
#define lis2mdl_write(addr, data, len) i2c_write((lis2mdl->i2c_write_func), (lis2mdl->i2c_addr), (addr), (data), (len)) |
|
|
|
/* Private Global Variables */ |
|
|
|
/* Function Prototypes */ |
|
|
|
/* Function Definitions */ |
|
lis2mdl_status_t lis2mdl_init(lis2mdl_t *lis2mdl, lis2mdl_init_t *init) |
|
{ |
|
int8_t *ptr = (int8_t*) lis2mdl; |
|
size_t i; |
|
|
|
return_val_if_fail(lis2mdl, LIS2MDL_BAD_ARG); |
|
return_val_if_fail(init, LIS2MDL_BAD_ARG); |
|
|
|
/* zero lis2mdl_t struct */ |
|
for(i = 0; i < sizeof(lis2mdl_t); i++) |
|
{ |
|
*ptr++ = 0; |
|
} |
|
|
|
lis2mdl->init = init; |
|
|
|
return LIS2MDL_OK; |
|
} |
|
|
|
void lis2mdl_set_delay_cb(lis2mdl_t *lis2mdl, delay_func_t delay_func) |
|
{ |
|
return_if_fail(lis2mdl); |
|
|
|
lis2mdl->delay_func = delay_func; |
|
} |
|
|
|
void lis2mdl_set_i2c_cbs(lis2mdl_t *lis2mdl, i2c_read_func_t i2c_read_func, |
|
i2c_write_func_t i2c_write_func, uint8_t dev_addr) |
|
{ |
|
return_if_fail(lis2mdl); |
|
|
|
lis2mdl->i2c_read_func = i2c_read_func; |
|
lis2mdl->i2c_write_func = i2c_write_func; |
|
lis2mdl->i2c_addr = dev_addr; |
|
} |
|
|
|
lis2mdl_status_t lis2mdl_config(lis2mdl_t *lis2mdl) |
|
{ |
|
int32_t ret = 0; |
|
|
|
/* enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00) */ |
|
ret |= lis2mdl_write_byte(LIS2MDL_CFG_REG_A, |
|
0x80 | lis2mdl->init->m_odr << 2); |
|
/* enable low pass filter (bit 0 == 1), set to ODR/4 */ |
|
ret |= lis2mdl_write_byte(LIS2MDL_CFG_REG_B, 0x01); |
|
/* enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1) */ |
|
ret |= lis2mdl_write_byte(LIS2MDL_CFG_REG_C, 0x01 | 0x10); |
|
|
|
return ret ? LIS2MDL_BAD_COMM : LIS2MDL_OK; |
|
} |
|
|
|
/* FIXME: haven't explored the usage/usefulness of these yet: */ |
|
#if(0) |
|
uint8_t lis2mdl_chip_id_get(lis2mdl_t *lis2mdl) |
|
{ |
|
uint8_t c; |
|
lis2mdl_read_byte(LIS2MDL_WHO_AM_I, &c); |
|
|
|
return c; |
|
} |
|
|
|
void lis2mdl_reset(lis2mdl_t *lis2mdl) |
|
{ |
|
// reset device |
|
uint8_t temp; |
|
lis2mdl_read_byte(LIS2MDL_CFG_REG_A, &temp); |
|
|
|
lis2mdl_write_byte(LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL |
|
lis2mdl->delay_func(1); |
|
lis2mdl_write_byte(LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL |
|
lis2mdl->delay_func(100); // Wait for all registers to reset |
|
} |
|
|
|
uint8_t lis2mdl_status(lis2mdl_t *lis2mdl) |
|
{ |
|
// Read the status register of the altimeter |
|
uint8_t temp; |
|
lis2mdl_read_byte(LIS2MDL_STATUS_REG, &temp); |
|
return temp; |
|
} |
|
|
|
void lis2mdl_data_get(lis2mdl_t *lis2mdl, int16_t *destination) |
|
{ |
|
uint8_t data[6]; // x/y/z mag register data stored here |
|
lis2mdl_read((0x80 | LIS2MDL_OUTX_L_REG), data, 8); // Read the 6 raw data registers into data array |
|
|
|
destination[0] = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value |
|
destination[1] = ((int16_t) data[3] << 8) | data[2]; |
|
destination[2] = ((int16_t) data[5] << 8) | data[4]; |
|
} |
|
|
|
int16_t lis2mdl_temp_get(lis2mdl_t *lis2mdl) |
|
{ |
|
uint8_t data[2]; // x/y/z mag register data stored here |
|
lis2mdl_read(0x80 | LIS2MDL_TEMP_OUT_L_REG, data, 2); // Read the 8 raw data registers into data array |
|
|
|
int16_t temp = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value |
|
return temp; |
|
} |
|
|
|
void lis2mdl_offset_bias(lis2mdl_t *lis2mdl, float *dest1, float *dest2) |
|
{ |
|
int32_t mag_bias[3] = { 0, 0, 0 }, mag_scale[3] = { 0, 0, 0 }; |
|
int16_t mag_max[3] = { -32767, -32767, -32767 }, mag_min[3] = |
|
{ 32767, 32767, 32767 }, mag_temp[3] = { 0, 0, 0 }; |
|
float m_res = 0.0015f; |
|
|
|
/* Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!"); */ |
|
lis2mdl->delay_func(4000); |
|
|
|
for(int ii = 0; ii < 4000; ii++) |
|
{ |
|
lis2mdl_data_get(lis2mdl, mag_temp); |
|
for(int jj = 0; jj < 3; jj++) |
|
{ |
|
if(mag_temp[jj] > mag_max[jj]) |
|
{ |
|
mag_max[jj] = mag_temp[jj]; |
|
} |
|
if(mag_temp[jj] < mag_min[jj]) |
|
{ |
|
mag_min[jj] = mag_temp[jj]; |
|
} |
|
} |
|
lis2mdl->delay_func(12); |
|
} |
|
|
|
m_res = 0.0015f; // fixed sensitivity |
|
// Get hard iron correction |
|
mag_bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts |
|
mag_bias[1] = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts |
|
mag_bias[2] = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts |
|
|
|
dest1[0] = (float) mag_bias[0] * m_res; // save mag biases in G for main program |
|
dest1[1] = (float) mag_bias[1] * m_res; |
|
dest1[2] = (float) mag_bias[2] * m_res; |
|
|
|
// Get soft iron correction estimate |
|
mag_scale[0] = (mag_max[0] - mag_min[0]) / 2; // get average x axis max chord length in counts |
|
mag_scale[1] = (mag_max[1] - mag_min[1]) / 2; // get average y axis max chord length in counts |
|
mag_scale[2] = (mag_max[2] - mag_min[2]) / 2; // get average z axis max chord length in counts |
|
|
|
float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2]; |
|
avg_rad /= 3.0f; |
|
|
|
dest2[0] = avg_rad / ((float) mag_scale[0]); |
|
dest2[1] = avg_rad / ((float) mag_scale[1]); |
|
dest2[2] = avg_rad / ((float) mag_scale[2]); |
|
|
|
/* Serial.println("Mag Calibration done!"); */ |
|
} |
|
|
|
void lis2mdl_self_test(lis2mdl_t *lis2mdl) |
|
{ |
|
uint8_t c; |
|
int16_t temp[3] = { 0, 0, 0 }; |
|
float magTest[3] = { 0., 0., 0. }; |
|
float magNom[3] = { 0., 0., 0. }; |
|
int32_t sum[3] = { 0, 0, 0 }; |
|
float m_res = 0.0015f; |
|
|
|
// first, get average response with self test disabled |
|
for(int ii = 0; ii < 50; ii++) |
|
{ |
|
lis2mdl_data_get(lis2mdl, temp); |
|
sum[0] += temp[0]; |
|
sum[1] += temp[1]; |
|
sum[2] += temp[2]; |
|
lis2mdl->delay_func(50); |
|
} |
|
|
|
magNom[0] = (float) sum[0] / 50.0f; |
|
magNom[1] = (float) sum[1] / 50.0f; |
|
magNom[2] = (float) sum[2] / 50.0f; |
|
|
|
lis2mdl_read_byte(LIS2MDL_CFG_REG_C, &c); |
|
lis2mdl_write_byte(LIS2MDL_CFG_REG_C, c | 0x02); // enable self test |
|
lis2mdl->delay_func(100); // let mag respond |
|
|
|
sum[0] = 0; |
|
sum[1] = 0; |
|
sum[2] = 0; |
|
for(int ii = 0; ii < 50; ii++) |
|
{ |
|
lis2mdl_data_get(lis2mdl, temp); |
|
sum[0] += temp[0]; |
|
sum[1] += temp[1]; |
|
sum[2] += temp[2]; |
|
lis2mdl->delay_func(50); |
|
} |
|
|
|
magTest[0] = (float) sum[0] / 50.0f; |
|
magTest[1] = (float) sum[1] / 50.0f; |
|
magTest[2] = (float) sum[2] / 50.0f; |
|
|
|
lis2mdl_write_byte(LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode |
|
lis2mdl->delay_func(100); // let mag respond |
|
|
|
/* Serial.println("Mag Self Test:"); */ |
|
/* Serial.print("Mx results:"); */ |
|
/* Serial.print((magTest[0] - magNom[0]) * m_res * 1000.0); */ |
|
/* Serial.println(" mG"); */ |
|
/* Serial.print("My results:"); */ |
|
/* Serial.println((magTest[0] - magNom[0]) * m_res * 1000.0); */ |
|
/* Serial.print("Mz results:"); */ |
|
/* Serial.println((magTest[1] - magNom[1]) * m_res * 1000.0); */ |
|
/* Serial.println("Should be between 15 and 500 mG"); */ |
|
/* lis2mdl->delay_func(2000); // give some time to read the screen */ |
|
} |
|
#endif
|
|
|