diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f77f74088..78cc8ef83e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -232,6 +232,7 @@ src/addons/input_macro.cpp src/addons/snes_input.cpp src/addons/tilt.cpp src/addons/spi_analog_ads1256.cpp +src/addons/i2c_qmi8658.cpp ${PROTO_OUTPUT_DIR}/enums.pb.c ${PROTO_OUTPUT_DIR}/config.pb.c ) @@ -262,6 +263,7 @@ WiiExtension SNESpad pico_mbedtls nanopb +QMI8658 ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/headers/addons/i2c_qmi8658.h b/headers/addons/i2c_qmi8658.h new file mode 100644 index 0000000000..752fb34f85 --- /dev/null +++ b/headers/addons/i2c_qmi8658.h @@ -0,0 +1,35 @@ +#ifndef _I2C_QMI8658_H +#define _I2C_QMI8658_H + +extern "C" { +#include "QMI8658.h" +} + +#include "gpaddon.h" + +#include "GamepadEnums.h" +#include "peripheralmanager.h" + +#ifndef I2C_QMI8658_ENABLED +#define I2C_QMI8658_ENABLED 0 +#endif + +// Analog Module Name +#define I2C_QMI8658_Name "I2C_QMI8658" + +class I2CQMI8658Input : public GPAddon { +public: + virtual bool available(); + virtual void setup(); + virtual void preprocess() {} + virtual void process(); + virtual std::string name() { return I2C_QMI8658_Name; } +private: + PeripheralI2C *i2c; + uint32_t uIntervalMS; + uint32_t nextTimer; + int16_t acc[3]; + int16_t gyro[3]; +}; + +#endif // _I2C_QMI8658_H_ diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index 9a2ab33a0b..84c9f8e9d4 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -13,3 +13,4 @@ add_subdirectory(PlayerLEDs) add_subdirectory(rndis) add_subdirectory(WiiExtension) add_subdirectory(SNESpad) +add_subdirectory(QMI8658) diff --git a/lib/QMI8658/CMakeLists.txt b/lib/QMI8658/CMakeLists.txt new file mode 100644 index 0000000000..ff46c763e9 --- /dev/null +++ b/lib/QMI8658/CMakeLists.txt @@ -0,0 +1,4 @@ +add_library(QMI8658 QMI8658.c) +target_link_libraries(QMI8658 PUBLIC PicoPeripherals) +target_include_directories(QMI8658 INTERFACE .) +target_include_directories(QMI8658 PUBLIC . PicoPeripherals) diff --git a/lib/QMI8658/QMI8658.c b/lib/QMI8658/QMI8658.c new file mode 100644 index 0000000000..8fe439df2f --- /dev/null +++ b/lib/QMI8658/QMI8658.c @@ -0,0 +1,628 @@ +/***************************************************************************** +* | File : QMI8658.c +* | Author : +* | Function : Hardware Interface of QMI8658 Sensor +* | Info : +*---------------- +* | This version: V1.0 +* | Date : 2022-06-07 +* | Info : +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documnetation 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 theex Software, and to permit persons to whom the Software is +# furished 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 OR 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. +******************************************************************************/ +//#include "stdafx.h" +#include "QMI8658.h" + +#define QMI8658_printf printf + +#define QMI8658_UINT_MG_DPS + +enum +{ + AXIS_X = 0, + AXIS_Y = 1, + AXIS_Z = 2, + + AXIS_TOTAL +}; + +typedef struct +{ + short sign[AXIS_TOTAL]; + unsigned short map[AXIS_TOTAL]; +} qst_imu_layout; + +static unsigned short acc_lsb_div = 0; +static unsigned short gyro_lsb_div = 0; +static unsigned short ae_q_lsb_div = (1 << 14); +static unsigned short ae_v_lsb_div = (1 << 10); +static unsigned int imu_timestamp = 0; +static struct QMI8658Config QMI8658_config; +static unsigned char QMI8658_slave_addr = QMI8658_SLAVE_ADDR_L; + +int16_t (*QMI8658_I2C_Write)(uint8_t address, uint8_t *data, uint16_t len, bool isBlock); +int16_t (*QMI8658_I2C_ReadRegister)(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len); + +unsigned char QMI8658_write_reg(unsigned char reg, unsigned char value) +{ + unsigned char ret = 0; + unsigned int retry = 0; + + uint8_t data[2] = {reg, value}; + while ((!ret) && (retry++ < 5)) + { + ret = QMI8658_I2C_Write(QMI8658_slave_addr, data, 2, false); + } + return ret; +} + +unsigned char QMI8658_write_regs(unsigned char reg, unsigned char *value, unsigned char len) +{ + int i, ret; + + for (i = 0; i < len; i++) + { + ret = QMI8658_write_reg(reg + i, value[i]); + } + + return ret; +} + +unsigned char QMI8658_read_reg(unsigned char reg, unsigned char *buf, unsigned short len) +{ + return QMI8658_I2C_ReadRegister(QMI8658_slave_addr, reg, buf, len); +} + +#if 0 +static qst_imu_layout imu_map; + +void QMI8658_set_layout(short layout) +{ + if(layout == 0) + { + imu_map.sign[AXIS_X] = 1; + imu_map.sign[AXIS_Y] = 1; + imu_map.sign[AXIS_Z] = 1; + imu_map.map[AXIS_X] = AXIS_X; + imu_map.map[AXIS_Y] = AXIS_Y; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else if(layout == 1) + { + imu_map.sign[AXIS_X] = -1; + imu_map.sign[AXIS_Y] = 1; + imu_map.sign[AXIS_Z] = 1; + imu_map.map[AXIS_X] = AXIS_Y; + imu_map.map[AXIS_Y] = AXIS_X; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else if(layout == 2) + { + imu_map.sign[AXIS_X] = -1; + imu_map.sign[AXIS_Y] = -1; + imu_map.sign[AXIS_Z] = 1; + imu_map.map[AXIS_X] = AXIS_X; + imu_map.map[AXIS_Y] = AXIS_Y; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else if(layout == 3) + { + imu_map.sign[AXIS_X] = 1; + imu_map.sign[AXIS_Y] = -1; + imu_map.sign[AXIS_Z] = 1; + imu_map.map[AXIS_X] = AXIS_Y; + imu_map.map[AXIS_Y] = AXIS_X; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else if(layout == 4) + { + imu_map.sign[AXIS_X] = -1; + imu_map.sign[AXIS_Y] = 1; + imu_map.sign[AXIS_Z] = -1; + imu_map.map[AXIS_X] = AXIS_X; + imu_map.map[AXIS_Y] = AXIS_Y; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else if(layout == 5) + { + imu_map.sign[AXIS_X] = 1; + imu_map.sign[AXIS_Y] = 1; + imu_map.sign[AXIS_Z] = -1; + imu_map.map[AXIS_X] = AXIS_Y; + imu_map.map[AXIS_Y] = AXIS_X; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else if(layout == 6) + { + imu_map.sign[AXIS_X] = 1; + imu_map.sign[AXIS_Y] = -1; + imu_map.sign[AXIS_Z] = -1; + imu_map.map[AXIS_X] = AXIS_X; + imu_map.map[AXIS_Y] = AXIS_Y; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else if(layout == 7) + { + imu_map.sign[AXIS_X] = -1; + imu_map.sign[AXIS_Y] = -1; + imu_map.sign[AXIS_Z] = -1; + imu_map.map[AXIS_X] = AXIS_Y; + imu_map.map[AXIS_Y] = AXIS_X; + imu_map.map[AXIS_Z] = AXIS_Z; + } + else + { + imu_map.sign[AXIS_X] = 1; + imu_map.sign[AXIS_Y] = 1; + imu_map.sign[AXIS_Z] = 1; + imu_map.map[AXIS_X] = AXIS_X; + imu_map.map[AXIS_Y] = AXIS_Y; + imu_map.map[AXIS_Z] = AXIS_Z; + } +} +#endif + +void QMI8658_config_acc(enum QMI8658_AccRange range, enum QMI8658_AccOdr odr, enum QMI8658_LpfConfig lpfEnable, enum QMI8658_StConfig stEnable) +{ + unsigned char ctl_dada; + + switch (range) + { + case QMI8658AccRange_2g: + acc_lsb_div = (1 << 14); + break; + case QMI8658AccRange_4g: + acc_lsb_div = (1 << 13); + break; + case QMI8658AccRange_8g: + acc_lsb_div = (1 << 12); + break; + case QMI8658AccRange_16g: + acc_lsb_div = (1 << 11); + break; + default: + range = QMI8658AccRange_8g; + acc_lsb_div = (1 << 12); + } + if (stEnable == QMI8658St_Enable) + ctl_dada = (unsigned char)range | (unsigned char)odr | 0x80; + else + ctl_dada = (unsigned char)range | (unsigned char)odr; + + QMI8658_write_reg(QMI8658Register_Ctrl2, ctl_dada); + // set LPF & HPF + QMI8658_read_reg(QMI8658Register_Ctrl5, &ctl_dada, 1); + ctl_dada &= 0xf0; + if (lpfEnable == QMI8658Lpf_Enable) + { + ctl_dada |= A_LSP_MODE_3; + ctl_dada |= 0x01; + } + else + { + ctl_dada &= ~0x01; + } + ctl_dada = 0x00; + QMI8658_write_reg(QMI8658Register_Ctrl5, ctl_dada); + // set LPF & HPF +} + +void QMI8658_config_gyro(enum QMI8658_GyrRange range, enum QMI8658_GyrOdr odr, enum QMI8658_LpfConfig lpfEnable, enum QMI8658_StConfig stEnable) +{ + // Set the CTRL3 register to configure dynamic range and ODR + unsigned char ctl_dada; + + // Store the scale factor for use when processing raw data + switch (range) + { + case QMI8658GyrRange_32dps: + gyro_lsb_div = 1024; + break; + case QMI8658GyrRange_64dps: + gyro_lsb_div = 512; + break; + case QMI8658GyrRange_128dps: + gyro_lsb_div = 256; + break; + case QMI8658GyrRange_256dps: + gyro_lsb_div = 128; + break; + case QMI8658GyrRange_512dps: + gyro_lsb_div = 64; + break; + case QMI8658GyrRange_1024dps: + gyro_lsb_div = 32; + break; + case QMI8658GyrRange_2048dps: + gyro_lsb_div = 16; + break; + case QMI8658GyrRange_4096dps: + gyro_lsb_div = 8; + break; + default: + range = QMI8658GyrRange_512dps; + gyro_lsb_div = 64; + break; + } + + if (stEnable == QMI8658St_Enable) + ctl_dada = (unsigned char)range | (unsigned char)odr | 0x80; + else + ctl_dada = (unsigned char)range | (unsigned char)odr; + QMI8658_write_reg(QMI8658Register_Ctrl3, ctl_dada); + + // Conversion from degrees/s to rad/s if necessary + // set LPF & HPF + QMI8658_read_reg(QMI8658Register_Ctrl5, &ctl_dada, 1); + ctl_dada &= 0x0f; + if (lpfEnable == QMI8658Lpf_Enable) + { + ctl_dada |= G_LSP_MODE_3; + ctl_dada |= 0x10; + } + else + { + ctl_dada &= ~0x10; + } + ctl_dada = 0x00; + QMI8658_write_reg(QMI8658Register_Ctrl5, ctl_dada); + // set LPF & HPF +} + +void QMI8658_config_mag(enum QMI8658_MagDev device, enum QMI8658_MagOdr odr) +{ + QMI8658_write_reg(QMI8658Register_Ctrl4, device | odr); +} + +void QMI8658_config_ae(enum QMI8658_AeOdr odr) +{ + // QMI8658_config_acc(QMI8658AccRange_8g, AccOdr_1000Hz, Lpf_Enable, St_Enable); + // QMI8658_config_gyro(QMI8658GyrRange_2048dps, GyrOdr_1000Hz, Lpf_Enable, St_Enable); + QMI8658_config_acc(QMI8658_config.accRange, QMI8658_config.accOdr, QMI8658Lpf_Enable, QMI8658St_Disable); + QMI8658_config_gyro(QMI8658_config.gyrRange, QMI8658_config.gyrOdr, QMI8658Lpf_Enable, QMI8658St_Disable); + QMI8658_config_mag(QMI8658_config.magDev, QMI8658_config.magOdr); + QMI8658_write_reg(QMI8658Register_Ctrl6, odr); +} + +unsigned char QMI8658_readStatus0(void) +{ + unsigned char status[2]; + + QMI8658_read_reg(QMI8658Register_Status0, status, sizeof(status)); + // printf("status[0x%x 0x%x]\n",status[0],status[1]); + + return status[0]; +} +/*! + * \brief Blocking read of data status register 1 (::QMI8658Register_Status1). + * \returns Status byte \see STATUS1 for flag definitions. + */ +unsigned char QMI8658_readStatus1(void) +{ + unsigned char status; + + QMI8658_read_reg(QMI8658Register_Status1, &status, sizeof(status)); + + return status; +} + +float QMI8658_readTemp(void) +{ + unsigned char buf[2]; + short temp = 0; + float temp_f = 0; + + QMI8658_read_reg(QMI8658Register_Tempearture_L, buf, 2); + temp = ((short)buf[1] << 8) | buf[0]; + temp_f = (float)temp / 256.0f; + + return temp_f; +} + +void QMI8658_read_acc_xyz(float acc_xyz[3]) +{ + unsigned char buf_reg[6]; + short raw_acc_xyz[3]; + + QMI8658_read_reg(QMI8658Register_Ax_L, buf_reg, 6); // 0x19, 25 + raw_acc_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0])); + raw_acc_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2])); + raw_acc_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4])); + + acc_xyz[0] = (raw_acc_xyz[0] * ONE_G) / acc_lsb_div; + acc_xyz[1] = (raw_acc_xyz[1] * ONE_G) / acc_lsb_div; + acc_xyz[2] = (raw_acc_xyz[2] * ONE_G) / acc_lsb_div; + + // QMI8658_printf("fis210x acc: %f %f %f\n", acc_xyz[0], acc_xyz[1], acc_xyz[2]); +} + +void QMI8658_read_gyro_xyz(float gyro_xyz[3]) +{ + unsigned char buf_reg[6]; + short raw_gyro_xyz[3]; + + QMI8658_read_reg(QMI8658Register_Gx_L, buf_reg, 6); // 0x1f, 31 + raw_gyro_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0])); + raw_gyro_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2])); + raw_gyro_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4])); + + gyro_xyz[0] = (raw_gyro_xyz[0] * 1.0f) / gyro_lsb_div; + gyro_xyz[1] = (raw_gyro_xyz[1] * 1.0f) / gyro_lsb_div; + gyro_xyz[2] = (raw_gyro_xyz[2] * 1.0f) / gyro_lsb_div; + + // QMI8658_printf("fis210x gyro: %f %f %f\n", gyro_xyz[0], gyro_xyz[1], gyro_xyz[2]); +} + +void QMI8658_read_xyz(float acc[3], float gyro[3], unsigned int *tim_count) +{ + unsigned char buf_reg[12]; + short raw_acc_xyz[3]; + short raw_gyro_xyz[3]; + // float acc_t[3]; + // float gyro_t[3]; + + if (tim_count) + { + unsigned char buf[3]; + unsigned int timestamp; + QMI8658_read_reg(QMI8658Register_Timestamp_L, buf, 3); // 0x18 24 + timestamp = (unsigned int)(((unsigned int)buf[2] << 16) | ((unsigned int)buf[1] << 8) | buf[0]); + if (timestamp > imu_timestamp) + imu_timestamp = timestamp; + else + imu_timestamp = (timestamp + 0x1000000 - imu_timestamp); + + *tim_count = imu_timestamp; + } + + QMI8658_read_reg(QMI8658Register_Ax_L, buf_reg, 12); // 0x19, 25 + raw_acc_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0])); + raw_acc_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2])); + raw_acc_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4])); + + raw_gyro_xyz[0] = (short)((unsigned short)(buf_reg[7] << 8) | (buf_reg[6])); + raw_gyro_xyz[1] = (short)((unsigned short)(buf_reg[9] << 8) | (buf_reg[8])); + raw_gyro_xyz[2] = (short)((unsigned short)(buf_reg[11] << 8) | (buf_reg[10])); + +#if defined(QMI8658_UINT_MG_DPS) + // mg + acc[AXIS_X] = (float)(raw_acc_xyz[AXIS_X] * 1000.0f) / acc_lsb_div; + acc[AXIS_Y] = (float)(raw_acc_xyz[AXIS_Y] * 1000.0f) / acc_lsb_div; + acc[AXIS_Z] = (float)(raw_acc_xyz[AXIS_Z] * 1000.0f) / acc_lsb_div; +#else + // m/s2 + acc[AXIS_X] = (float)(raw_acc_xyz[AXIS_X] * ONE_G) / acc_lsb_div; + acc[AXIS_Y] = (float)(raw_acc_xyz[AXIS_Y] * ONE_G) / acc_lsb_div; + acc[AXIS_Z] = (float)(raw_acc_xyz[AXIS_Z] * ONE_G) / acc_lsb_div; +#endif + // acc[AXIS_X] = imu_map.sign[AXIS_X]*acc_t[imu_map.map[AXIS_X]]; + // acc[AXIS_Y] = imu_map.sign[AXIS_Y]*acc_t[imu_map.map[AXIS_Y]]; + // acc[AXIS_Z] = imu_map.sign[AXIS_Z]*acc_t[imu_map.map[AXIS_Z]]; + +#if defined(QMI8658_UINT_MG_DPS) + // dps + gyro[0] = (float)(raw_gyro_xyz[0] * 1.0f) / gyro_lsb_div; + gyro[1] = (float)(raw_gyro_xyz[1] * 1.0f) / gyro_lsb_div; + gyro[2] = (float)(raw_gyro_xyz[2] * 1.0f) / gyro_lsb_div; +#else + // rad/s + gyro[AXIS_X] = (float)(raw_gyro_xyz[AXIS_X] * 0.01745f) / gyro_lsb_div; // *pi/180 + gyro[AXIS_Y] = (float)(raw_gyro_xyz[AXIS_Y] * 0.01745f) / gyro_lsb_div; + gyro[AXIS_Z] = (float)(raw_gyro_xyz[AXIS_Z] * 0.01745f) / gyro_lsb_div; +#endif + // gyro[AXIS_X] = imu_map.sign[AXIS_X]*gyro_t[imu_map.map[AXIS_X]]; + // gyro[AXIS_Y] = imu_map.sign[AXIS_Y]*gyro_t[imu_map.map[AXIS_Y]]; + // gyro[AXIS_Z] = imu_map.sign[AXIS_Z]*gyro_t[imu_map.map[AXIS_Z]]; +} + +void QMI8658_read_xyz_raw(short raw_acc_xyz[3], short raw_gyro_xyz[3], unsigned int *tim_count) +{ + unsigned char buf_reg[12]; + + if (tim_count) + { + unsigned char buf[3]; + unsigned int timestamp; + QMI8658_read_reg(QMI8658Register_Timestamp_L, buf, 3); // 0x18 24 + timestamp = (unsigned int)(((unsigned int)buf[2] << 16) | ((unsigned int)buf[1] << 8) | buf[0]); + if (timestamp > imu_timestamp) + imu_timestamp = timestamp; + else + imu_timestamp = (timestamp + 0x1000000 - imu_timestamp); + + *tim_count = imu_timestamp; + } + QMI8658_read_reg(QMI8658Register_Ax_L, buf_reg, 12); // 0x19, 25 + + raw_acc_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0])); + raw_acc_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2])); + raw_acc_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4])); + + raw_gyro_xyz[0] = (short)((unsigned short)(buf_reg[7] << 8) | (buf_reg[6])); + raw_gyro_xyz[1] = (short)((unsigned short)(buf_reg[9] << 8) | (buf_reg[8])); + raw_gyro_xyz[2] = (short)((unsigned short)(buf_reg[11] << 8) | (buf_reg[10])); +} + +void QMI8658_read_ae(float quat[4], float velocity[3]) +{ + unsigned char buf_reg[14]; + short raw_q_xyz[4]; + short raw_v_xyz[3]; + + QMI8658_read_reg(QMI8658Register_Q1_L, buf_reg, 14); + raw_q_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0])); + raw_q_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2])); + raw_q_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4])); + raw_q_xyz[3] = (short)((unsigned short)(buf_reg[7] << 8) | (buf_reg[6])); + + raw_v_xyz[1] = (short)((unsigned short)(buf_reg[9] << 8) | (buf_reg[8])); + raw_v_xyz[2] = (short)((unsigned short)(buf_reg[11] << 8) | (buf_reg[10])); + raw_v_xyz[2] = (short)((unsigned short)(buf_reg[13] << 8) | (buf_reg[12])); + + quat[0] = (float)(raw_q_xyz[0] * 1.0f) / ae_q_lsb_div; + quat[1] = (float)(raw_q_xyz[1] * 1.0f) / ae_q_lsb_div; + quat[2] = (float)(raw_q_xyz[2] * 1.0f) / ae_q_lsb_div; + quat[3] = (float)(raw_q_xyz[3] * 1.0f) / ae_q_lsb_div; + + velocity[0] = (float)(raw_v_xyz[0] * 1.0f) / ae_v_lsb_div; + velocity[1] = (float)(raw_v_xyz[1] * 1.0f) / ae_v_lsb_div; + velocity[2] = (float)(raw_v_xyz[2] * 1.0f) / ae_v_lsb_div; +} + +void QMI8658_enableWakeOnMotion(void) +{ + unsigned char womCmd[3]; + enum QMI8658_Interrupt interrupt = QMI8658_Int1; + enum QMI8658_InterruptState initialState = QMI8658State_low; + enum QMI8658_WakeOnMotionThreshold threshold = QMI8658WomThreshold_low; + unsigned char blankingTime = 0x00; + const unsigned char blankingTimeMask = 0x3F; + + QMI8658_enableSensors(QMI8658_CTRL7_DISABLE_ALL); + QMI8658_config_acc(QMI8658AccRange_2g, QMI8658AccOdr_LowPower_21Hz, QMI8658Lpf_Disable, QMI8658St_Disable); + + womCmd[0] = QMI8658Register_Cal1_L; // WoM Threshold: absolute value in mg (with 1mg/LSB resolution) + womCmd[1] = threshold; + womCmd[2] = (unsigned char)interrupt | (unsigned char)initialState | (blankingTime & blankingTimeMask); + QMI8658_write_reg(QMI8658Register_Cal1_L, womCmd[1]); + QMI8658_write_reg(QMI8658Register_Cal1_H, womCmd[2]); + + // QMI8658_doCtrl9Command(Ctrl9_ConfigureWakeOnMotion); + QMI8658_enableSensors(QMI8658_CTRL7_ACC_ENABLE); + // while(1) + //{ + // QMI8658_read_reg(QMI8658Register_Status1,&womCmd[0],1); + // if(womCmd[0]&0x01) + // break; + // } +} + +void QMI8658_disableWakeOnMotion(void) +{ + QMI8658_enableSensors(QMI8658_CTRL7_DISABLE_ALL); + QMI8658_write_reg(QMI8658Register_Cal1_L, 0); + // QMI8658_doCtrl9Command(Ctrl9_ConfigureWakeOnMotion); +} + +void QMI8658_enableSensors(unsigned char enableFlags) +{ + if (enableFlags & QMI8658_CONFIG_AE_ENABLE) + { + enableFlags |= QMI8658_CTRL7_ACC_ENABLE | QMI8658_CTRL7_GYR_ENABLE; + } + + QMI8658_write_reg(QMI8658Register_Ctrl7, enableFlags & QMI8658_CTRL7_ENABLE_MASK); +} + +void QMI8658_Config_apply(struct QMI8658Config const *config) +{ + unsigned char fisSensors = config->inputSelection; + + if (fisSensors & QMI8658_CONFIG_AE_ENABLE) + { + QMI8658_config_ae(config->aeOdr); + } + else + { + if (config->inputSelection & QMI8658_CONFIG_ACC_ENABLE) + { + QMI8658_config_acc(config->accRange, config->accOdr, QMI8658Lpf_Enable, QMI8658St_Disable); + } + if (config->inputSelection & QMI8658_CONFIG_GYR_ENABLE) + { + QMI8658_config_gyro(config->gyrRange, config->gyrOdr, QMI8658Lpf_Enable, QMI8658St_Disable); + } + } + + if (config->inputSelection & QMI8658_CONFIG_MAG_ENABLE) + { + QMI8658_config_mag(config->magDev, config->magOdr); + } + QMI8658_enableSensors(fisSensors); +} + +unsigned char QMI8658_init(void) +{ + unsigned char QMI8658_chip_id = 0x00; + unsigned char QMI8658_revision_id = 0x00; + unsigned char QMI8658_slave[2] = {QMI8658_SLAVE_ADDR_L, QMI8658_SLAVE_ADDR_H}; + unsigned char iCount = 0; + int retry = 0; + + while (iCount < 2) + { + QMI8658_slave_addr = QMI8658_slave[iCount]; + retry = 0; + + while ((QMI8658_chip_id != 0x05) && (retry++ < 5)) + { + + QMI8658_read_reg(QMI8658Register_WhoAmI, &QMI8658_chip_id, 1); + QMI8658_printf("QMI8658Register_WhoAmI = 0x%x\n", QMI8658_chip_id); + } + if (QMI8658_chip_id == 0x05) + { + break; + } + iCount++; + } + QMI8658_read_reg(QMI8658Register_Revision, &QMI8658_revision_id, 1); + if (QMI8658_chip_id == 0x05) + { + QMI8658_printf("QMI8658_init slave=0x%x \r\nQMI8658Register_WhoAmI=0x%x 0x%x\n", QMI8658_slave_addr, QMI8658_chip_id, QMI8658_revision_id); + QMI8658_write_reg(QMI8658Register_Ctrl1, 0x60); + QMI8658_config.inputSelection = QMI8658_CONFIG_ACCGYR_ENABLE; + QMI8658_config.accRange = QMI8658AccRange_8g; + QMI8658_config.accOdr = QMI8658AccOdr_1000Hz; + QMI8658_config.gyrRange = QMI8658GyrRange_2048dps; + QMI8658_config.gyrOdr = QMI8658GyrOdr_1000Hz; + QMI8658_config.magOdr = QMI8658MagOdr_125Hz; + QMI8658_config.magDev = MagDev_AKM09918; + QMI8658_config.aeOdr = QMI8658AeOdr_128Hz; + + QMI8658_Config_apply(&QMI8658_config); + if (1) + { + unsigned char read_data = 0x00; + QMI8658_read_reg(QMI8658Register_Ctrl1, &read_data, 1); + QMI8658_printf("QMI8658Register_Ctrl1=0x%x \n", read_data); + QMI8658_read_reg(QMI8658Register_Ctrl2, &read_data, 1); + QMI8658_printf("QMI8658Register_Ctrl2=0x%x \n", read_data); + QMI8658_read_reg(QMI8658Register_Ctrl3, &read_data, 1); + QMI8658_printf("QMI8658Register_Ctrl3=0x%x \n", read_data); + QMI8658_read_reg(QMI8658Register_Ctrl4, &read_data, 1); + QMI8658_printf("QMI8658Register_Ctrl4=0x%x \n", read_data); + QMI8658_read_reg(QMI8658Register_Ctrl5, &read_data, 1); + QMI8658_printf("QMI8658Register_Ctrl5=0x%x \n", read_data); + QMI8658_read_reg(QMI8658Register_Ctrl6, &read_data, 1); + QMI8658_printf("QMI8658Register_Ctrl6=0x%x \n", read_data); + QMI8658_read_reg(QMI8658Register_Ctrl7, &read_data, 1); + QMI8658_printf("QMI8658Register_Ctrl7=0x%x \n", read_data); + } + // QMI8658_set_layout(2); + return 1; + } + else + { + QMI8658_printf("QMI8658_init fail\n"); + QMI8658_chip_id = 0; + return 0; + } + // return QMI8658_chip_id; +} diff --git a/lib/QMI8658/QMI8658.h b/lib/QMI8658/QMI8658.h new file mode 100644 index 0000000000..3ddc859581 --- /dev/null +++ b/lib/QMI8658/QMI8658.h @@ -0,0 +1,524 @@ +/***************************************************************************** +* | File : QMI8658.h +* | Author : +* | Function : Hardware Interface of QMI8658 Sensor +* | Info : +*---------------- +* | This version: V1.0 +* | Date : 2022-06-07 +* | Info : +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documnetation 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 theex Software, and to permit persons to whom the Software is +# furished 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 OR 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. +******************************************************************************/ +#ifndef QMI8658_H +#define QMI8658_H + +#include +#include +#include //itoa() +#include + +#ifndef M_PI +#define M_PI (3.14159265358979323846f) +#endif +#ifndef ONE_G +#define ONE_G (9.807f) +#endif + +#define QMI8658_SLAVE_ADDR_L 0x6a +#define QMI8658_SLAVE_ADDR_H 0x6b + +#define QMI8658_CTRL7_DISABLE_ALL (0x0) +#define QMI8658_CTRL7_ACC_ENABLE (0x1) +#define QMI8658_CTRL7_GYR_ENABLE (0x2) +#define QMI8658_CTRL7_MAG_ENABLE (0x4) +#define QMI8658_CTRL7_AE_ENABLE (0x8) +#define QMI8658_CTRL7_GYR_SNOOZE_ENABLE (0x10) +#define QMI8658_CTRL7_ENABLE_MASK (0xF) + +#define QMI8658_CONFIG_ACC_ENABLE QMI8658_CTRL7_ACC_ENABLE +#define QMI8658_CONFIG_GYR_ENABLE QMI8658_CTRL7_GYR_ENABLE +#define QMI8658_CONFIG_MAG_ENABLE QMI8658_CTRL7_MAG_ENABLE +#define QMI8658_CONFIG_AE_ENABLE QMI8658_CTRL7_AE_ENABLE +#define QMI8658_CONFIG_ACCGYR_ENABLE (QMI8658_CONFIG_ACC_ENABLE | QMI8658_CONFIG_GYR_ENABLE) +#define QMI8658_CONFIG_ACCGYRMAG_ENABLE (QMI8658_CONFIG_ACC_ENABLE | QMI8658_CONFIG_GYR_ENABLE | QMI8658_CONFIG_MAG_ENABLE) +#define QMI8658_CONFIG_AEMAG_ENABLE (QMI8658_CONFIG_AE_ENABLE | QMI8658_CONFIG_MAG_ENABLE) + +#define QMI8658_STATUS1_CMD_DONE (0x01) +#define QMI8658_STATUS1_WAKEUP_EVENT (0x04) + +enum QMI8658Register +{ + /*! \brief FIS device identifier register. */ + QMI8658Register_WhoAmI = 0, // 0 + /*! \brief FIS hardware revision register. */ + QMI8658Register_Revision, // 1 + /*! \brief General and power management modes. */ + QMI8658Register_Ctrl1, // 2 + /*! \brief Accelerometer control. */ + QMI8658Register_Ctrl2, // 3 + /*! \brief Gyroscope control. */ + QMI8658Register_Ctrl3, // 4 + /*! \brief Magnetometer control. */ + QMI8658Register_Ctrl4, // 5 + /*! \brief Data processing settings. */ + QMI8658Register_Ctrl5, // 6 + /*! \brief AttitudeEngine control. */ + QMI8658Register_Ctrl6, // 7 + /*! \brief Sensor enabled status. */ + QMI8658Register_Ctrl7, // 8 + /*! \brief Reserved - do not write. */ + QMI8658Register_Ctrl8, // 9 + /*! \brief Host command register. */ + QMI8658Register_Ctrl9, // 10 + /*! \brief Calibration register 1 most significant byte. */ + QMI8658Register_Cal1_L = 11, + /*! \brief Calibration register 1 least significant byte. */ + QMI8658Register_Cal1_H, + /*! \brief Calibration register 2 most significant byte. */ + QMI8658Register_Cal2_L, + /*! \brief Calibration register 2 least significant byte. */ + QMI8658Register_Cal2_H, + /*! \brief Calibration register 3 most significant byte. */ + QMI8658Register_Cal3_L, + /*! \brief Calibration register 3 least significant byte. */ + QMI8658Register_Cal3_H, + /*! \brief Calibration register 4 most significant byte. */ + QMI8658Register_Cal4_L, + /*! \brief Calibration register 4 least significant byte. */ + QMI8658Register_Cal4_H, + /*! \brief FIFO control register. */ + QMI8658Register_FifoCtrl = 19, + /*! \brief FIFO data register. */ + QMI8658Register_FifoData, // 20 + /*! \brief FIFO status register. */ + QMI8658Register_FifoStatus, // 21 + /*! \brief Output data overrun and availability. */ + QMI8658Register_StatusInt = 45, + /*! \brief Output data overrun and availability. */ + QMI8658Register_Status0, + /*! \brief Miscellaneous status register. */ + QMI8658Register_Status1, + /*! \brief timestamp low. */ + QMI8658Register_Timestamp_L = 48, + /*! \brief timestamp low. */ + QMI8658Register_Timestamp_M, + /*! \brief timestamp low. */ + QMI8658Register_Timestamp_H, + /*! \brief tempearture low. */ + QMI8658Register_Tempearture_L = 51, + /*! \brief tempearture low. */ + QMI8658Register_Tempearture_H, + /*! \brief Accelerometer X axis least significant byte. */ + QMI8658Register_Ax_L = 53, + /*! \brief Accelerometer X axis most significant byte. */ + QMI8658Register_Ax_H, + /*! \brief Accelerometer Y axis least significant byte. */ + QMI8658Register_Ay_L, + /*! \brief Accelerometer Y axis most significant byte. */ + QMI8658Register_Ay_H, + /*! \brief Accelerometer Z axis least significant byte. */ + QMI8658Register_Az_L, + /*! \brief Accelerometer Z axis most significant byte. */ + QMI8658Register_Az_H, + /*! \brief Gyroscope X axis least significant byte. */ + QMI8658Register_Gx_L = 59, + /*! \brief Gyroscope X axis most significant byte. */ + QMI8658Register_Gx_H, + /*! \brief Gyroscope Y axis least significant byte. */ + QMI8658Register_Gy_L, + /*! \brief Gyroscope Y axis most significant byte. */ + QMI8658Register_Gy_H, + /*! \brief Gyroscope Z axis least significant byte. */ + QMI8658Register_Gz_L, + /*! \brief Gyroscope Z axis most significant byte. */ + QMI8658Register_Gz_H, + /*! \brief Magnetometer X axis least significant byte. */ + QMI8658Register_Mx_L = 65, + /*! \brief Magnetometer X axis most significant byte. */ + QMI8658Register_Mx_H, + /*! \brief Magnetometer Y axis least significant byte. */ + QMI8658Register_My_L, + /*! \brief Magnetometer Y axis most significant byte. */ + QMI8658Register_My_H, + /*! \brief Magnetometer Z axis least significant byte. */ + QMI8658Register_Mz_L, + /*! \brief Magnetometer Z axis most significant byte. */ + QMI8658Register_Mz_H, + /*! \brief Quaternion increment W least significant byte. */ + QMI8658Register_Q1_L = 73, + /*! \brief Quaternion increment W most significant byte. */ + QMI8658Register_Q1_H, + /*! \brief Quaternion increment X least significant byte. */ + QMI8658Register_Q2_L, + /*! \brief Quaternion increment X most significant byte. */ + QMI8658Register_Q2_H, + /*! \brief Quaternion increment Y least significant byte. */ + QMI8658Register_Q3_L, + /*! \brief Quaternion increment Y most significant byte. */ + QMI8658Register_Q3_H, + /*! \brief Quaternion increment Z least significant byte. */ + QMI8658Register_Q4_L, + /*! \brief Quaternion increment Z most significant byte. */ + QMI8658Register_Q4_H, + /*! \brief Velocity increment X least significant byte. */ + QMI8658Register_Dvx_L = 81, + /*! \brief Velocity increment X most significant byte. */ + QMI8658Register_Dvx_H, + /*! \brief Velocity increment Y least significant byte. */ + QMI8658Register_Dvy_L, + /*! \brief Velocity increment Y most significant byte. */ + QMI8658Register_Dvy_H, + /*! \brief Velocity increment Z least significant byte. */ + QMI8658Register_Dvz_L, + /*! \brief Velocity increment Z most significant byte. */ + QMI8658Register_Dvz_H, + /*! \brief AttitudeEngine reg1. */ + QMI8658Register_AeReg1 = 87, + /*! \brief AttitudeEngine overflow flags. */ + QMI8658Register_AeOverflow, + + QMI8658Register_I2CM_STATUS = 110 +}; + +enum QMI8658_Ois_Register +{ + /*-----------------------------*/ + /* Setup and Control Registers */ + /*-----------------------------*/ + /*! \brief SPI Endian Selection, and SPI 3/4 Wire */ + QMI8658_OIS_Reg_Ctrl1 = 0x02, // 2 [0x02] -- Dflt: 0x20 + /*! \brief Accelerometer control: ODR, Full Scale, Self Test */ + QMI8658_OIS_Reg_Ctrl2, // 3 [0x03] + /*! \brief Gyroscope control: ODR, Full Scale, Self Test */ + QMI8658_OIS_Reg_Ctrl3, // 4 [0x04] + /*! \brief Sensor Data Processing Settings */ + QMI8658_OIS_Reg_Ctrl5 = 0x06, // 6 [0x06] + /*! \brief Sensor enabled status: Enable Sensors */ + QMI8658_OIS_Reg_Ctrl7 = 0x08, // 8 [0x08] + /*-------------------*/ + /* Status Registers */ + /*-------------------*/ + /*! \brief Sensor Data Availability and Lock Register */ + QMI8658_OIS_Reg_StatusInt = 0x2D, // 45 [0x2D] + /*! \brief Output data overrun and availability */ + QMI8658_OIS_Reg_Status0 = 0x2E, // 46 [0x2E] + + /*-----------------------------------------------------*/ + /* OIS Sensor Data Output Registers. 16-bit 2's complement */ + /*-----------------------------------------------------*/ + /*! \brief Accelerometer X axis least significant byte */ + QMI8658_OIS_Reg_Ax_L = 0x33, // 53 [0x35] + /*! \brief Accelerometer X axis most significant byte */ + QMI8658_OIS_Reg_Ax_H, // 54 [0x36] + /*! \brief Accelerometer Y axis least significant byte */ + QMI8658_OIS_Reg_Ay_L, // 55 [0x37] + /*! \brief Accelerometer Y axis most significant byte */ + QMI8658_OIS_Reg_Ay_H, // 56 [0x38] + /*! \brief Accelerometer Z axis least significant byte */ + QMI8658_OIS_Reg_Az_L, // 57 [0x39] + /*! \brief Accelerometer Z axis most significant byte */ + QMI8658_OIS_Reg_Az_H, // 58 [0x3A] + + /*! \brief Gyroscope X axis least significant byte */ + QMI8658_OIS_Reg_Gx_L = 0x3B, // 59 [0x3B] + /*! \brief Gyroscope X axis most significant byte */ + QMI8658_OIS_Reg_Gx_H, // 60 [0x3C] + /*! \brief Gyroscope Y axis least significant byte */ + QMI8658_OIS_Reg_Gy_L, // 61 [0x3D] + /*! \brief Gyroscope Y axis most significant byte */ + QMI8658_OIS_Reg_Gy_H, // 62 [0x3E] + /*! \brief Gyroscope Z axis least significant byte */ + QMI8658_OIS_Reg_Gz_L, // 63 [0x3F] + /*! \brief Gyroscope Z axis most significant byte */ + QMI8658_OIS_Reg_Gz_H, // 64 [0x40] +}; + +enum QMI8658_Ctrl9Command +{ + QMI8658_Ctrl9_Cmd_NOP = 0X00, + QMI8658_Ctrl9_Cmd_GyroBias = 0X01, + QMI8658_Ctrl9_Cmd_Rqst_Sdi_Mod = 0X03, + QMI8658_Ctrl9_Cmd_WoM_Setting = 0x08, + QMI8658_Ctrl9_Cmd_AccelHostDeltaOffset = 0x09, + QMI8658_Ctrl9_Cmd_GyroHostDeltaOffset = 0x0A, + QMI8658_Ctrl9_Cmd_Dbg_WoM_Data_Enable = 0xF8, + +}; + +enum QMI8658_LpfConfig +{ + QMI8658Lpf_Disable, /*!< \brief Disable low pass filter. */ + QMI8658Lpf_Enable /*!< \brief Enable low pass filter. */ +}; + +enum QMI8658_HpfConfig +{ + QMI8658Hpf_Disable, /*!< \brief Disable high pass filter. */ + QMI8658Hpf_Enable /*!< \brief Enable high pass filter. */ +}; + +enum QMI8658_StConfig +{ + QMI8658St_Disable, /*!< \brief Disable high pass filter. */ + QMI8658St_Enable /*!< \brief Enable high pass filter. */ +}; + +enum QMI8658_LpfMode +{ + A_LSP_MODE_0 = 0x00 << 1, + A_LSP_MODE_1 = 0x01 << 1, + A_LSP_MODE_2 = 0x02 << 1, + A_LSP_MODE_3 = 0x03 << 1, + + G_LSP_MODE_0 = 0x00 << 5, + G_LSP_MODE_1 = 0x01 << 5, + G_LSP_MODE_2 = 0x02 << 5, + G_LSP_MODE_3 = 0x03 << 5 +}; + +enum QMI8658_AccRange +{ + QMI8658AccRange_2g = 0x00 << 4, /*!< \brief +/- 2g range */ + QMI8658AccRange_4g = 0x01 << 4, /*!< \brief +/- 4g range */ + QMI8658AccRange_8g = 0x02 << 4, /*!< \brief +/- 8g range */ + QMI8658AccRange_16g = 0x03 << 4 /*!< \brief +/- 16g range */ +}; + +enum QMI8658_AccOdr +{ + QMI8658AccOdr_8000Hz = 0x00, /*!< \brief High resolution 8000Hz output rate. */ + QMI8658AccOdr_4000Hz = 0x01, /*!< \brief High resolution 4000Hz output rate. */ + QMI8658AccOdr_2000Hz = 0x02, /*!< \brief High resolution 2000Hz output rate. */ + QMI8658AccOdr_1000Hz = 0x03, /*!< \brief High resolution 1000Hz output rate. */ + QMI8658AccOdr_500Hz = 0x04, /*!< \brief High resolution 500Hz output rate. */ + QMI8658AccOdr_250Hz = 0x05, /*!< \brief High resolution 250Hz output rate. */ + QMI8658AccOdr_125Hz = 0x06, /*!< \brief High resolution 125Hz output rate. */ + QMI8658AccOdr_62_5Hz = 0x07, /*!< \brief High resolution 62.5Hz output rate. */ + QMI8658AccOdr_31_25Hz = 0x08, /*!< \brief High resolution 31.25Hz output rate. */ + QMI8658AccOdr_LowPower_128Hz = 0x0c, /*!< \brief Low power 128Hz output rate. */ + QMI8658AccOdr_LowPower_21Hz = 0x0d, /*!< \brief Low power 21Hz output rate. */ + QMI8658AccOdr_LowPower_11Hz = 0x0e, /*!< \brief Low power 11Hz output rate. */ + QMI8658AccOdr_LowPower_3Hz = 0x0f /*!< \brief Low power 3Hz output rate. */ +}; + +enum QMI8658_GyrRange +{ + QMI8658GyrRange_32dps = 0 << 4, /*!< \brief +-32 degrees per second. */ + QMI8658GyrRange_64dps = 1 << 4, /*!< \brief +-64 degrees per second. */ + QMI8658GyrRange_128dps = 2 << 4, /*!< \brief +-128 degrees per second. */ + QMI8658GyrRange_256dps = 3 << 4, /*!< \brief +-256 degrees per second. */ + QMI8658GyrRange_512dps = 4 << 4, /*!< \brief +-512 degrees per second. */ + QMI8658GyrRange_1024dps = 5 << 4, /*!< \brief +-1024 degrees per second. */ + QMI8658GyrRange_2048dps = 6 << 4, /*!< \brief +-2048 degrees per second. */ + QMI8658GyrRange_4096dps = 7 << 4 /*!< \brief +-2560 degrees per second. */ +}; + +/*! + * \brief Gyroscope output rate configuration. + */ +enum QMI8658_GyrOdr +{ + QMI8658GyrOdr_8000Hz = 0x00, /*!< \brief High resolution 8000Hz output rate. */ + QMI8658GyrOdr_4000Hz = 0x01, /*!< \brief High resolution 4000Hz output rate. */ + QMI8658GyrOdr_2000Hz = 0x02, /*!< \brief High resolution 2000Hz output rate. */ + QMI8658GyrOdr_1000Hz = 0x03, /*!< \brief High resolution 1000Hz output rate. */ + QMI8658GyrOdr_500Hz = 0x04, /*!< \brief High resolution 500Hz output rate. */ + QMI8658GyrOdr_250Hz = 0x05, /*!< \brief High resolution 250Hz output rate. */ + QMI8658GyrOdr_125Hz = 0x06, /*!< \brief High resolution 125Hz output rate. */ + QMI8658GyrOdr_62_5Hz = 0x07, /*!< \brief High resolution 62.5Hz output rate. */ + QMI8658GyrOdr_31_25Hz = 0x08 /*!< \brief High resolution 31.25Hz output rate. */ +}; + +enum QMI8658_AeOdr +{ + QMI8658AeOdr_1Hz = 0x00, /*!< \brief 1Hz output rate. */ + QMI8658AeOdr_2Hz = 0x01, /*!< \brief 2Hz output rate. */ + QMI8658AeOdr_4Hz = 0x02, /*!< \brief 4Hz output rate. */ + QMI8658AeOdr_8Hz = 0x03, /*!< \brief 8Hz output rate. */ + QMI8658AeOdr_16Hz = 0x04, /*!< \brief 16Hz output rate. */ + QMI8658AeOdr_32Hz = 0x05, /*!< \brief 32Hz output rate. */ + QMI8658AeOdr_64Hz = 0x06, /*!< \brief 64Hz output rate. */ + QMI8658AeOdr_128Hz = 0x07, /*!< \brief 128Hz output rate. */ + /*! + * \brief Motion on demand mode. + * + * In motion on demand mode the application can trigger AttitudeEngine + * output samples as necessary. This allows the AttitudeEngine to be + * synchronized with external data sources. + * + * When in Motion on Demand mode the application should request new data + * by calling the QMI8658_requestAttitudeEngineData() function. The + * AttitudeEngine will respond with a data ready event (INT2) when the + * data is available to be read. + */ + QMI8658AeOdr_motionOnDemand = 128 +}; + +enum QMI8658_MagOdr +{ + QMI8658MagOdr_1000Hz = 0x00, /*!< \brief 1000Hz output rate. */ + QMI8658MagOdr_500Hz = 0x01, /*!< \brief 500Hz output rate. */ + QMI8658MagOdr_250Hz = 0x02, /*!< \brief 250Hz output rate. */ + QMI8658MagOdr_125Hz = 0x03, /*!< \brief 125Hz output rate. */ + QMI8658MagOdr_62_5Hz = 0x04, /*!< \brief 62.5Hz output rate. */ + QMI8658MagOdr_31_25Hz = 0x05 /*!< \brief 31.25Hz output rate. */ +}; + +enum QMI8658_MagDev +{ + MagDev_AKM09918 = (0 << 3), /*!< \brief AKM09918. */ +}; + +enum QMI8658_AccUnit +{ + QMI8658AccUnit_g, /*!< \brief Accelerometer output in terms of g (9.81m/s^2). */ + QMI8658AccUnit_ms2 /*!< \brief Accelerometer output in terms of m/s^2. */ +}; + +enum QMI8658_GyrUnit +{ + QMI8658GyrUnit_dps, /*!< \brief Gyroscope output in degrees/s. */ + QMI8658GyrUnit_rads /*!< \brief Gyroscope output in rad/s. */ +}; + +struct QMI8658Config +{ + /*! \brief Sensor fusion input selection. */ + unsigned char inputSelection; + /*! \brief Accelerometer dynamic range configuration. */ + enum QMI8658_AccRange accRange; + /*! \brief Accelerometer output rate. */ + enum QMI8658_AccOdr accOdr; + /*! \brief Gyroscope dynamic range configuration. */ + enum QMI8658_GyrRange gyrRange; + /*! \brief Gyroscope output rate. */ + enum QMI8658_GyrOdr gyrOdr; + /*! \brief AttitudeEngine output rate. */ + enum QMI8658_AeOdr aeOdr; + /*! + * \brief Magnetometer output data rate. + * + * \remark This parameter is not used when using an external magnetometer. + * In this case the external magnetometer is sampled at the FIS output + * data rate, or at an integer divisor thereof such that the maximum + * sample rate is not exceeded. + */ + enum QMI8658_MagOdr magOdr; + + /*! + * \brief Magnetometer device to use. + * + * \remark This parameter is not used when using an external magnetometer. + */ + enum QMI8658_MagDev magDev; +}; + +#define QMI8658_SAMPLE_SIZE (3 * sizeof(short)) +#define QMI8658_AE_SAMPLE_SIZE ((4 + 3 + 1) * sizeof(short) + sizeof(unsigned char)) +struct FisImuRawSample +{ + /*! \brief The sample counter of the sample. */ + unsigned char timestamp[3]; + /*! + * \brief Pointer to accelerometer data in the sample buffer. + * + * \c NULL if no accelerometer data is available in the buffer. + */ + unsigned char const *accelerometerData; + /*! + * \brief Pointer to gyroscope data in the sample buffer. + * + * \c NULL if no gyroscope data is available in the buffer. + */ + unsigned char const *gyroscopeData; + /*! + * \brief Pointer to magnetometer data in the sample buffer. + * + * \c NULL if no magnetometer data is available in the buffer. + */ + unsigned char const *magnetometerData; + /*! + * \brief Pointer to AttitudeEngine data in the sample buffer. + * + * \c NULL if no AttitudeEngine data is available in the buffer. + */ + unsigned char const *attitudeEngineData; + /*! \brief Raw sample buffer. */ + unsigned char sampleBuffer[QMI8658_SAMPLE_SIZE + QMI8658_AE_SAMPLE_SIZE]; + /*! \brief Contents of the FIS status 1 register. */ + unsigned char status1; + // unsigned char status0; + // unsigned int durT; +}; + +struct QMI8658_offsetCalibration +{ + enum QMI8658_AccUnit accUnit; + float accOffset[3]; + enum QMI8658_GyrUnit gyrUnit; + float gyrOffset[3]; +}; + +struct QMI8658_sensitivityCalibration +{ + float accSensitivity[3]; + float gyrSensitivity[3]; +}; + +enum QMI8658_Interrupt +{ + /*! \brief FIS INT1 line. */ + QMI8658_Int1 = (0 << 6), + /*! \brief FIS INT2 line. */ + QMI8658_Int2 = (1 << 6) +}; + +enum QMI8658_InterruptState +{ + QMI8658State_high = (1 << 7), /*!< Interrupt high. */ + QMI8658State_low = (0 << 7) /*!< Interrupt low. */ +}; + +enum QMI8658_WakeOnMotionThreshold +{ + QMI8658WomThreshold_high = 128, /*!< High threshold - large motion needed to wake. */ + QMI8658WomThreshold_low = 32 /*!< Low threshold - small motion needed to wake. */ +}; + +extern int16_t (*QMI8658_I2C_Write)(uint8_t address, uint8_t *data, uint16_t len, bool isBlock); +extern int16_t (*QMI8658_I2C_ReadRegister)(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len); + +extern unsigned char QMI8658_write_reg(unsigned char reg, unsigned char value); +extern unsigned char QMI8658_read_reg(unsigned char reg, unsigned char *buf, unsigned short len); +extern unsigned char QMI8658_init(void); +extern void QMI8658_Config_apply(struct QMI8658Config const *config); +extern void QMI8658_enableSensors(unsigned char enableFlags); +extern void QMI8658_read_acc_xyz(float acc_xyz[3]); +extern void QMI8658_read_gyro_xyz(float gyro_xyz[3]); +extern void QMI8658_read_xyz(float acc[3], float gyro[3], unsigned int *tim_count); +extern void QMI8658_read_xyz_raw(short raw_acc_xyz[3], short raw_gyro_xyz[3], unsigned int *tim_count); +extern void QMI8658_read_ae(float quat[4], float velocity[3]); +extern unsigned char QMI8658_readStatus0(void); +extern unsigned char QMI8658_readStatus1(void); +extern float QMI8658_readTemp(void); +extern void QMI8658_enableWakeOnMotion(void); +extern void QMI8658_disableWakeOnMotion(void); + +#endif diff --git a/lib/QMI8658/README.md b/lib/QMI8658/README.md new file mode 100644 index 0000000000..d61fef3b97 --- /dev/null +++ b/lib/QMI8658/README.md @@ -0,0 +1,4 @@ +# QMI8658 + +Ported from (MIT License) to +GP2040-CE PicoPeripherals compatible interface. diff --git a/proto/config.proto b/proto/config.proto index af78fe71b7..d005dbe71a 100644 --- a/proto/config.proto +++ b/proto/config.proto @@ -788,6 +788,11 @@ message ReactiveLEDOptions repeated ReactiveLEDInfo leds = 2 [(nanopb).max_count = 10]; } +message QMI8658Options +{ + optional bool enabled = 1; +} + message AddonOptions { optional BootselButtonOptions bootselButtonOptions = 1; @@ -817,6 +822,7 @@ message AddonOptions optional PCF8575Options pcf8575Options = 25; optional DRV8833RumbleOptions drv8833RumbleOptions = 26; optional ReactiveLEDOptions reactiveLEDOptions = 27; + optional QMI8658Options qmi8658Options = 28; } message MigrationHistory diff --git a/src/addons/i2c_qmi8658.cpp b/src/addons/i2c_qmi8658.cpp new file mode 100644 index 0000000000..d86f6f9403 --- /dev/null +++ b/src/addons/i2c_qmi8658.cpp @@ -0,0 +1,58 @@ +#include "addons/i2c_qmi8658.h" +#include "storagemanager.h" +#include "helper.h" +#include "config.pb.h" + +bool I2CQMI8658Input::available() { + const QMI8658Options& options = Storage::getInstance().getAddonOptions().qmi8658Options; + if (options.enabled) { + PeripheralI2CScanResult result = PeripheralManager::getInstance().scanForI2CDevice({QMI8658_SLAVE_ADDR_L, QMI8658_SLAVE_ADDR_H}); + if (result.address > -1) { + i2c = PeripheralManager::getInstance().getI2C(result.block); + return true; + } + } + return false; +} + +static PeripheralI2C *QMI8658Input_I2C; +static int16_t QMI8658Input_I2C_Write(uint8_t address, uint8_t *data, uint16_t len, bool isBlock) { + return QMI8658Input_I2C->write(address, data, len, isBlock); +} +static int16_t QMI8658Input_I2C_ReadRegister(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len) { + return QMI8658Input_I2C->readRegister(address, reg, data, len); +} + +void I2CQMI8658Input::setup() { + uIntervalMS = 1; + nextTimer = getMillis(); + + QMI8658Input_I2C = i2c; + QMI8658_I2C_Write = &QMI8658Input_I2C_Write; + QMI8658_I2C_ReadRegister = &QMI8658Input_I2C_ReadRegister; + if (QMI8658_init()) { + Gamepad * gamepad = Storage::getInstance().GetGamepad(); + gamepad->auxState.sensors.accelerometer.enabled = true; + gamepad->auxState.sensors.gyroscope.enabled = true; + } +} + +void I2CQMI8658Input::process() { + if (nextTimer < getMillis()) { + unsigned int tim_count = 0; + QMI8658_read_xyz_raw(acc, gyro, &tim_count); + nextTimer = getMillis() + uIntervalMS; + } + + Gamepad* gamepad = Storage::getInstance().GetGamepad(); + + gamepad->auxState.sensors.accelerometer.x = acc[0]; + gamepad->auxState.sensors.accelerometer.y = acc[1]; + gamepad->auxState.sensors.accelerometer.z = acc[2]; + gamepad->auxState.sensors.accelerometer.active = true; + + gamepad->auxState.sensors.gyroscope.x = gyro[0]; + gamepad->auxState.sensors.gyroscope.y = gyro[1]; + gamepad->auxState.sensors.gyroscope.z = gyro[2]; + gamepad->auxState.sensors.gyroscope.active = true; +} diff --git a/src/config_utils.cpp b/src/config_utils.cpp index 05bc0e0f87..9ff7ecc60b 100644 --- a/src/config_utils.cpp +++ b/src/config_utils.cpp @@ -34,6 +34,7 @@ #include "addons/rotaryencoder.h" #include "addons/i2c_gpio_pcf8575.h" #include "addons/drv8833_rumble.h" +#include "addons/i2c_qmi8658.h" #include "CRC32.h" #include "FlashPROM.h" @@ -675,6 +676,9 @@ void ConfigUtils::initUnsetPropertiesWithDefaults(Config& config) INIT_UNSET_PROPERTY(config.addonOptions.pcf8575Options, enabled, I2C_PCF8575_ENABLED); INIT_UNSET_PROPERTY(config.addonOptions.pcf8575Options, deprecatedI2cBlock, (I2C_PCF8575_BLOCK == i2c0) ? 0 : 1); + // addonOptions.QMI8658Options + INIT_UNSET_PROPERTY(config.addonOptions.qmi8658Options, enabled, !!I2C_QMI8658_ENABLED); + GpioAction pcf8575Actions[PCF8575_PIN_COUNT] = { PCF8575_PIN00_ACTION,PCF8575_PIN01_ACTION,PCF8575_PIN02_ACTION,PCF8575_PIN03_ACTION, PCF8575_PIN04_ACTION,PCF8575_PIN05_ACTION,PCF8575_PIN06_ACTION,PCF8575_PIN07_ACTION, diff --git a/src/configs/webconfig.cpp b/src/configs/webconfig.cpp index a77fdae53e..9b28e0c669 100644 --- a/src/configs/webconfig.cpp +++ b/src/configs/webconfig.cpp @@ -1628,6 +1628,9 @@ std::string setAddonOptions() docToValue(drv8833RumbleOptions.dutyMin, doc, "drv8833RumbleDutyMin"); docToValue(drv8833RumbleOptions.dutyMax, doc, "drv8833RumbleDutyMax"); + QMI8658Options& qmi8658Options = Storage::getInstance().getAddonOptions().qmi8658Options; + docToValue(qmi8658Options.enabled, doc, "I2CQMI8658InputEnabled"); + Storage::getInstance().save(); return serialize_json(doc); @@ -2064,6 +2067,9 @@ std::string getAddonOptions() writeDoc(doc, "drv8833RumbleDutyMin", drv8833RumbleOptions.dutyMin); writeDoc(doc, "drv8833RumbleDutyMax", drv8833RumbleOptions.dutyMax); + const QMI8658Options& qmi8658Options = Storage::getInstance().getAddonOptions().qmi8658Options; + writeDoc(doc, "I2CQMI8658InputEnabled", qmi8658Options.enabled); + return serialize_json(doc); } diff --git a/src/gp2040.cpp b/src/gp2040.cpp index ed76528e6d..e59005fbe2 100644 --- a/src/gp2040.cpp +++ b/src/gp2040.cpp @@ -30,6 +30,7 @@ #include "addons/snes_input.h" #include "addons/rotaryencoder.h" #include "addons/i2c_gpio_pcf8575.h" +#include "addons/i2c_qmi8658.h" // Pico includes #include "pico/bootrom.h" @@ -103,6 +104,7 @@ void GP2040::setup() { addons.LoadAddon(new TiltInput(), CORE0_INPUT); addons.LoadAddon(new RotaryEncoderInput(), CORE0_INPUT); addons.LoadAddon(new PCF8575Addon(), CORE0_INPUT); + addons.LoadAddon(new I2CQMI8658Input(), CORE0_INPUT); // Input override addons addons.LoadAddon(new ReverseInput(), CORE0_INPUT); diff --git a/www/server/app.js b/www/server/app.js index e31241fc64..d63bc3f27f 100644 --- a/www/server/app.js +++ b/www/server/app.js @@ -526,6 +526,7 @@ app.get('/api/getAddonsOptions', (req, res) => { PCF8575AddonEnabled: 1, DRV8833RumbleAddonEnabled: 1, ReactiveLEDAddonEnabled: 1, + I2CQMI8658InputEnabled: 1, usedPins: Object.values(picoController), }); }); diff --git a/www/src/Addons/I2CQMI8658.tsx b/www/src/Addons/I2CQMI8658.tsx new file mode 100644 index 0000000000..f089a9b6fa --- /dev/null +++ b/www/src/Addons/I2CQMI8658.tsx @@ -0,0 +1,74 @@ +import { AppContext } from '../Contexts/AppContext'; +import React, { useContext } from 'react'; +import { Trans, useTranslation } from 'react-i18next'; +import { FormCheck, Row, FormLabel } from 'react-bootstrap'; +import { NavLink } from 'react-router-dom'; +import * as yup from 'yup'; + +import Section from '../Components/Section'; +import FormSelect from '../Components/FormSelect'; + +import FormControl from '../Components/FormControl'; +import { I2C_BLOCKS } from '../Data/Peripherals'; + +export const i2cQMI8658Scheme = { + I2CQMI8658InputEnabled: yup.number().label('I2C QMI8658 Input Enabled'), +}; + +export const i2cQMI8658State = { + I2CQMI8658InputEnabled: 0, +}; + +const I2CQMI8658 = ({ values, errors, handleChange, handleCheckbox }) => { + const { t } = useTranslation(); + const { getAvailablePeripherals, getSelectedPeripheral } = + useContext(AppContext); + + const handlePeripheralChange = (e) => { + let device = getSelectedPeripheral('i2c', e.target.value); + handleChange(e); + }; + + return ( +
+ + {getAvailablePeripherals('i2c') ? ( + { + handleCheckbox('I2CQMI8658InputEnabled', values); + handleChange(e); + }} + /> + ) : ( + + + + {t('PeripheralMapping:header-text')} + + + + )} +
+ ); +}; + +export default I2CQMI8658; diff --git a/www/src/Locales/en/AddonsConfig.jsx b/www/src/Locales/en/AddonsConfig.jsx index 17b05ebbcc..9ae5f48146 100644 --- a/www/src/Locales/en/AddonsConfig.jsx +++ b/www/src/Locales/en/AddonsConfig.jsx @@ -167,4 +167,5 @@ export default { 'drv8833-rumble-pwm-frequency-label': 'PWM Frequency', 'drv8833-rumble-duty-min-label': 'Minimum Duty Cycle', 'drv8833-rumble-duty-max-label': 'Maximum Duty Cycle', + 'i2c-qmi8658-header-text': 'QMI8658 IMU', }; diff --git a/www/src/Pages/AddonsConfigPage.jsx b/www/src/Pages/AddonsConfigPage.jsx index 394d343b4f..bfdeea7d7c 100644 --- a/www/src/Pages/AddonsConfigPage.jsx +++ b/www/src/Pages/AddonsConfigPage.jsx @@ -55,6 +55,10 @@ import DRV8833Rumble, { drv8833RumbleState, } from '../Addons/DRV8833'; import ReactiveLED, { reactiveLEDScheme, reactiveLEDState } from '../Addons/ReactiveLED'; +import I2CQMI8658, { + i2cQMI8658Scheme, + i2cQMI8658State, +} from '../Addons/I2CQMI8658'; const schema = yup.object().shape({ ...analogScheme, @@ -77,6 +81,7 @@ const schema = yup.object().shape({ ...pcf8575Scheme, ...drv8833RumbleScheme, ...reactiveLEDScheme, + ...i2cQMI8658Scheme, }); const defaultValues = { @@ -101,6 +106,7 @@ const defaultValues = { ...pcf8575State, ...drv8833RumbleState, ...reactiveLEDState, + ...i2cQMI8658State, }; const ADDONS = [ @@ -125,6 +131,7 @@ const ADDONS = [ PCF8575, DRV8833Rumble, ReactiveLED, + I2CQMI8658, ]; const FormContext = ({ setStoredData }) => {