106 lines
4.0 KiB
C
106 lines
4.0 KiB
C
#include "bsp_qmi8658.h"
|
|
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
#include "esp_log.h"
|
|
|
|
#include "bsp_i2c.h"
|
|
|
|
static const char *TAG = "bsp_qmi8658";
|
|
#define ACC_SENSITIVITY (4 / 32768.0f) // 每LSB对应的加速度(单位为g)
|
|
|
|
static i2c_master_dev_handle_t dev_handle;
|
|
|
|
|
|
// 读取QMI8658寄存器的值
|
|
static esp_err_t bsp_qmi8658_reg_read(uint8_t reg_addr, uint8_t *data, size_t len)
|
|
{
|
|
//return bsp_i2c_reg8_read(QMI8658_SENSOR_ADDR, reg_addr, data, len);
|
|
return i2c_master_transmit_receive(dev_handle, ®_addr, 1, data, len, pdMS_TO_TICKS(1000));
|
|
}
|
|
|
|
// 给QMI8658的寄存器写值
|
|
static esp_err_t bsp_qmi8658_reg_write_byte(uint8_t reg_addr, uint8_t *data, size_t len)
|
|
{
|
|
uint8_t buf[len + 1];
|
|
buf[0] = reg_addr;
|
|
memcpy(buf + 1, data, len);
|
|
//return bsp_i2c_reg8_write(QMI8658_SENSOR_ADDR, reg_addr, data, len);
|
|
return i2c_master_transmit(dev_handle, buf, len + 1, pdMS_TO_TICKS(1000));
|
|
}
|
|
|
|
bool bsp_qmi8658_read_data(qmi8658_data_t *data)
|
|
{
|
|
uint8_t status;
|
|
float mask;
|
|
uint16_t buf[6];
|
|
bsp_qmi8658_reg_read(QMI8658_STATUS0, &status, 1); // 读状态寄存器
|
|
if (status & 0x03)
|
|
{
|
|
bsp_qmi8658_reg_read(QMI8658_AX_L, (uint8_t *)buf, 12); // 读加速度和陀螺仪值
|
|
data->acc_x = buf[0];
|
|
data->acc_y = buf[1];
|
|
data->acc_z = buf[2];
|
|
data->gyr_x = buf[3];
|
|
data->gyr_y = buf[4];
|
|
data->gyr_z = buf[5];
|
|
// ESP_LOGI(TAG, "QMI8658 read data success!");
|
|
|
|
mask = (float)data->acc_x / sqrt(((float)data->acc_y * (float)data->acc_y + (float)data->acc_z * (float)data->acc_z));
|
|
data->AngleX = atan(mask) * 57.29578f; // 180/π=57.29578
|
|
mask = (float)data->acc_y / sqrt(((float)data->acc_x * (float)data->acc_x + (float)data->acc_z * (float)data->acc_z));
|
|
data->AngleY = atan(mask) * 57.29578f; // 180/π=57.29578
|
|
mask = sqrt(((float)data->acc_x * (float)data->acc_x + (float)data->acc_y * (float)data->acc_y)) / (float)data->acc_z;
|
|
data->AngleZ = atan(mask) * 57.29578f; // 180/π=57.29578
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void bsp_qmi8658_init(i2c_master_bus_handle_t bus_handle)
|
|
{
|
|
uint8_t id = 0;
|
|
ESP_LOGI(TAG, "QMI8658 Initialize");
|
|
|
|
i2c_device_config_t dev_cfg = {
|
|
.dev_addr_length = I2C_ADDR_BIT_LEN_7,
|
|
.device_address = QMI8658_SENSOR_ADDR,
|
|
.scl_speed_hz = 400000,
|
|
};
|
|
ESP_ERROR_CHECK(i2c_master_bus_add_device(bus_handle, &dev_cfg, &dev_handle));
|
|
|
|
ESP_ERROR_CHECK(bsp_qmi8658_reg_read(QMI8658_WHO_AM_I, &id, 1));
|
|
|
|
if (0x05 != id)
|
|
{
|
|
ESP_LOGI(TAG, "QMI8658 not found");
|
|
}
|
|
ESP_LOGI(TAG, "Find QMI8658");
|
|
bsp_qmi8658_reg_write_byte(QMI8658_RESET, (uint8_t[]){0xb0}, 1); // 复位
|
|
vTaskDelay(pdMS_TO_TICKS(10)); // 延时10ms
|
|
bsp_qmi8658_reg_write_byte(QMI8658_CTRL1, (uint8_t[]){0x40}, 1); // CTRL1 设置地址自动增加
|
|
bsp_qmi8658_reg_write_byte(QMI8658_CTRL7, (uint8_t[]){0x03}, 1); // CTRL7 允许加速度和陀螺仪
|
|
bsp_qmi8658_reg_write_byte(QMI8658_CTRL2, (uint8_t[]){0x95}, 1); // CTRL2 设置ACC 4g 250Hz
|
|
bsp_qmi8658_reg_write_byte(QMI8658_CTRL3, (uint8_t[]){0xd5}, 1); // CTRL3 设置GRY 512dps 250Hz
|
|
}
|
|
|
|
static void qmi8658_test_task(void *arg)
|
|
{
|
|
qmi8658_data_t data;
|
|
while (1)
|
|
{
|
|
bsp_qmi8658_read_data(&data);
|
|
// printf("Acc: %.2f %.2f %.2f-----Gyr: %04d %04d %04d\n", data.acc_x * ACC_SENSITIVITY * 9.8f, data.acc_y * ACC_SENSITIVITY * 9.8f, data.acc_z * ACC_SENSITIVITY * 9.8f, data.gyr_x, data.gyr_y, data.gyr_z);
|
|
// printf("-------------------------------------------------------------------------\n");
|
|
// printf("Angle: %.2f %.2f %.2f\n", data.AngleX, data.AngleY, data.AngleZ);
|
|
vTaskDelay(pdMS_TO_TICKS(50));
|
|
}
|
|
}
|
|
|
|
void bsp_qmi8658_test(void)
|
|
{
|
|
xTaskCreate(qmi8658_test_task, "qmi8658_test", 4096, NULL, 0, NULL);
|
|
} |