diff options
-rw-r--r-- | driver/accelgyro_bmi160.c | 5 | ||||
-rw-r--r-- | driver/mag_bmm150.c | 26 | ||||
-rw-r--r-- | driver/mag_bmm150.h | 10 | ||||
-rw-r--r-- | include/ec_commands.h | 1 |
4 files changed, 42 insertions, 0 deletions
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index 29152fe1f0..b3d32ec880 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -487,6 +487,8 @@ static int get_offset(const struct motion_sensor_t *s, BMI160_OFFSET_GYRO_DIV_MDS; } break; + case MOTIONSENSE_TYPE_MAG: + return bmm150_get_offset(s, offset, temp); default: for (i = X; i <= Z; i++) offset[i] = 0; @@ -539,6 +541,9 @@ static int set_offset(const struct motion_sensor_t *s, ret = raw_write8(s->addr, BMI160_OFFSET_EN_GYR98, val98 | BMI160_OFFSET_GYRO_EN); break; + case MOTIONSENSE_TYPE_MAG: + ret = bmm150_set_offset(s, offset, temp); + break; default: ret = EC_RES_INVALID_PARAM; } diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c index 4bc0d7a8e7..fb2d5c7108 100644 --- a/driver/mag_bmm150.c +++ b/driver/mag_bmm150.c @@ -199,6 +199,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); /* X and Y are two's complement 13 bits vectors */ raw[X] = ((int16_t)(data[0] | (data[1] << 8))) >> 3; @@ -211,5 +212,30 @@ 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]; +} + +int bmm150_set_offset(const struct motion_sensor_t *s, + const int16_t *offset, + int16_t temp) +{ + struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); + regs->offset[X] = offset[X]; + regs->offset[Y] = offset[Y]; + regs->offset[Z] = offset[Z]; + return EC_SUCCESS; } +int bmm150_get_offset(const struct motion_sensor_t *s, + int16_t *offset, + int16_t *temp) +{ + struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); + offset[X] = regs->offset[X]; + offset[Y] = regs->offset[Y]; + offset[Z] = regs->offset[Z]; + *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; + return EC_SUCCESS; +} diff --git a/driver/mag_bmm150.h b/driver/mag_bmm150.h index 6ae8e679b9..7486a77608 100644 --- a/driver/mag_bmm150.h +++ b/driver/mag_bmm150.h @@ -74,6 +74,9 @@ struct bmm150_comp_registers { int8_t dig_xy2; uint16_t dig_xyz1; + + /* Factory or online calibration */ + int16_t offset[3]; }; #define BMM150_COMP_REG(_s) \ @@ -87,5 +90,12 @@ void bmm150_normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data); +int bmm150_set_offset(const struct motion_sensor_t *s, + const int16_t *offset, + int16_t temp); + +int bmm150_get_offset(const struct motion_sensor_t *s, + int16_t *offset, + int16_t *temp); #endif /* __CROS_EC_MAG_BMM150_H */ diff --git a/include/ec_commands.h b/include/ec_commands.h index fa820b7ebb..13ce200683 100644 --- a/include/ec_commands.h +++ b/include/ec_commands.h @@ -1853,6 +1853,7 @@ struct ec_params_motion_sense { * Unit: * Accelerometer: 1/1024 g * Gyro: 1/1024 deg/s + * Compass: 1/16 uT */ int16_t offset[3]; } __packed sensor_offset; |