Files
lilygo-t-deck/lib/SensorsLib/src/SensorQMI8658.hpp
T

1511 lines
45 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/**
*
* @license MIT License
*
* Copyright (c) 2022 lewis he
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*
* @file SensorQMI8658.hpp
* @author Lewis He (lewishe@outlook.com)
* @date 2022-10-16
*
*/
#pragma once
#include "REG/QMI8658Constants.h"
#include "SensorCommon.tpp"
#ifndef ARDUINO
#include <math.h>
#include <stdio.h>
#endif
typedef struct __IMUdata {
float x;
float y;
float z;
} IMUdata;
class SensorQMI8658 :
public SensorCommon<SensorQMI8658>
{
friend class SensorCommon<SensorQMI8658>;
public:
typedef void (*EventCallBack_t)(void);
enum AccelRange {
ACC_RANGE_2G,
ACC_RANGE_4G,
ACC_RANGE_8G,
ACC_RANGE_16G
};
enum GyroRange {
GYR_RANGE_16DPS,
GYR_RANGE_32DPS,
GYR_RANGE_64DPS,
GYR_RANGE_128DPS,
GYR_RANGE_256DPS,
GYR_RANGE_512DPS,
GYR_RANGE_1024DPS,
};
// In 6DOF mode (accelerometer and gyroscope are both enabled),
// the output data rate is derived from the nature frequency of gyroscope
enum AccelODR {
ACC_ODR_1000Hz = 3,
ACC_ODR_500Hz,
ACC_ODR_250Hz,
ACC_ODR_125Hz,
ACC_ODR_62_5Hz,
ACC_ODR_31_25Hz,
ACC_ODR_LOWPOWER_128Hz = 12,
ACC_ODR_LOWPOWER_21Hz,
ACC_ODR_LOWPOWER_11Hz,
ACC_ODR_LOWPOWER_3Hz
};
enum GyroODR {
GYR_ODR_7174_4Hz,
GYR_ODR_3587_2Hz,
GYR_ODR_1793_6Hz,
GYR_ODR_896_8Hz,
GYR_ODR_448_4Hz,
GYR_ODR_224_2Hz,
GYR_ODR_112_1Hz,
GYR_ODR_56_05Hz,
GYR_ODR_28_025Hz
};
//Low-Pass Filter.
enum LpfMode {
LPF_MODE_0, //2.66% of ODR
LPF_MODE_1, //3.63% of ODR
LPF_MODE_2, //5.39% of ODR
LPF_MODE_3, //13.37% of ODR
};
enum MotionEvent {
MOTION_TAP,
MOTION_ANT_MOTION,
MOTION_NO_MOTION,
MOTION_SIGNIFICANT,
MOTION_PEDOMETER,
};
enum IntPin {
IntPin1,
IntPin2,
};
enum Fifo_Samples {
FIFO_SAMPLES_16,
FIFO_SAMPLES_32,
FIFO_SAMPLES_64,
FIFO_SAMPLES_128,
FIFO_SAMPLES_MAX,
} ;
enum FIFO_Mode {
FIFO_MODE_BYPASS,
FIFO_MODE_FIFO,
FIFO_MODE_STREAM,
FIFO_MODE_MAX,
};
enum SampleMode {
SYNC_MODE, //Synchronous sampling
ASYNC_MODE, //Asynchronous sampling
};
enum CommandTable {
CTRL_CMD_ACK = 0x00,
CTRL_CMD_RST_FIFO = 0x04,
CTRL_CMD_REQ_FIFO = 0x05,
CTRL_CMD_WRITE_WOM_SETTING = 0x08,
CTRL_CMD_ACCEL_HOST_DELTA_OFFSET = 0x09,
CTRL_CMD_GYRO_HOST_DELTA_OFFSET = 0x0A,
CTRL_CMD_CONFIGURE_TAP = 0x0C,
CTRL_CMD_CONFIGURE_PEDOMETER = 0x0D,
CTRL_CMD_CONFIGURE_MOTION = 0x0E,
CTRL_CMD_RESET_PEDOMETER = 0x0F,
CTRL_CMD_COPY_USID = 0x10,
CTRL_CMD_SET_RPU = 0x11,
CTRL_CMD_AHB_CLOCK_GATING = 0x12,
CTRL_CMD_ON_DEMAND_CALIBRATION = 0xA2,
CTRL_CMD_APPLY_GYRO_GAINS = 0xAA,
};
enum StatusReg {
EVENT_SIGNIFICANT_MOTION = 128,
EVENT_NO_MOTION = 64,
EVENT_ANY_MOTION = 32,
EVENT_PEDOMETER_MOTION = 16,
EVENT_WOM_MOTION = 4,
EVENT_TAP_MOTION = 2,
};
#if defined(ARDUINO)
SensorQMI8658(PLATFORM_WIRE_TYPE &w, int sda = DEFAULT_SDA, int scl = DEFAULT_SCL, uint8_t addr = QMI8658_L_SLAVE_ADDRESS)
{
__wire = &w;
__sda = sda;
__scl = scl;
__addr = addr;
}
SensorQMI8658(int cs, int mosi = -1, int miso = -1, int sck = -1, PLATFORM_SPI_TYPE &spi = SPI)
{
__spi = &spi;
__cs = cs;
__miso = miso;
__mosi = mosi;
__sck = sck;
}
#endif
SensorQMI8658()
{
#if defined(ARDUINO)
__wire = &Wire;
__sda = DEFAULT_SDA;
__scl = DEFAULT_SCL;
#endif
__addr = QMI8658_L_SLAVE_ADDRESS;
}
~SensorQMI8658()
{
log_i("~SensorQMI8658");
deinit();
}
#if defined(ARDUINO)
bool init(PLATFORM_WIRE_TYPE &w, int sda = DEFAULT_SDA, int scl = DEFAULT_SCL, uint8_t addr = QMI8658_L_SLAVE_ADDRESS)
{
return SensorCommon::begin(w, addr, sda, scl);
}
#endif
void deinit()
{
// end();
}
bool reset(bool waitResult = true, uint32_t timeout = 500)
{
int val = 0; // initialize with some value to avoid compilation errors
writeRegister(QMI8658_REG_RESET, QMI8658_REG_RESET_DEFAULT);
// Maximum 15ms for the Reset process to be finished
if (waitResult) {
uint32_t start = millis();
while (millis() - start < timeout) {
val = readRegister(QMI8658_REG_RST_RESULT);
if (val != DEV_WIRE_ERR && val == QMI8658_REG_RST_RESULT_VAL) {
//EN.ADDR_AI
setRegisterBit(QMI8658_REG_CTRL1, 6);
return true;
}
delay(10);
}
LOG("Reset chip failed, respone val = %d - 0x%X\n", val, val);
return false;
}
//EN.ADDR_AI
setRegisterBit(QMI8658_REG_CTRL1, 6);
return true;
}
uint8_t getChipID()
{
return readRegister(QMI8658_REG_REVISION);
}
int whoAmI()
{
return readRegister(QMI8658_REG_WHOAMI);
}
uint32_t getTimestamp()
{
uint8_t buffer[3];
uint32_t timestamp;
if (readRegister(QMI8658_REG_TIMESTAMP_L, buffer, 3) != DEV_WIRE_ERR) {
timestamp = (uint32_t)(((uint32_t)buffer[2] << 16) |
((uint32_t)buffer[1] << 8) | buffer[0]);
if (timestamp > lastTimestamp) {
lastTimestamp = timestamp;
} else {
lastTimestamp = (timestamp + 0x1000000 - lastTimestamp);
}
}
return lastTimestamp;
}
float getTemperature_C()
{
uint8_t buffer[2];
if (readRegister(QMI8658_REG_TEMPEARTURE_L, buffer, 2) != DEV_WIRE_ERR) {
return (float)buffer[1] + ((float)buffer[0] / 256.0);
}
return NAN;
}
void enableINT(IntPin pin, bool enable = true)
{
switch (pin) {
case IntPin1:
enable ? setRegisterBit(QMI8658_REG_CTRL1, 3) : clrRegisterBit(QMI8658_REG_CTRL1, 3);
break;
case IntPin2:
enable ? setRegisterBit(QMI8658_REG_CTRL1, 4) : clrRegisterBit(QMI8658_REG_CTRL1, 4);
break;
default:
break;
}
}
uint8_t getIrqStatus()
{
return readRegister(QMI8658_REG_STATUSINT);
}
void enableDataReadyINT(bool enable = true)
{
enable ? clrRegisterBit(QMI8658_REG_CTRL7, 5) :
setRegisterBit(QMI8658_REG_CTRL7, 5);
}
/**
* @brief
* @note
* @param range:
* @param odr:
* @param lpfOdr:
* @param lpf:
* @param selfTest:
* @retval
*/
int configAccelerometer(AccelRange range, AccelODR odr, LpfMode lpfOdr = LPF_MODE_0,
bool lpf = true, bool selfTest = true)
{
bool en = isEnableAccelerometer();
if (en) {
disableAccelerometer();
}
//setAccelRange
if (writeRegister(QMI8658_REG_CTRL2, 0x8F, (range << 4)) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
switch (range) {
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
// 2-bit value:
case ACC_RANGE_2G: accelScales = 2.0 / 32768.0; break;
case ACC_RANGE_4G: accelScales = 4.0 / 32768.0; break;
case ACC_RANGE_8G: accelScales = 8.0 / 32768.0; break;
case ACC_RANGE_16G: accelScales = 16.0 / 32768.0; break;
}
// setAccelOutputDataRate
if (writeRegister(QMI8658_REG_CTRL2, 0xF0, odr) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
// setAccelLowPassFitter
lpf ? setRegisterBit(QMI8658_REG_CTRL5, 0) : clrRegisterBit(QMI8658_REG_CTRL5, 0);
// setAccelLowPassFitterOdr
if (writeRegister(QMI8658_REG_CTRL5, QMI8658_ACCEL_LPF_MASK, (lpfOdr << 1)) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
// setAccelSelfTest
selfTest ? setRegisterBit(QMI8658_REG_CTRL2, 7) : clrRegisterBit(QMI8658_REG_CTRL2, 7);
if (en) {
enableAccelerometer();
}
return DEV_WIRE_NONE;
}
/**
* @brief
* @note
* @param range:
* @param odr:
* @param lpfOdr:
* @param lpf:
* @param selfTest:
* @retval
*/
int configGyroscope(GyroRange range, GyroODR odr, LpfMode lpfOdr = LPF_MODE_0,
bool lpf = true, bool selfTest = true)
{
bool en = isEnableGyroscope();
if (en) {
disableGyroscope();
}
// setGyroRange
if (writeRegister(QMI8658_REG_CTRL3, 0x8F, (range << 4)) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
switch (range) {
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
// 2-bit value:
case GYR_RANGE_16DPS: gyroScales = 16.0 / 32768.0; break;
case GYR_RANGE_32DPS: gyroScales = 32.0 / 32768.0; break;
case GYR_RANGE_64DPS: gyroScales = 64.0 / 32768.0; break;
case GYR_RANGE_128DPS: gyroScales = 128.0 / 32768.0; break;
case GYR_RANGE_256DPS: gyroScales = 256.0 / 32768.0; break;
case GYR_RANGE_512DPS: gyroScales = 512.0 / 32768.0; break;
case GYR_RANGE_1024DPS: gyroScales = 1024.0 / 32768.0; break;
}
// setGyroOutputDataRate
if (writeRegister(QMI8658_REG_CTRL3, 0xF0, odr) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
// setGyroLowPassFitter
lpf ? setRegisterBit(QMI8658_REG_CTRL5, 4) : clrRegisterBit(QMI8658_REG_CTRL5, 4);
// setGyroLowPassFitterOdr
if (writeRegister(QMI8658_REG_CTRL5, QMI8658_GYRO_LPF_MASK, (lpfOdr << 5)) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
// setGyroSelfTest
selfTest ? setRegisterBit(QMI8658_REG_CTRL3, 7) : clrRegisterBit(QMI8658_REG_CTRL3, 7);
if (en) {
enableGyroscope();
}
return DEV_WIRE_NONE;
}
/**
* @brief
* @note
* @param mode:
* @param samples:
* @param pin:
* @param watermark:
* @retval
*/
int configFIFO(FIFO_Mode mode,
Fifo_Samples samples = FIFO_SAMPLES_16, IntPin pin = IntPin2,
uint8_t watermark = 8)
{
bool enGyro = isEnableGyroscope();
bool enAccel = isEnableAccelerometer();
if (enGyro) {
disableGyroscope();
}
if (enAccel) {
disableAccelerometer();
}
/////////////////
pin == IntPin2 ? clrRegisterBit(QMI8658_REG_CTRL1, 2) : setRegisterBit(QMI8658_REG_CTRL1, 2);
// set fifo mode and samples len
fifoMode = (samples << 2) | mode;
if (writeRegister(QMI8658_REG_FIFOCTRL, fifoMode) == DEV_WIRE_ERR) {
return DEV_WIRE_ERR;
}
// set watermark
if (writeRegister(QMI8658_REG_FIFOWMKTH, watermark) == DEV_WIRE_ERR) {
return DEV_WIRE_ERR;
}
//reset fifo
writeCommand(CTRL_CMD_RST_FIFO);
/////////////////
if (enGyro) {
enableGyroscope();
}
if (enAccel) {
enableAccelerometer();
}
return DEV_WIRE_NONE;
}
uint16_t getFifoNeedBytes()
{
uint8_t sam[] = {16, 32, 64, 128};
uint8_t sensors = 0;
if (gyroEn && accelEn) {
sensors = 2;
} else if (gyroEn || accelEn) {
sensors = 1;
}
uint8_t samples = ((fifoMode >> 2) & 0x03) ;
return sam[samples] * 6 * sensors;
}
bool readFromFifo(IMUdata *acc, uint16_t accLenght, IMUdata *gyr, uint16_t gyrLenght)
{
uint16_t bytes = getFifoNeedBytes();
uint8_t *buffer = new uint8_t [bytes];
if (!buffer) {
LOG("No memory!");
return false;
}
if (!readFromFifo(buffer, bytes)) {
delete buffer;
return false;
}
int counter = 0;
for (int i = 0; i < bytes; ) {
if (accelEn) {
if (counter < accLenght) {
acc[counter].x = (float)((int16_t)buffer[i] | (buffer[i + 1] << 8)) * accelScales;
acc[counter].y = (float)((int16_t)buffer[i + 2] | (buffer[i + 3] << 8)) * accelScales;
acc[counter].z = (float)((int16_t)buffer[i + 4] | (buffer[i + 5] << 8)) * accelScales;
}
i += 6;
}
if (gyroEn) {
if (counter < gyrLenght) {
gyr[counter].x = (float)((int16_t)buffer[i] | (buffer[i + 1] << 8)) * gyroScales;
gyr[counter].y = (float)((int16_t)buffer[i + 2] | (buffer[i + 3] << 8)) * gyroScales;
gyr[counter].z = (float)((int16_t)buffer[i + 4] | (buffer[i + 5] << 8)) * gyroScales;
}
i += 6;
}
counter++;
}
delete buffer;
return true;
}
bool readFromFifo(uint8_t *data, size_t lenght)
{
uint8_t status[2];
uint8_t fifo_sensors = 1;
uint16_t fifo_bytes = 0;
uint16_t fifo_level = 0;
// get fifo status
int val = readRegister(QMI8658_REG_FIFOSTATUS);
if (val == DEV_WIRE_ERR) {
return false;
}
LOG("fifo status:0x%x ", val);
if (val & (1 << 5)) {
LOG("\t\tFIFO Overflow condition has happened (data dropping happened)\n");
}
if (val & (1 << 6)) {
LOG("\t\tFIFO Water Mark Level Hit\n");
}
if (val & (1 << 7)) {
LOG("\t\tFIFO is Full\n");
}
val = readRegister(QMI8658_REG_FIFOCOUNT, status, 2);
if (val == DEV_WIRE_ERR) {
return false;
}
fifo_bytes = ((status[1] & 0x03)) << 8 | status[0];
if (accelEn && gyroEn) {
fifo_sensors = 2;
} else if (accelEn || gyroEn) {
fifo_sensors = 1;
}
fifo_level = fifo_bytes / (3 * fifo_sensors);
fifo_bytes = fifo_level * (6 * fifo_sensors);
LOG("fifo-level : %d fifo_bytes : %d fifo_sensors : %d\n", fifo_level, fifo_bytes, fifo_sensors);
if (lenght < fifo_bytes) {
writeCommand(CTRL_CMD_RST_FIFO);
return false;
}
if (fifo_level) {
writeCommand(CTRL_CMD_REQ_FIFO);
if (readRegister(QMI8658_REG_FIFODATA, data, fifo_bytes) ==
DEV_WIRE_ERR) {
LOG("get fifo error !");
return false;
}
val = writeRegister(QMI8658_REG_FIFOCTRL, fifoMode);
if (val == DEV_WIRE_ERR) {
return false;
}
}
writeCommand(CTRL_CMD_RST_FIFO);
return fifo_level;
}
bool enableAccelerometer()
{
accelEn = true;
return setRegisterBit(QMI8658_REG_CTRL7, 0) == DEV_WIRE_NONE;
}
bool disableAccelerometer()
{
accelEn = false;
return clrRegisterBit(QMI8658_REG_CTRL7, 0) == DEV_WIRE_NONE;
}
bool isEnableAccelerometer()
{
accelEn = getRegisterBit(QMI8658_REG_CTRL7, 0);
return accelEn;
}
bool isEnableGyroscope()
{
gyroEn = getRegisterBit(QMI8658_REG_CTRL7, 1);
return gyroEn;
}
bool enableGyroscope()
{
gyroEn = true;
return setRegisterBit(QMI8658_REG_CTRL7, 1) == DEV_WIRE_NONE;
}
bool disableGyroscope()
{
gyroEn = false;
return clrRegisterBit(QMI8658_REG_CTRL7, 1) == DEV_WIRE_NONE;
}
bool getAccelRaw(int16_t *rawBuffer)
{
uint8_t buffer[6] = {0};
if (readRegister(QMI8658_REG_AX_L, buffer, 6) != DEV_WIRE_ERR) {
rawBuffer[0] = (int16_t)(buffer[1] << 8) | (buffer[0]);
rawBuffer[1] = (int16_t)(buffer[3] << 8) | (buffer[2]);
rawBuffer[2] = (int16_t)(buffer[5] << 8) | (buffer[4]);
} else {
return false;
}
return true;
}
bool getAccelerometer(float &x, float &y, float &z)
{
int16_t raw[3];
if (getAccelRaw(raw)) {
x = raw[0] * accelScales;
y = raw[1] * accelScales;
z = raw[2] * accelScales;
return true;
}
return false;
}
float getAccelerometerScales()
{
return accelScales;
}
float getGyroscopeScales()
{
return gyroScales;
}
bool getGyroRaw(int16_t *rawBuffer)
{
uint8_t buffer[6] = {0};
if (readRegister(QMI8658_REG_GX_L, buffer, 6) != DEV_WIRE_ERR) {
rawBuffer[0] = (int16_t)(buffer[1] << 8) | (buffer[0]);
rawBuffer[1] = (int16_t)(buffer[3] << 8) | (buffer[2]);
rawBuffer[2] = (int16_t)(buffer[5] << 8) | (buffer[4]);
} else {
return false;
}
return true;
}
int getGyroscope(float &x, float &y, float &z)
{
int16_t raw[3];
if (getGyroRaw(raw)) {
x = raw[0] * gyroScales;
y = raw[1] * gyroScales;
z = raw[2] * gyroScales;
return true;
}
return false;
}
bool getDataReady()
{
switch (sampleMode) {
case SYNC_MODE:
return getRegisterBit(QMI8658_REG_STATUSINT, 1);
case ASYNC_MODE:
//TODO: When Accel and Gyro are configured with different rates, this will always be false
if (gyroEn && accelEn) {
return readRegister(QMI8658_REG_STATUS0) & 0x03;
} else if (gyroEn) {
return readRegister(QMI8658_REG_STATUS0) & 0x02;
} else if (accelEn) {
return readRegister(QMI8658_REG_STATUS0) & 0x01;
}
break;
default:
break;
}
return false;
}
int enableSyncSampleMode()
{
sampleMode = SYNC_MODE;
return setRegisterBit(QMI8658_REG_CTRL7, 7);
}
int disableSyncSampleMode()
{
sampleMode = ASYNC_MODE;
return clrRegisterBit(QMI8658_REG_CTRL7, 7);
}
int enableLockingMechanism()
{
enableSyncSampleMode();
if (writeRegister(QMI8658_REG_CAL1_L, 0x01) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
return writeCommand(CTRL_CMD_AHB_CLOCK_GATING);
}
int disableLockingMechanism()
{
disableSyncSampleMode();
if (writeRegister(QMI8658_REG_CAL1_L, 0x00) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
return writeCommand(CTRL_CMD_AHB_CLOCK_GATING);
}
void dumpCtrlRegister()
{
uint8_t buffer[9];
readRegister(QMI8658_REG_CTRL1, buffer, 9);
for (int i = 0; i < 9; ++i) {
#if defined(ARDUINO)
Serial.printf("CTRL%d: REG:0x%02X HEX:0x%02X ", i + 1, QMI8658_REG_CTRL1 + i, buffer[i]);
#else
printf("CTRL%d: 0x%02x", i + 1, buffer[i]);
#endif
#if defined(ARDUINO)
Serial.print(" BIN:0b");
Serial.println(buffer[i], BIN);
#else
LOG("\n");
#endif
}
#if defined(ARDUINO)
Serial.println();
#else
printf("\n");
#endif
buffer[0] = readRegister(QMI8658_REG_FIFOCTRL);
#if defined(ARDUINO)
Serial.printf("FIFOCTRL: REG:0x%02X HEX:0x%02X ", QMI8658_REG_FIFOCTRL, buffer[0]);
Serial.print(" BIN:0b");
Serial.println(buffer[0], BIN);
#else
printf("FIFOCTRL: REG:0x%02X HEX:0x%02X \n", QMI8658_REG_FIFOCTRL, buffer[0]);
#endif
}
void powerDown()
{
disableAccelerometer();
disableGyroscope();
setRegisterBit(QMI8658_REG_CTRL1, 1);
}
void powerOn()
{
clrRegisterBit(QMI8658_REG_CTRL1, 1);
}
int getStatusRegister()
{
return readRegister(QMI8658_REG_STATUS1);
}
int configActivityInterruptMap(IntPin pin)
{
return pin == IntPin1 ? setRegisterBit(QMI8658_REG_CTRL8, 6)
: clrRegisterBit(QMI8658_REG_CTRL8, 6);
}
/**
* @brief configPedometer
* @note The calculation of the Pedometer Detection is based on the accelerometer ODR defined by CTRL2.aODR,
* refer to Table 22 for details.
* @param ped_sample_cnt: Indicates the count of sample batch/window for calculation
* @param ped_fix_peak2peak:Indicates the threshold of the valid peak-to-peak detection
* E.g., 0x00CC means 200mg
* @param ped_fix_peak:Indicates the threshold of the peak detection comparing to average
* E.g., 0x0066 means 100mg
* @param ped_time_up:Indicates the maximum duration (timeout window) for a step.
* Reset counting calculation if no peaks detected within this duration.
* E.g., 80 means 1.6s @ ODR = 50Hz
* @param ped_time_low:Indicates the minimum duration for a step.
* The peaks detected within this duration (quiet time) is ignored.
* E.g., 12 means 0.25s @ ODR = 50Hz
* @param ped_time_cnt_entry:Indicates the minimum continuous steps to start the valid step counting.
* If the continuously detected steps is lower than this count and timeout,
* the steps will not be take into account;
* if yes, the detected steps will all be taken into account and
* counting is started to count every following step before timeout.
* This is useful to screen out the fake steps detected by non-step vibrations
* The timeout duration is defined by ped_time_up.
* E.g., 10 means 10 steps entry count
* @param ped_fix_precision:0 is recommended
* @param ped_sig_count: The amount of steps when to update the pedometer output registers.
* E.g., ped_sig_count = 4, every 4 valid steps is detected, update the registers once (added by 4).
* @retval
*/
int configPedometer(uint16_t ped_sample_cnt, uint16_t ped_fix_peak2peak, uint16_t ped_fix_peak,
uint16_t ped_time_up, uint8_t ped_time_low = 0x14, uint8_t ped_time_cnt_entry = 0x0A, uint8_t ped_fix_precision = 0x00,
uint8_t ped_sig_count = 0x04)
{
// The Pedometer can only work in Non-SyncSample mode
disableSyncSampleMode();
bool enGyro = isEnableGyroscope();
bool enAccel = isEnableAccelerometer();
if (enGyro) {
disableGyroscope();
}
if (enAccel) {
disableAccelerometer();
}
writeRegister(QMI8658_REG_CAL1_L, ped_sample_cnt & 0xFF);
writeRegister(QMI8658_REG_CAL1_H, (ped_sample_cnt >> 8) & 0xFF);
writeRegister(QMI8658_REG_CAL2_L, ped_fix_peak2peak & 0xFF);
writeRegister(QMI8658_REG_CAL2_H, (ped_fix_peak2peak >> 8) & 0xFF);
writeRegister(QMI8658_REG_CAL3_L, ped_fix_peak & 0xFF);
writeRegister(QMI8658_REG_CAL3_H, (ped_fix_peak >> 8) & 0xFF);
writeRegister(QMI8658_REG_CAL4_H, 0x01);
writeRegister(QMI8658_REG_CAL4_L, 0x02);
writeCommand(CTRL_CMD_CONFIGURE_PEDOMETER);
writeRegister(QMI8658_REG_CAL1_L, ped_time_up & 0xFF);
writeRegister(QMI8658_REG_CAL1_H, (ped_time_up >> 8) & 0xFF);
writeRegister(QMI8658_REG_CAL2_L, ped_time_low);
writeRegister(QMI8658_REG_CAL2_H, ped_time_cnt_entry);
writeRegister(QMI8658_REG_CAL3_L, ped_fix_precision);
writeRegister(QMI8658_REG_CAL3_H, ped_sig_count);
writeRegister(QMI8658_REG_CAL4_H, 0x02);
writeRegister(QMI8658_REG_CAL4_L, 0x02);
writeCommand(CTRL_CMD_CONFIGURE_PEDOMETER);
if (enGyro) {
enableGyroscope();
}
if (enAccel) {
enableAccelerometer();
}
return 0;
}
uint32_t getPedometerCounter()
{
uint8_t buffer[3];
if (readRegister(QMI8658_REG_PEDO_L, buffer, 3) != DEV_WIRE_ERR) {
return (uint32_t)(((uint32_t)buffer[2] << 16) | ((uint32_t)buffer[1] << 8) | buffer[0]);
}
return 0;
}
int clearPedometerCounter()
{
return writeCommand(CTRL_CMD_RESET_PEDOMETER);
}
// The Pedometer can only work in Non-SyncSample mode
int enablePedometer()
{
return setRegisterBit(QMI8658_REG_CTRL8, 4);
}
int disablePedometer()
{
return clrRegisterBit(QMI8658_REG_CTRL8, 4);
}
enum TagPriority {
PRIORITY0, // (X > Y> Z)
PRIORITY1, // (X > Z > Y)
PRIORITY2, // (Y > X > Z)
PRIORITY3, // (Y > Z > X)
PRIORITY4, // (Z > X > Y)
PRIORITY5, // (Z > Y > X)
};
/**
* @brief configTap
* @note The calculation of the Tap Detection is based on the accelerometer ODR defined by CTRL2.aODR, refer to Table 22 for details.
* @param priority: Priority definition between the x, y, z axes of acceleration. Only
Priority[2:0] bits are used.
The axis that output the first peak of Linear Acceleration in a valid
Tap detection, will be consider as the Tap axis. However, there is
possibility that two or three of the axes shows same Linear
Acceleration at exactly same time when reach (or be higher than)
the PeakMagThr. In this case, the defined priority is used to judge
and select the axis as Tap axis.
* @param peakWindow:Defines the maximum duration (in sample) for a valid peak. In a
valid peak, the linear acceleration should reach or be higher than
the PeakMagThr and should return to quiet (no significant
movement) within UDMThr, at the end of PeakWindow.
E.g., 20 @500Hz ODR
* @param tapWindow:Defines the minimum quiet time before the second Tap happen.
After the first Tap is detected, there should be no significant
movement (defined by UDMThr) during the TapWindow. The valid
second tap should be detected after TapWindow and before
DTapWindow.
E.g., 50 @500Hz ODR
* @param dTapWindow:Defines the maximum time for a valid second Tap for Double Tap,
count start from the first peak of the valid first Tap.
E.g., 250 @500Hz ODR
* @param alpha:Defines the ratio for calculation the average of the acceleration.
The bigger of Alpha, the bigger weight of the latest data.
E.g., 0.0625
* @param gamma:Defines the ratio for calculating the average of the movement
magnitude. The bigger of Gamma, the bigger weight of the latest
data.
E.g., 0.25
* @param peakMagThr:Threshold for peak detection.
E.g, 0.8g2 (0x0320)
* @param UDMThr:Undefined Motion threshold. This defines the threshold of the
Linear Acceleration for quiet status.
E.g., 0.4g2 (0x0190)
* @retval
*/
int configTap(uint8_t priority, uint8_t peakWindow, uint16_t tapWindow, uint16_t dTapWindow,
uint8_t alpha, uint8_t gamma, uint16_t peakMagThr, uint16_t UDMThr)
{
bool enGyro = isEnableGyroscope();
bool enAccel = isEnableAccelerometer();
if (enGyro) {
disableGyroscope();
}
if (enAccel) {
disableAccelerometer();
}
writeRegister(QMI8658_REG_CAL1_L, peakWindow);
writeRegister(QMI8658_REG_CAL1_H, priority);
writeRegister(QMI8658_REG_CAL2_L, tapWindow & 0xFF);
writeRegister(QMI8658_REG_CAL2_H, (tapWindow >> 8) & 0xFF);
writeRegister(QMI8658_REG_CAL3_L, dTapWindow & 0xFF);
writeRegister(QMI8658_REG_CAL3_H, (dTapWindow >> 8) & 0xFF);
// writeRegister(QMI8658_REG_CAL4_L, 0x02);
writeRegister(QMI8658_REG_CAL4_H, 0x01);
writeCommand(CTRL_CMD_CONFIGURE_TAP);
writeRegister(QMI8658_REG_CAL1_L, alpha);
writeRegister(QMI8658_REG_CAL1_H, gamma);
writeRegister(QMI8658_REG_CAL2_L, peakMagThr & 0xFF);
writeRegister(QMI8658_REG_CAL2_H, (peakMagThr >> 8) & 0xFF);
writeRegister(QMI8658_REG_CAL3_L, UDMThr & 0xFF);
writeRegister(QMI8658_REG_CAL3_H, (UDMThr >> 8) & 0xFF);
// writeRegister(QMI8658_REG_CAL4_L, 0x02);
writeRegister(QMI8658_REG_CAL4_H, 0x02);
writeCommand(CTRL_CMD_CONFIGURE_TAP);
if (enGyro) {
enableGyroscope();
}
if (enAccel) {
enableAccelerometer();
}
return 0;
}
int enableTap()
{
return setRegisterBit(QMI8658_REG_CTRL8, 0);
}
int disableTap()
{
return clrRegisterBit(QMI8658_REG_CTRL8, 0);
}
void getTapStatus()
{
int val = readRegister(QMI8658_REG_TAP_STATUS);
if (val & _BV(7)) {
LOG("Tap was detected on the negative direction of the Tap axis\n");
} else {
LOG("Tap was detected on the positive direction of the Tap axis\n");
}
uint8_t t = (val >> 4) & 0x03;
switch (t) {
case 0:
LOG("No Tap was detected\n");
break;
case 1:
LOG("Tap was detected on X axis\n");
break;
case 2:
LOG("Tap was detected on Y axis\n");
break;
case 3:
LOG("Tap was detected on Z axis\n");
break;
default:
break;
}
t = val & 0x03;
switch (t) {
case 0:
LOG("No Tap was detected\n");
break;
case 1:
LOG("Single-Tap was detected\n");
break;
case 2:
LOG("Double-Tap was detected\n");
break;
default:
break;
}
LOG("\n\n\n");
}
/**
* @brief
* @note
* @param AnyMotionXThr:
* @param AnyMotionYThr:
* @param AnyMotionZThr:
* @param NoMotionXThr:
* @param NoMotionYThr:
* @param NoMotionZThr:
* @param modeCtrl:
* @param AnyMotionWindow:
* @param NoMotionWindow:
* @param SigMotionWaitWindow:
* @param SigMotionConfirmWindow:
* @retval
*/
int configMotion(uint8_t AnyMotionXThr, uint8_t AnyMotionYThr, uint8_t AnyMotionZThr,
uint8_t NoMotionXThr, uint8_t NoMotionYThr, uint8_t NoMotionZThr, uint8_t modeCtrl,
uint8_t AnyMotionWindow, uint8_t NoMotionWindow,
uint16_t SigMotionWaitWindow, uint16_t SigMotionConfirmWindow)
{
bool enGyro = isEnableGyroscope();
bool enAccel = isEnableAccelerometer();
if (enGyro) {
disableGyroscope();
}
if (enAccel) {
disableAccelerometer();
}
writeRegister(QMI8658_REG_CAL1_L, AnyMotionXThr);
writeRegister(QMI8658_REG_CAL1_H, AnyMotionYThr);
writeRegister(QMI8658_REG_CAL2_L, AnyMotionZThr);
writeRegister(QMI8658_REG_CAL2_H, NoMotionXThr);
writeRegister(QMI8658_REG_CAL3_L, NoMotionYThr);
writeRegister(QMI8658_REG_CAL3_H, NoMotionZThr);
writeRegister(QMI8658_REG_CAL4_L, modeCtrl);
writeRegister(QMI8658_REG_CAL4_H, 0x01);
writeCommand(CTRL_CMD_CONFIGURE_MOTION);
writeRegister(QMI8658_REG_CAL1_L, AnyMotionWindow);
writeRegister(QMI8658_REG_CAL1_H, NoMotionWindow);
writeRegister(QMI8658_REG_CAL2_L, SigMotionWaitWindow & 0xFF);
writeRegister(QMI8658_REG_CAL2_H, (SigMotionWaitWindow >> 8) & 0xFF);
writeRegister(QMI8658_REG_CAL3_L, SigMotionConfirmWindow & 0xFF);
writeRegister(QMI8658_REG_CAL3_H, (SigMotionConfirmWindow >> 8) & 0xFF);
// writeRegister(QMI8658_REG_CAL4_L, 0x02);
writeRegister(QMI8658_REG_CAL4_H, 0x02);
writeCommand(CTRL_CMD_CONFIGURE_MOTION);
if (enGyro) {
enableGyroscope();
}
if (enAccel) {
enableAccelerometer();
}
return 0;
}
int enableMotionDetect()
{
setRegisterBit(QMI8658_REG_CTRL8, 1);
setRegisterBit(QMI8658_REG_CTRL8, 3);
return setRegisterBit(QMI8658_REG_CTRL8, 2);
}
int disableMotionDetect()
{
return clrRegisterBit(QMI8658_REG_CTRL8, 2);
}
/**
* @brief configWakeOnMotion
* @note Configuring Wom will reset the sensor, set the function to Wom, and there will be no data output
* @param WoMThreshold: Resolution = 1mg ,default 200g
* @param odr: Accelerometer output data rate ,defalut low power 128Hz
* @param pin: Interrupt Pin( 1 or 2 ) ,defalut use pin2
* @param defaultPinValue: WoM Interrupt Initial Value select: ,default pin high
* 01 – INT2 (with initial value 0)
* 11 – INT2 (with initial value 1)
* 00 – INT1 (with initial value 0)
* 10 – INT1 (with initial value 1)
* @param blankingTime: Interrupt Blanking Time
* (in number of accelerometer samples), the
* number of consecutive samples that will be ignored after
* enabling the WoM, to screen out unwanted fake detection
* @retval
*/
int configWakeOnMotion(uint8_t WoMThreshold = 200,
AccelODR odr = ACC_ODR_LOWPOWER_128Hz,
IntPin pin = IntPin2,
uint8_t defaultPinValue = 1,
uint8_t blankingTime = 0x20
)
{
uint8_t val = 0;
//Reset default value
if (!reset()) {
return DEV_WIRE_ERR;
}
// Disable sensors
clrRegisterBit(QMI8658_REG_CTRL7, 0);
//setAccelRange
if (writeRegister(QMI8658_REG_CTRL2, 0x8F, (ACC_RANGE_8G << 4)) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
// setAccelOutputDataRate
if (writeRegister(QMI8658_REG_CTRL2, 0xF0, odr) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
//set wom
if (writeRegister(QMI8658_REG_CAL1_L, WoMThreshold) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
if ( pin == IntPin1) {
val = defaultPinValue ? 0x02 : 0x00;
} else if (pin == IntPin2) {
val = defaultPinValue ? 0x03 : 0x01;
}
val <<= 6;
val |= (blankingTime & 0x3F);
if (writeRegister(QMI8658_REG_CAL1_H, val) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
if (writeCommand(CTRL_CMD_WRITE_WOM_SETTING) != DEV_WIRE_NONE) {
return DEV_WIRE_ERR;
}
enableAccelerometer();
enableINT(pin);
return DEV_WIRE_NONE;
}
void getChipUsid(uint8_t *buffer, uint8_t lenght)
{
if (lenght > 6) {
lenght = 6;
}
memcpy(buffer, usid, lenght);
}
uint32_t getChipFirmwareVersion()
{
return revisionID;
}
enum SensorStatus {
STATUS_INT_CTRL9_CMD_DONE = _BV(0),
STATUS_INT_LOCKED = _BV(1),
STATUS_INT_AVAIL = _BV(2),
STATUS0_GDATA_REDAY = _BV(3),
STATUS0_ADATA_REDAY = _BV(4),
STATUS1_SIGNI_MOTION = _BV(5),
STATUS1_NO_MOTION = _BV(6),
STATUS1_ANY_MOTION = _BV(7),
STATUS1_PEDOME_MOTION = _BV(8),
STATUS1_WOM_MOTION = _BV(9),
STATUS1_TAP_MOTION = _BV(10),
};
/**
* @brief readSensorStatus
* @note Get the interrupt status and status 0, status 1 of the sensor
* @retval Return SensorStatus
*/
uint16_t readSensorStatus()
{
uint16_t result = 0;
// STATUSINT 0x2D
// STATUS0 0x2E
// STATUS1 0x2F
uint8_t status[3];
if (readRegister(QMI8658_REG_STATUSINT, status, 3) != DEV_WIRE_NONE) {
return 0;
}
// LOG("STATUSINT:0x%X BIN:", status[0]);
// LOG_BIN(status[0]);
// LOG("STATUS0:0x%X BIN:", status[1]);
// LOG_BIN(status[1]);
// LOG("STATUS1:0x%X BIN:", status[2]);
// LOG_BIN(status[2]);
// LOG("------------------\n");
// Ctrl9 CmdDone
// Indicates CTRL9 Command was done, as part of CTRL9 protocol
// 0: Not Completed
// 1: Done
if (status[0] & 0x80) {
result |= STATUS_INT_CTRL9_CMD_DONE;
}
// If syncSmpl (CTRL7.bit7) = 1:
// 0: Sensor Data is not locked.
// 1: Sensor Data is locked.
// If syncSmpl = 0, this bit shows the same value of INT1 level
if (status[0] & 0x02) {
result |= STATUS_INT_LOCKED;
}
// If syncSmpl (CTRL7.bit7) = 1:
// 0: Sensor Data is not available
// 1: Sensor Data is available for reading
// If syncSmpl = 0, this bit shows the same value of INT2 level
if (status[0] & 0x01) {
result |= STATUS_INT_AVAIL;
// if (eventGyroDataReady)eventGyroDataReady();
// if (eventAccelDataReady)eventAccelDataReady();
}
//Locking Mechanism Can reading..
if ((status[0] & 0x03) == 0x03) {
if (eventDataLocking)eventDataLocking();
}
//=======================================
// Valid only in asynchronous mode
if (sampleMode == ASYNC_MODE) {
// Gyroscope new data available
// 0: No updates since last read.
// 1: New data available
if (status[1] & 0x02) {
result |= STATUS0_GDATA_REDAY;
if (eventGyroDataReady)eventGyroDataReady();
__gDataReady = true;
}
// Accelerometer new data available
// 0: No updates since last read.
// 1: New data available.
if (status[1] & 0x01) {
result |= STATUS0_ADATA_REDAY;
if (eventAccelDataReady)eventAccelDataReady();
__aDataReady = true;
}
}
//=======================================
// Significant Motion
// 0: No Significant-Motion was detected
// 1: Significant-Motion was detected
if (status[2] & 0x80) {
result |= STATUS1_SIGNI_MOTION;
if (eventSignificantMotion)eventSignificantMotion();
}
// No Motion
// 0: No No-Motion was detected
// 1: No-Motion was detected
if (status[2] & 0x40) {
result |= STATUS1_NO_MOTION;
if (eventNoMotionEvent)eventNoMotionEvent();
}
// Any Motion
// 0: No Any-Motion was detected
// 1: Any-Motion was detected
if (status[2] & 0x20) {
result |= STATUS1_ANY_MOTION;
if (eventAnyMotionEvent)eventAnyMotionEvent();
}
// Pedometer
// 0: No step was detected
// 1: step was detected
if (status[2] & 0x10) {
result |= STATUS1_PEDOME_MOTION;
if (eventPedometerEvent)eventPedometerEvent();
}
// WoM
// 0: No WoM was detected
// 1: WoM was detected
if (status[2] & 0x04) {
result |= STATUS1_WOM_MOTION;
if (eventWomEvent)eventWomEvent();
}
// TAP
// 0: No Tap was detected
// 1: Tap was detected
if (status[2] & 0x02) {
result |= STATUS1_TAP_MOTION;
if (eventTagEvent)eventTagEvent();
}
return result;
}
void setWakeupMotionEventCallBack(EventCallBack_t cb)
{
eventWomEvent = cb;
}
void setTapEventCallBack(EventCallBack_t cb)
{
eventTagEvent = cb;
}
void setPedometerEventCallBack(EventCallBack_t cb)
{
eventPedometerEvent = cb;
}
void setNoMotionEventCallBack(EventCallBack_t cb)
{
eventNoMotionEvent = cb;
}
void setAnyMotionEventCallBack(EventCallBack_t cb)
{
eventAnyMotionEvent = cb;
}
void setSignificantMotionEventCallBack(EventCallBack_t cb)
{
eventSignificantMotion = cb;
}
void setGyroDataReadyCallBack(EventCallBack_t cb)
{
eventGyroDataReady = cb;
}
void setAccelDataReadyEventCallBack(EventCallBack_t cb)
{
eventAccelDataReady = cb;
}
void setDataLockingEvevntCallBack(EventCallBack_t cb)
{
eventDataLocking = cb;
}
private:
float accelScales, gyroScales;
uint32_t lastTimestamp = 0;
uint8_t sampleMode = ASYNC_MODE;
bool accelEn, gyroEn;
uint8_t fifoMode;
uint32_t revisionID;
uint8_t usid[6];
bool __gDataReady = false;
bool __aDataReady = false;
EventCallBack_t eventWomEvent = NULL;
EventCallBack_t eventTagEvent = NULL;
EventCallBack_t eventPedometerEvent = NULL;
EventCallBack_t eventNoMotionEvent = NULL;
EventCallBack_t eventAnyMotionEvent = NULL;
EventCallBack_t eventSignificantMotion = NULL;
EventCallBack_t eventGyroDataReady = NULL;
EventCallBack_t eventAccelDataReady = NULL;
EventCallBack_t eventDataLocking = NULL;
int writeCommand(CommandTable cmd)
{
int val;
uint32_t startMillis;
if (writeRegister(QMI8658_REG_CTRL9, cmd) == DEV_WIRE_ERR) {
return DEV_WIRE_ERR;
}
startMillis = millis();
do {
val = readRegister(QMI8658_REG_STATUSINT);
delay(1);
if (millis() - startMillis > 1000) {
LOG("wait for ctrl9 command done time out : %d val:%d \n", cmd, val);
return DEV_WIRE_TIMEOUT;
}
} while (val != DEV_WIRE_ERR && !(val & 0x80));
if (writeRegister(QMI8658_REG_CTRL9, CTRL_CMD_ACK) == DEV_WIRE_ERR) {
return DEV_WIRE_ERR;
}
startMillis = millis();
do {
val = readRegister(QMI8658_REG_STATUSINT);
delay(1);
if (millis() - startMillis > 1000) {
LOG("Clear ctrl9 command done flag timeout : %d val:%d \n", cmd, val);
return DEV_WIRE_TIMEOUT;
}
} while (val != DEV_WIRE_ERR && (val & 0x80));
return DEV_WIRE_NONE;
}
protected:
bool initImpl()
{
uint8_t buffer[6] = {0};
if (!reset()) {
return false;
}
uint8_t id = whoAmI();
if (id != QMI8658_REG_WHOAMI_DEFAULT) {
LOG("ERROR! ID NOT MATCH QMI8658 , Respone id is 0x%x\n", id);
return false;
}
// Eanble address auto increment, Big-Endian format
// writeRegister(QMI8658_REG_CTRL1, 0x60);
// Little-Endian / address auto increment
// writeRegister(QMI8658_REG_CTRL1, 0x40);
// no need . reset function has set
//EN.ADDR_AI
// setRegisterBit(QMI8658_REG_CTRL1, 6);
// Use STATUSINT.bit7 as CTRL9 handshake
writeRegister(QMI8658_REG_CTRL8, 0x80);
// Get firmware version and usid
writeCommand(CTRL_CMD_COPY_USID);
if (readRegister(QMI8658_REG_DQW_L, buffer, 3) != DEV_WIRE_ERR) {
revisionID = buffer[0] | (uint32_t)(buffer[1] << 8) |
(uint32_t)(buffer[2] << 16);
LOG("FW Version :0x%02X%02X%02X\n", buffer[0], buffer[1], buffer[2]);
}
if (readRegister(QMI8658_REG_DVX_L, usid, 6) != DEV_WIRE_ERR) {
LOG("USID :%02X%02X%02X%02X%02X%02X\n",
usid[0], usid[1], usid[2],
usid[3], usid[4], usid[5]);
}
return true;
}
int getReadMaskImpl()
{
return 0x80;
}
};