WS_QMI8658.cpp
C++ · 58 lines
//WS_QMI8658.cpp
#include "WS_QMI8658.h"
#define I2C_SDA 11
#define I2C_SCL 12
SensorQMI8658 QMI;
IMUdata Accel;
IMUdata Gyro;
void QMI8658_Init()
{
Wire.begin(I2C_SDA, I2C_SCL);
//Using WIRE !!
if (!QMI.begin(Wire, QMI8658_L_SLAVE_ADDRESS, I2C_SDA, I2C_SCL)) {
printf("Failed to find QMI8658 - check your wiring!\r\n");
while (1) {
delay(1000);
}
}
printf("Device ID: %x\r\n",QMI.getChipID()); // Get chip id
QMI.configAccelerometer(
SensorQMI8658::ACC_RANGE_4G, // ACC_RANGE_2G / ACC_RANGE_4G / ACC_RANGE_8G / ACC_RANGE_16G
SensorQMI8658::ACC_ODR_1000Hz, // ACC_ODR_1000H / ACC_ODR_500Hz / ACC_ODR_250Hz / ACC_ODR_125Hz / ACC_ODR_62_5Hz / ACC_ODR_31_25Hz / ACC_ODR_LOWPOWER_128Hz / ACC_ODR_LOWPOWER_21Hz / ACC_ODR_LOWPOWER_11Hz / ACC_ODR_LOWPOWER_3Hz
SensorQMI8658::LPF_MODE_0 //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)
);
QMI.configGyroscope(
SensorQMI8658::GYR_RANGE_64DPS, // GYR_RANGE_16DPS / GYR_RANGE_32DPS / GYR_RANGE_64DPS / GYR_RANGE_128DPS / GYR_RANGE_256DPS / GYR_RANGE_512DPS / GYR_RANGE_1024DPS
SensorQMI8658::GYR_ODR_896_8Hz, // 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_025H
SensorQMI8658::LPF_MODE_3 // 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)
);
// In 6DOF mode (accelerometer and gyroscope are both enabled),
// the output data rate is derived from the nature frequency of gyroscope
QMI.enableGyroscope();
QMI.enableAccelerometer();
QMI.dumpCtrlRegister(); // printf register configuration information
printf("Read data now...\r\n");
}
void QMI8658_Loop()
{
if (QMI.getDataReady()) {
if (QMI.getAccelerometer(Accel.x, Accel.y, Accel.z)) {
}
if (QMI.getGyroscope(Gyro.x, Gyro.y, Gyro.z)) {
}
}
}