1+ /*
2+ * Copyright (c) 2024 xyzroe <i@xyzroe.cc>
3+ *
4+ * Redistribution and use in source and binary forms, with or without
5+ * modification, are permitted provided that the following conditions are met:
6+ *
7+ * 1. Redistributions of source code must retain the above copyright notice,
8+ * this list of conditions and the following disclaimer.
9+ *
10+ * 2. Redistributions in binary form must reproduce the above copyright notice,
11+ * this list of conditions and the following disclaimer in the documentation
12+ * and/or other materials provided with the distribution.
13+ *
14+ * 3. Neither the name of the copyright holder nor the names of its contributors
15+ * may be used to endorse or promote products derived from this software without
16+ * specific prior written permission.
17+ *
18+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+ */
29+
30+ /**
31+ * @file qmi8658c.c
32+ * @brief QMI8658C sensor driver
33+ * @author xyzroe
34+ * ESP-IDF driver for QMI8658C sensor
35+ *
36+ * Datasheet: https://qstcorp.com/upload/pdf/202202/QMI8658C%20datasheet%20rev%200.9.pdf
37+ *
38+ * Copyright (c) 2024 xyzroe <i@xyzroe.cc>
39+ */
40+
41+ #include <stdio.h>
42+ #include <freertos/FreeRTOS.h>
43+ #include <freertos/task.h>
44+ #include <esp_log.h>
45+ #include <esp_idf_lib_helpers.h>
46+ #include "qmi8658c.h"
47+
48+ static const char * TAG = "qmi8658c" ;
49+
50+ #define CHECK (x ) \
51+ do \
52+ { \
53+ esp_err_t __; \
54+ if ((__ = x) != ESP_OK) \
55+ return __; \
56+ } \
57+ while (0)
58+ #define CHECK_ARG (VAL ) \
59+ do \
60+ { \
61+ if (!(VAL)) \
62+ return ESP_ERR_INVALID_ARG; \
63+ } \
64+ while (0)
65+
66+ static qmi_ctx_t qmi_ctx ;
67+
68+ inline static esp_err_t send_command_nolock (i2c_dev_t * dev , uint8_t reg , uint8_t value )
69+ {
70+ uint8_t data [2 ] = { reg , value };
71+ return i2c_dev_write (dev , NULL , 0 , data , 2 );
72+ }
73+
74+ static esp_err_t send_command (i2c_dev_t * dev , uint8_t reg , uint8_t value )
75+ {
76+ I2C_DEV_TAKE_MUTEX (dev );
77+ I2C_DEV_CHECK (dev , send_command_nolock (dev , reg , value ));
78+ I2C_DEV_GIVE_MUTEX (dev );
79+
80+ return ESP_OK ;
81+ }
82+
83+ #define REPLY_DELAY_MS 25
84+
85+ static esp_err_t read_register (i2c_dev_t * dev , uint8_t reg , uint8_t * value )
86+ {
87+ I2C_DEV_TAKE_MUTEX (dev );
88+
89+ I2C_DEV_CHECK (dev , i2c_dev_read_reg (dev , reg , value , 1 ));
90+
91+ vTaskDelay (pdMS_TO_TICKS (REPLY_DELAY_MS ));
92+
93+ I2C_DEV_GIVE_MUTEX (dev );
94+
95+ return ESP_OK ;
96+ }
97+
98+ esp_err_t qmi8658c_init_desc (i2c_dev_t * dev , uint8_t addr , i2c_port_t port , gpio_num_t sda_gpio , gpio_num_t scl_gpio )
99+ {
100+ CHECK_ARG (dev );
101+
102+ dev -> port = port ;
103+ dev -> addr = addr ;
104+ dev -> cfg .sda_io_num = sda_gpio ;
105+ dev -> cfg .scl_io_num = scl_gpio ;
106+ #if HELPER_TARGET_IS_ESP32
107+ dev -> cfg .master .clk_speed = QMI8658C_I2C_FREQ_HZ ;
108+ #endif
109+ return i2c_dev_create_mutex (dev );
110+ }
111+
112+ esp_err_t qmi8658c_free_desc (i2c_dev_t * dev )
113+ {
114+ CHECK_ARG (dev );
115+ return i2c_dev_delete_mutex (dev );
116+ /*
117+ uint8_t ctrl7, ctrl1;
118+
119+ // Read QMI8658_CTRL7 register
120+ CHECK(read_register(dev, QMI8658_CTRL7, &ctrl7));
121+
122+ // Disable accelerometer, gyroscope, magnetometer and attitude engine
123+ ctrl7 &= 0xF0;
124+ CHECK(send_command(dev, QMI8658_CTRL7, ctrl7));
125+
126+ // Disable sensor by turning off the internal 2 MHz oscillator
127+ CHECK(read_register(dev, QMI8658_CTRL1, &ctrl1));
128+ ctrl1 |= (1 << 0);
129+ CHECK(send_command(dev, QMI8658_CTRL1, ctrl1));
130+
131+ // Read these two registers again to verify
132+ CHECK(read_register(dev, QMI8658_CTRL7, &ctrl7));
133+ CHECK(read_register(dev, QMI8658_CTRL1, &ctrl1));
134+
135+ // Verify if the sensor is properly disabled
136+ if (!(ctrl7 & 0x0F) && (ctrl1 & 0x01))
137+ {
138+ return i2c_dev_delete_mutex(dev);
139+ }
140+ else
141+ {
142+ return ESP_FAIL;
143+ }
144+ */
145+ }
146+
147+ esp_err_t qmi8658c_reset (i2c_dev_t * dev )
148+ {
149+ CHECK_ARG (dev );
150+
151+ return send_command (dev , QMI8658_RESET , 0xB0 );
152+ }
153+
154+ /* Accelerometer sensitivity table */
155+ uint16_t acc_scale_sensitivity_table [4 ] = {
156+ ACC_SCALE_SENSITIVITY_2G , // Sensitivity for ±2g range.
157+ ACC_SCALE_SENSITIVITY_4G , // Sensitivity for ±4g range.
158+ ACC_SCALE_SENSITIVITY_8G , // Sensitivity for ±8g range.
159+ ACC_SCALE_SENSITIVITY_16G // Sensitivity for ±16g range.
160+ };
161+
162+ /* Gyroscope sensitivity table */
163+ uint16_t gyro_scale_sensitivity_table [8 ] = {
164+ GYRO_SCALE_SENSITIVITY_16DPS , // Sensitivity for ±16 degrees per second range.
165+ GYRO_SCALE_SENSITIVITY_32DPS , // Sensitivity for ±32 degrees per second range.
166+ GYRO_SCALE_SENSITIVITY_64DPS , // Sensitivity for ±64 degrees per second range.
167+ GYRO_SCALE_SENSITIVITY_128DPS , // Sensitivity for ±128 degrees per second range.
168+ GYRO_SCALE_SENSITIVITY_256DPS , // Sensitivity for ±256 degrees per second range.
169+ GYRO_SCALE_SENSITIVITY_512DPS , // Sensitivity for ±512 degrees per second range.
170+ GYRO_SCALE_SENSITIVITY_1024DPS , // Sensitivity for ±1024 degrees per second range.
171+ GYRO_SCALE_SENSITIVITY_2048DPS // Sensitivity for ±2048 degrees per second range.
172+ };
173+
174+ esp_err_t qmi8658c_setup (i2c_dev_t * dev , qmi8658c_config_t * config )
175+ {
176+ CHECK_ARG (dev && config );
177+
178+ // reset device
179+ qmi8658c_reset (dev );
180+
181+ // set mode
182+ uint8_t ctrl7 ;
183+ CHECK (read_register (dev , QMI8658_CTRL7 , & ctrl7 ));
184+ ctrl7 = (ctrl7 & 0xFC ) | config -> mode ;
185+ CHECK (send_command (dev , QMI8658_CTRL7 , ctrl7 ));
186+
187+ // set accelerometr scale and ODR
188+ uint8_t ctrl2 ;
189+ CHECK (read_register (dev , QMI8658_CTRL2 , & ctrl2 ));
190+ ctrl2 = (ctrl2 & 0xF0 ) | config -> acc_odr ;
191+ ctrl2 = (ctrl2 & 0x8F ) | (config -> acc_scale << 4 );
192+ CHECK (send_command (dev , QMI8658_CTRL2 , ctrl2 ));
193+
194+ // set accelerometer scale and sensitivity
195+ qmi_ctx .acc_scale = config -> acc_scale ;
196+ qmi_ctx .acc_sensitivity = acc_scale_sensitivity_table [config -> acc_scale ];
197+
198+ // set gyroscope scale and ODR
199+ uint8_t ctrl3 ;
200+ CHECK (read_register (dev , QMI8658_CTRL3 , & ctrl3 ));
201+ ctrl3 = (ctrl3 & 0xF0 ) | config -> gyro_odr ;
202+ ctrl3 = (ctrl3 & 0x8F ) | (config -> gyro_scale << 4 );
203+ CHECK (send_command (dev , QMI8658_CTRL3 , ctrl3 ));
204+
205+ // set gyroscope scale and sensitivity
206+ qmi_ctx .gyro_scale = config -> gyro_scale ;
207+ qmi_ctx .gyro_sensitivity = gyro_scale_sensitivity_table [config -> gyro_scale ];
208+
209+ // read device ID and revision ID
210+ CHECK (read_register (dev , QMI8658_WHO_AM_I , & qmi_ctx .who_am_i ));
211+ CHECK (read_register (dev , QMI8658_REVISION , & qmi_ctx .revision ));
212+
213+ ESP_LOGW (TAG , "device ID: 0x%02X, revision: 0x%02X" , qmi_ctx .who_am_i , qmi_ctx .revision );
214+
215+ // Verify mode setting
216+ uint8_t qmi8658_ctrl7 ;
217+ CHECK (read_register (dev , QMI8658_CTRL7 , & qmi8658_ctrl7 ));
218+ if ((qmi8658_ctrl7 & 0x03 ) != config -> mode )
219+ {
220+ ESP_LOGE (TAG , "Mode setting verification failed" );
221+ return ESP_FAIL ;
222+ }
223+
224+ return ESP_OK ;
225+ }
226+
227+ esp_err_t qmi8658c_read_data (i2c_dev_t * dev , qmi8658c_data_t * data )
228+ {
229+ CHECK_ARG (dev && data );
230+
231+ // Read accelerometer data
232+ uint8_t acc_x_l , acc_x_h , acc_y_l , acc_y_h , acc_z_l , acc_z_h ;
233+ CHECK (read_register (dev , QMI8658_ACC_X_L , & acc_x_l ));
234+ CHECK (read_register (dev , QMI8658_ACC_X_H , & acc_x_h ));
235+ CHECK (read_register (dev , QMI8658_ACC_Y_L , & acc_y_l ));
236+ CHECK (read_register (dev , QMI8658_ACC_Y_H , & acc_y_h ));
237+ CHECK (read_register (dev , QMI8658_ACC_Z_L , & acc_z_l ));
238+ CHECK (read_register (dev , QMI8658_ACC_Z_H , & acc_z_h ));
239+
240+ // ESP_LOGE(TAG, "acc_x_l: %d, acc_x_h: %d, acc_y_l: %d, acc_y_h: %d, acc_z_l: %d, acc_z_h: %d", acc_x_l, acc_x_h, acc_y_l, acc_y_h, acc_z_l, acc_z_h);
241+
242+ int16_t acc_x = (int16_t )((acc_x_h << 8 ) | acc_x_l );
243+ int16_t acc_y = (int16_t )((acc_y_h << 8 ) | acc_y_l );
244+ int16_t acc_z = (int16_t )((acc_z_h << 8 ) | acc_z_l );
245+
246+ // ESP_LOGW(TAG, "acc_x: %d, acc_y: %d, acc_z: %d", acc_x, acc_y, acc_z);
247+
248+ data -> acc .x = (float )acc_x / qmi_ctx .acc_sensitivity ;
249+ data -> acc .y = (float )acc_y / qmi_ctx .acc_sensitivity ;
250+ data -> acc .z = (float )acc_z / qmi_ctx .acc_sensitivity ;
251+
252+ // Read gyroscope data
253+ uint8_t gyr_x_l , gyr_x_h , gyr_y_l , gyr_y_h , gyr_z_l , gyr_z_h ;
254+ CHECK (read_register (dev , QMI8658_GYR_X_L , & gyr_x_l ));
255+ CHECK (read_register (dev , QMI8658_GYR_X_H , & gyr_x_h ));
256+ CHECK (read_register (dev , QMI8658_GYR_Y_L , & gyr_y_l ));
257+ CHECK (read_register (dev , QMI8658_GYR_Y_H , & gyr_y_h ));
258+ CHECK (read_register (dev , QMI8658_GYR_Z_L , & gyr_z_l ));
259+ CHECK (read_register (dev , QMI8658_GYR_Z_H , & gyr_z_h ));
260+
261+ // ESP_LOGE(TAG, "gyr_x_l: %d, gyr_x_h: %d, gyr_y_l: %d, gyr_y_h: %d, gyr_z_l: %d, gyr_z_h: %d", gyr_x_l, gyr_x_h, gyr_y_l, gyr_y_h, gyr_z_l, gyr_z_h);
262+
263+ int16_t gyr_x = (int16_t )((gyr_x_h << 8 ) | gyr_x_l );
264+ int16_t gyr_y = (int16_t )((gyr_y_h << 8 ) | gyr_y_l );
265+ int16_t gyr_z = (int16_t )((gyr_z_h << 8 ) | gyr_z_l );
266+
267+ // ESP_LOGW(TAG, "gyr_x: %d, gyr_y: %d, gyr_z: %d", gyr_x, gyr_y, gyr_z);
268+
269+ data -> gyro .x = (float )gyr_x / qmi_ctx .gyro_sensitivity ;
270+ data -> gyro .y = (float )gyr_y / qmi_ctx .gyro_sensitivity ;
271+ data -> gyro .z = (float )gyr_z / qmi_ctx .gyro_sensitivity ;
272+
273+ // Read temperature data
274+ uint8_t temp_l , temp_h ;
275+ CHECK (read_register (dev , QMI8658_TEMP_L , & temp_l ));
276+ CHECK (read_register (dev , QMI8658_TEMP_H , & temp_h ));
277+
278+ // ESP_LOGE(TAG, "temp_l: %d, temp_h: %d", temp_l, temp_h);
279+
280+ int16_t temp = (int16_t )((temp_h << 8 ) | temp_l );
281+ // ESP_LOGW(TAG, "temp: %d", temp);
282+
283+ data -> temperature = (float )temp / TEMPERATURE_SENSOR_RESOLUTION ;
284+
285+ // ESP_LOGW(TAG, "Acc: x=%f, y=%f, z=%f; Gyro: x=%f, y=%f, z=%f; Temp: %f",
286+ // data->acc.x, data->acc.y, data->acc.z, data->gyro.x, data->gyro.y, data->gyro.z, data->temperature);
287+
288+ return ESP_OK ;
289+ }
0 commit comments