Skip to content

F
fwatch
Commit 797d2335 authored by Grzegorz Daniluk's avatar Grzegorz Daniluk
Browse files
Options

drivers/lsm303c: allow on-the-fly calibration

parent 7ffb3344
Hide whitespace changes
Inline Side-by-side
Showing with 34 additions and 29 deletions
sw/common/drivers/LSM303C/lsm303c.c
View file @ 797d2335
...@@ -4,6 +4,8 @@ ...@@ -4,6 +4,8 @@
#include <efm32gg330f1024.h> #include <efm32gg330f1024.h>
#include "lsm303c.h" #include "lsm303c.h"
static int iron_x, iron_y, iron_z;
extern volatile uint32_t msTicks; extern volatile uint32_t msTicks;
void Delay(uint32_t dlyTicks); void Delay(uint32_t dlyTicks);
...@@ -130,6 +132,11 @@ int lsm303_init() ...@@ -130,6 +132,11 @@ int lsm303_init()
lsm303_fullscale(DEV_MAG, LSM303_16Ga); lsm303_fullscale(DEV_MAG, LSM303_16Ga);
lsm303_selftest(DEV_MAG, 0, 0); lsm303_selftest(DEV_MAG, 0, 0);
/*load default hard iron calibration values*/
iron_x = LSM303_IRON_X;
iron_y = LSM303_IRON_Y;
iron_z = LSM303_IRON_Z;
return LSM303_SUCCESS; return LSM303_SUCCESS;
} }
...@@ -359,11 +366,15 @@ int lsm303_fifo_mode(int dev, LSM303_FMODE_t mode, int en) ...@@ -359,11 +366,15 @@ int lsm303_fifo_mode(int dev, LSM303_FMODE_t mode, int en)
return LSM303_SUCCESS; return LSM303_SUCCESS;
} }
static void lsm303_iron_calib(lsm303_smpl *smpl) static void lsm303_iron_corr(lsm303_smpl *smpl)
{ {
smpl->x -= LSM303_IRON_X; smpl->x -= iron_x;
smpl->y -= LSM303_IRON_Y; smpl->y -= iron_y;
smpl->z -= LSM303_IRON_Z; smpl->z -= iron_z;
//smpl->x = (int)( (float)smpl->x*siron_x );
//smpl->y = (int)( (float)smpl->y*siron_y );
//smpl->z = (int)( (float)smpl->z*siron_z );
} }
int lsm303_get_sample(int dev, lsm303_smpl *smpl) int lsm303_get_sample(int dev, lsm303_smpl *smpl)
...@@ -385,32 +396,23 @@ int lsm303_get_sample(int dev, lsm303_smpl *smpl) ...@@ -385,32 +396,23 @@ int lsm303_get_sample(int dev, lsm303_smpl *smpl)
return LSM303_ERROR; return LSM303_ERROR;
smpl->z = (int16_t) ((val_h << 8) | val_l); smpl->z = (int16_t) ((val_h << 8) | val_l);
lsm303_iron_calib(smpl); lsm303_iron_corr(smpl);
return LSM303_SUCCESS; return LSM303_SUCCESS;
} }
int lsm303_mag_calibrate(lsm303_smpl *max, lsm303_smpl *min) lsm303_smpl lsm303_calib_iron(lsm303_smpl *max, lsm303_smpl *min)
{ {
int i = 1000; lsm303_smpl ret;
lsm303_smpl val;
max->x = -0x7fff; iron_x = (min->x + max->x)/2;
max->y = -0x7fff; iron_y = (min->y + max->y)/2;
max->z = -0x7fff; iron_z = (min->z + max->z)/2;
min->x = 0x7fff;
min->y = 0x7fff; ret.x = iron_x;
min->z = 0x7fff; ret.y = iron_y;
ret.z = iron_z;
while(i--) { return ret;
lsm303_get_sample(DEV_MAG, &val);
if(val.x > max->x) max->x = val.x;
if(val.x < min->x) min->x = val.x;
if(val.y > max->y) max->y = val.y;
if(val.y < min->y) min->y = val.y;
if(val.z > max->z) max->z = val.z;
if(val.z < min->z) min->z = val.z;
Delay(10);
}
} }
#define LCD_W 100 #define LCD_W 100
......
sw/common/drivers/LSM303C/lsm303c.h
View file @ 797d2335
...@@ -20,9 +20,12 @@ ...@@ -20,9 +20,12 @@
#define DEV_ACC 0 #define DEV_ACC 0
#define DEV_MAG 1 #define DEV_MAG 1
#define LSM303_IRON_X 403 #define LSM303_IRON_X 415
#define LSM303_IRON_Y 431 #define LSM303_IRON_Y 417
#define LSM303_IRON_Z 588 #define LSM303_IRON_Z 491
#define LSM303_SIRON_X 0.98
#define LSM303_SIRON_Y 0.99
#define LSM303_SIRON_Z 1.02
/* Accelerometer & Magnetometer registers */ /* Accelerometer & Magnetometer registers */
//#define LSM303_ACC_TEMP_L 0x0B //#define LSM303_ACC_TEMP_L 0x0B
...@@ -172,7 +175,7 @@ int lsm303_serialmode(int dev, LSM303_SMODE_t mode); ...@@ -172,7 +175,7 @@ int lsm303_serialmode(int dev, LSM303_SMODE_t mode);
int lsm303_enableaxis(int dev, int mask); int lsm303_enableaxis(int dev, int mask);
int lsm303_fifo_mode(int dev, LSM303_FMODE_t mode, int en); int lsm303_fifo_mode(int dev, LSM303_FMODE_t mode, int en);
int lsm303_get_sample(int dev, lsm303_smpl *smpl); int lsm303_get_sample(int dev, lsm303_smpl *smpl);
int lsm303_mag_calibrate(lsm303_smpl *max, lsm303_smpl *min); lsm303_smpl lsm303_calib_iron(lsm303_smpl *max, lsm303_smpl *min);
int compass_xy(lsm303_smpl *max, lsm303_smpl *min, int *x, int *y); int compass_xy(lsm303_smpl *max, lsm303_smpl *min, int *x, int *y);
int spi_read(uint8_t dev, uint8_t adr, uint8_t *dat); int spi_read(uint8_t dev, uint8_t adr, uint8_t *dat);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment