From 4c6c402e8efe13ad6ea33a732332ded27603fa3c Mon Sep 17 00:00:00 2001 From: lewisxhe Date: Wed, 25 Sep 2024 17:50:21 +0800 Subject: [PATCH] Added QMI8658 offset method --- src/SensorQMI8658.hpp | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/src/SensorQMI8658.hpp b/src/SensorQMI8658.hpp index 0ed0301..6ebe181 100644 --- a/src/SensorQMI8658.hpp +++ b/src/SensorQMI8658.hpp @@ -600,6 +600,44 @@ class SensorQMI8658 : return samples_per_sensor; } + // This offset change is lost when the sensor is power cycled, or the system is reset + // Each delta offset value should contain 16 bits and the format is signed 11.5 (5 fraction bits, unit is 1 / 2^5). + void setAccelOffset(int16_t offset_x, int16_t offset_y, int16_t offset_z) + { + writeCommand(CTRL_CMD_ACCEL_HOST_DELTA_OFFSET); + + uint8_t data[6]; + data[0] = lowByte(offset_x); + data[1] = highByte(offset_x); + data[2] = lowByte(offset_y); + data[3] = highByte(offset_y); + data[4] = lowByte(offset_z); + data[5] = highByte(offset_z); + + writeRegister(QMI8658_REG_CAL1_L, data, 2); + writeRegister(QMI8658_REG_CAL2_L, data + 2, 2); + writeRegister(QMI8658_REG_CAL3_L, data + 4, 2); + } + + // This offset change is lost when the sensor is power cycled, or the system is reset + // Each delta offset value should contain 16 bits and the format is signed 11.5 (5 fraction bits, unit is 1 / 2^5). + void setGyroOffset(int16_t offset_x, int16_t offset_y, int16_t offset_z) + { + writeCommand(CTRL_CMD_GYRO_HOST_DELTA_OFFSET); + + uint8_t data[6]; + data[0] = lowByte(offset_x); + data[1] = highByte(offset_x); + data[2] = lowByte(offset_y); + data[3] = highByte(offset_y); + data[4] = lowByte(offset_z); + data[5] = highByte(offset_z); + + writeRegister(QMI8658_REG_CAL1_L, data, 2); + writeRegister(QMI8658_REG_CAL2_L, data + 2, 2); + writeRegister(QMI8658_REG_CAL3_L, data + 4, 2); + } + private: