|
|
@@ -1,13 +1,11 @@
|
|
|
#include "sensors.h"
|
|
|
|
|
|
#include <math.h>
|
|
|
-#include <string.h>
|
|
|
|
|
|
#include "config.h"
|
|
|
-#include "driver/i2c.h"
|
|
|
+#include "driver/i2c_master.h"
|
|
|
+#include "esp_check.h"
|
|
|
#include "esp_log.h"
|
|
|
-#include "freertos/FreeRTOS.h"
|
|
|
-#include "freertos/task.h"
|
|
|
|
|
|
static const char *TAG = "guguji_sensors";
|
|
|
|
|
|
@@ -27,16 +25,19 @@ static const char *TAG = "guguji_sensors";
|
|
|
|
|
|
static bool g_qmi_ok = false;
|
|
|
static bool g_qmc_ok = false;
|
|
|
+static i2c_master_bus_handle_t g_i2c_bus = NULL;
|
|
|
+static i2c_master_dev_handle_t g_qmi_device = NULL;
|
|
|
+static i2c_master_dev_handle_t g_qmc_device = NULL;
|
|
|
|
|
|
-static esp_err_t i2c_write_u8(uint8_t addr, uint8_t reg, uint8_t value)
|
|
|
+static esp_err_t i2c_write_u8(i2c_master_dev_handle_t device, uint8_t reg, uint8_t value)
|
|
|
{
|
|
|
uint8_t payload[2] = {reg, value};
|
|
|
- return i2c_master_write_to_device(GUGUJI_I2C_PORT, addr, payload, sizeof(payload), pdMS_TO_TICKS(50));
|
|
|
+ return i2c_master_transmit(device, payload, sizeof(payload), 50);
|
|
|
}
|
|
|
|
|
|
-static esp_err_t i2c_read(uint8_t addr, uint8_t reg, uint8_t *data, size_t length)
|
|
|
+static esp_err_t i2c_read(i2c_master_dev_handle_t device, uint8_t reg, uint8_t *data, size_t length)
|
|
|
{
|
|
|
- return i2c_master_write_read_device(GUGUJI_I2C_PORT, addr, ®, 1, data, length, pdMS_TO_TICKS(50));
|
|
|
+ return i2c_master_transmit_receive(device, ®, 1, data, length, 50);
|
|
|
}
|
|
|
|
|
|
static int16_t le_i16(const uint8_t *data)
|
|
|
@@ -46,29 +47,47 @@ static int16_t le_i16(const uint8_t *data)
|
|
|
|
|
|
esp_err_t guguji_sensors_init(void)
|
|
|
{
|
|
|
- i2c_config_t config = {
|
|
|
- .mode = I2C_MODE_MASTER,
|
|
|
+ i2c_master_bus_config_t bus_config = {
|
|
|
+ .i2c_port = GUGUJI_I2C_PORT,
|
|
|
.sda_io_num = GUGUJI_I2C_SDA_GPIO,
|
|
|
.scl_io_num = GUGUJI_I2C_SCL_GPIO,
|
|
|
- .sda_pullup_en = GPIO_PULLUP_ENABLE,
|
|
|
- .scl_pullup_en = GPIO_PULLUP_ENABLE,
|
|
|
- .master.clk_speed = GUGUJI_I2C_FREQ_HZ,
|
|
|
- .clk_flags = 0,
|
|
|
+ .clk_source = I2C_CLK_SRC_DEFAULT,
|
|
|
+ .glitch_ignore_cnt = 7,
|
|
|
+ .flags.enable_internal_pullup = true,
|
|
|
};
|
|
|
- ESP_ERROR_CHECK(i2c_param_config(GUGUJI_I2C_PORT, &config));
|
|
|
- ESP_ERROR_CHECK(i2c_driver_install(GUGUJI_I2C_PORT, config.mode, 0, 0, 0));
|
|
|
+ ESP_RETURN_ON_ERROR(i2c_new_master_bus(&bus_config, &g_i2c_bus), TAG, "创建 I2C master bus 失败");
|
|
|
+
|
|
|
+ i2c_device_config_t qmi_config = {
|
|
|
+ .dev_addr_length = I2C_ADDR_BIT_LEN_7,
|
|
|
+ .device_address = GUGUJI_QMI8658A_ADDR,
|
|
|
+ .scl_speed_hz = GUGUJI_I2C_FREQ_HZ,
|
|
|
+ };
|
|
|
+ ESP_RETURN_ON_ERROR(
|
|
|
+ i2c_master_bus_add_device(g_i2c_bus, &qmi_config, &g_qmi_device),
|
|
|
+ TAG,
|
|
|
+ "添加 QMI8658A I2C 设备失败");
|
|
|
+
|
|
|
+ i2c_device_config_t qmc_config = {
|
|
|
+ .dev_addr_length = I2C_ADDR_BIT_LEN_7,
|
|
|
+ .device_address = GUGUJI_QMC6309_ADDR,
|
|
|
+ .scl_speed_hz = GUGUJI_I2C_FREQ_HZ,
|
|
|
+ };
|
|
|
+ ESP_RETURN_ON_ERROR(
|
|
|
+ i2c_master_bus_add_device(g_i2c_bus, &qmc_config, &g_qmc_device),
|
|
|
+ TAG,
|
|
|
+ "添加 QMC6309 I2C 设备失败");
|
|
|
|
|
|
uint8_t who_am_i = 0;
|
|
|
- if (i2c_read(GUGUJI_QMI8658A_ADDR, QMI_REG_WHO_AM_I, &who_am_i, 1) == ESP_OK && who_am_i == 0x05) {
|
|
|
+ if (i2c_read(g_qmi_device, QMI_REG_WHO_AM_I, &who_am_i, 1) == ESP_OK && who_am_i == 0x05) {
|
|
|
// CTRL1: 开启地址自增并使用 little-endian,方便一次读完 AX~GZ。
|
|
|
- i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL1, 0x40);
|
|
|
+ i2c_write_u8(g_qmi_device, QMI_REG_CTRL1, 0x40);
|
|
|
// CTRL2: 加速度 ±8g,ODR 约 112Hz;CTRL3: 陀螺仪 ±512dps,ODR 约 112Hz。
|
|
|
- i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL2, 0x26);
|
|
|
- i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL3, 0x56);
|
|
|
+ i2c_write_u8(g_qmi_device, QMI_REG_CTRL2, 0x26);
|
|
|
+ i2c_write_u8(g_qmi_device, QMI_REG_CTRL3, 0x56);
|
|
|
// 打开加速度和陀螺仪低通滤波。
|
|
|
- i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL5, 0x11);
|
|
|
+ i2c_write_u8(g_qmi_device, QMI_REG_CTRL5, 0x11);
|
|
|
// 使能 accel + gyro。
|
|
|
- i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL7, 0x03);
|
|
|
+ i2c_write_u8(g_qmi_device, QMI_REG_CTRL7, 0x03);
|
|
|
g_qmi_ok = true;
|
|
|
ESP_LOGI(TAG, "QMI8658A 初始化成功");
|
|
|
} else {
|
|
|
@@ -76,10 +95,10 @@ esp_err_t guguji_sensors_init(void)
|
|
|
}
|
|
|
|
|
|
uint8_t chip_id = 0;
|
|
|
- if (i2c_read(GUGUJI_QMC6309_ADDR, QMC_REG_CHIP_ID, &chip_id, 1) == ESP_OK && chip_id == 0x90) {
|
|
|
+ if (i2c_read(g_qmc_device, QMC_REG_CHIP_ID, &chip_id, 1) == ESP_OK && chip_id == 0x90) {
|
|
|
// 数据手册推荐的连续测量配置:32Gauss 量程、Set/Reset 开、OSR1/OSR2=8。
|
|
|
- i2c_write_u8(GUGUJI_QMC6309_ADDR, QMC_REG_CTRL2, 0x00);
|
|
|
- i2c_write_u8(GUGUJI_QMC6309_ADDR, QMC_REG_CTRL1, 0x63);
|
|
|
+ i2c_write_u8(g_qmc_device, QMC_REG_CTRL2, 0x00);
|
|
|
+ i2c_write_u8(g_qmc_device, QMC_REG_CTRL1, 0x63);
|
|
|
g_qmc_ok = true;
|
|
|
ESP_LOGI(TAG, "QMC6309 初始化成功");
|
|
|
} else {
|
|
|
@@ -93,7 +112,7 @@ static void update_imu(guguji_sensor_state_t *state)
|
|
|
{
|
|
|
uint8_t raw[12] = {0};
|
|
|
state->imu_ok = false;
|
|
|
- if (!g_qmi_ok || i2c_read(GUGUJI_QMI8658A_ADDR, QMI_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
|
|
|
+ if (!g_qmi_ok || i2c_read(g_qmi_device, QMI_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
@@ -120,7 +139,7 @@ static void update_mag(guguji_sensor_state_t *state)
|
|
|
{
|
|
|
uint8_t status = 0;
|
|
|
state->mag_ok = false;
|
|
|
- if (!g_qmc_ok || i2c_read(GUGUJI_QMC6309_ADDR, QMC_REG_STATUS, &status, 1) != ESP_OK) {
|
|
|
+ if (!g_qmc_ok || i2c_read(g_qmc_device, QMC_REG_STATUS, &status, 1) != ESP_OK) {
|
|
|
return;
|
|
|
}
|
|
|
if ((status & 0x01u) == 0) {
|
|
|
@@ -128,7 +147,7 @@ static void update_mag(guguji_sensor_state_t *state)
|
|
|
}
|
|
|
|
|
|
uint8_t raw[6] = {0};
|
|
|
- if (i2c_read(GUGUJI_QMC6309_ADDR, QMC_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
|
|
|
+ if (i2c_read(g_qmc_device, QMC_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
|
|
|
return;
|
|
|
}
|
|
|
// QMC6309 典型分辨率约 1mGs/LSB,即 0.1uT/LSB。
|