diff options
author | Gwendal Grignou <gwendal@chromium.org> | 2015-09-11 12:02:26 -0700 |
---|---|---|
committer | chrome-bot <chrome-bot@chromium.org> | 2015-10-13 05:28:54 -0700 |
commit | 828b55a7358ad5ec8bc27552bfb280eb173dd453 (patch) | |
tree | b9430d2bf624ee2b6976eed211696b50ae56decb /driver/mag_bmm150.c | |
parent | 0647f66f81de880af603a7fb70f57159519782ac (diff) | |
download | chrome-ec-828b55a7358ad5ec8bc27552bfb280eb173dd453.tar.gz |
common: Add magnetometer online calibration.
Code for hard iron calibration: Every seconds (or faster if enough
samples), find a sphere that fit the compass data.
Based on Android code.
BRANCH=smaug
BUG=chrome-os-partner:39900
TEST=Check hard-iron bias is removed. Works better outside.
Change-Id: Iab479d5113b6560b4f01b0fd87373d2eecdb9b54
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/299583
Reviewed-by: Anton Staaf <robotboy@chromium.org>
Diffstat (limited to 'driver/mag_bmm150.c')
-rw-r--r-- | driver/mag_bmm150.c | 29 |
1 files changed, 17 insertions, 12 deletions
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c index 83f650213b..0f8bc8b102 100644 --- a/driver/mag_bmm150.c +++ b/driver/mag_bmm150.c @@ -79,6 +79,7 @@ int bmm150_init(const struct motion_sensor_t *s) int ret; int val; struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); + struct mag_cal_t *moc = BMM150_CAL(s); /* Set the compass from Suspend to Sleep */ ret = raw_mag_write8(s->addr, BMM150_PWR_CTRL, BMM150_PWR_ON); @@ -129,6 +130,8 @@ int bmm150_init(const struct motion_sensor_t *s) ret = raw_mag_write8(s->addr, BMM150_OP_CTRL, BMM150_OP_MODE_FORCED << BMM150_OP_MODE_OFFSET); + init_mag_cal(moc); + moc->radius = 0.0f; return ret; } @@ -207,7 +210,7 @@ void bmm150_normalize(const struct motion_sensor_t *s, { uint16_t r; vector_3_t raw; - struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); + struct mag_cal_t *cal = BMM150_CAL(s); /* X and Y are two's complement 13 bits vectors */ raw[X] = ((int16_t)(data[0] | (data[1] << 8))) >> 3; @@ -220,27 +223,29 @@ void bmm150_normalize(const struct motion_sensor_t *s, bmm150_temp_compensate_xy(s, raw, v, r); bmm150_temp_compensate_z(s, raw, v, r); - v[X] += regs->offset[X]; - v[Y] += regs->offset[Y]; - v[Z] += regs->offset[Z]; + mag_cal_update(cal, v); + + v[X] += cal->bias[X]; + v[Y] += cal->bias[Y]; + v[Z] += cal->bias[Z]; } int bmm150_set_offset(const struct motion_sensor_t *s, const vector_3_t offset) { - struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); - regs->offset[X] = offset[X]; - regs->offset[Y] = offset[Y]; - regs->offset[Z] = offset[Z]; + struct mag_cal_t *cal = BMM150_CAL(s); + cal->bias[X] = offset[X]; + cal->bias[Y] = offset[Y]; + cal->bias[Z] = offset[Z]; return EC_SUCCESS; } int bmm150_get_offset(const struct motion_sensor_t *s, vector_3_t offset) { - struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); - offset[X] = regs->offset[X]; - offset[Y] = regs->offset[Y]; - offset[Z] = regs->offset[Z]; + struct mag_cal_t *cal = BMM150_CAL(s); + offset[X] = cal->bias[X]; + offset[Y] = cal->bias[Y]; + offset[Z] = cal->bias[Z]; return EC_SUCCESS; } |