esp-who/components/modules/imu/qma7981.c

199 lines
4.3 KiB
C
Raw Permalink Normal View History

2021-11-26 20:01:03 +08:00
/**
* @file qma7981.c
* @brief
* @version 0.1
* @date 2021-09-01
*
* @copyright Copyright (c) 2021
*
*/
// #include "bsp_i2c.h"
#include "esp_err.h"
#include "esp_log.h"
#include "i2c_bus.h"
#include "qma7981.h"
#define QMA7981_REG_CHIP_ID 0x00
#define QMA7981_REG_DX_L 0x01
#define QMA7981_REG_DX_H 0x02
#define QMA7981_REG_DY_L 0x03
#define QMA7981_REG_DY_H 0x04
#define QMA7981_REG_DZ_L 0x05
#define QMA7981_REG_DZ_H 0x06
#define QMA7981_REG_STEP_L 0x07
#define QMA7981_REG_STEP_H 0x08
#define QMA7981_REG_INT_STAT_0 0x0A
#define QMA7981_REG_INT_STAT_1 0x0B
#define QMA7981_REG_INT_STAT_4 0x0D
#define QMA7981_REG_RANGE 0x0F
#define QMA7981_REG_BAND_WIDTH 0x10
#define QMA7981_REG_PWR_MANAGE 0x11
#define QMA7981_REG_STEP_CONF_0 0x12
#define QMA7981_REG_STEP_CONF_1 0x13
#define QMA7981_REG_STEP_CONF_2 0x14
#define QMA7981_REG_STEP_CONF_3 0x15
#define QMA7981_REG_INT_EN_0 0x16
#define QMA7981_REG_INT_EN_1 0x17
#define QMA7981_REG_INT_MAP_0 0x19
#define QMA7981_REG_INT_MAP_1 0x1A
#define QMA7981_REG_INT_MAP_2 0x1B
#define QMA7981_REG_INT_MAP_3 0x1C
#define QMA7981_REG_SIG_STEP_TH 0x1D
#define QMA7981_REG_STEP 0x1F
static const char *TAG = "qma7981";
static qma_range_t qma_range = QMA_RANGE_2G;
static i2c_bus_device_handle_t qma7981_handle;
static i2c_bus_handle_t i2c_bus_handle;
/**
* @brief
*
* @param reg_addr
* @param data
* @return esp_err_t
*/
static esp_err_t qma7981_read_byte(uint8_t reg_addr, uint8_t *data)
{
return i2c_bus_read_byte(qma7981_handle, reg_addr, data);
}
static esp_err_t qma7981_write_byte(uint8_t reg_addr, uint8_t data)
{
return i2c_bus_write_byte(qma7981_handle, reg_addr, data);
}
static esp_err_t qma7981_read_bytes(uint8_t reg_addr, size_t data_len, uint8_t *data)
{
return i2c_bus_read_bytes(qma7981_handle, reg_addr, data_len, data);
}
esp_err_t qma7981_init(void)
{
if (NULL != qma7981_handle)
{
return ESP_FAIL;
}
esp_err_t ret_val = ESP_OK;
uint32_t clk_speed = 400000;
i2c_config_t conf = {
.mode = I2C_MODE_MASTER,
.scl_io_num = 5,
.sda_io_num = 4,
.scl_pullup_en = GPIO_PULLUP_ENABLE,
.sda_pullup_en = GPIO_PULLUP_ENABLE,
.master.clk_speed = clk_speed,
};
i2c_bus_handle = i2c_bus_create(0, &conf);
assert(i2c_bus_handle != NULL);
qma7981_handle = i2c_bus_device_create(i2c_bus_handle, 0x12, clk_speed);
assert(qma7981_handle != NULL);
uint8_t id = 0;
ret_val |= qma7981_read_byte(0x00, &id);
ESP_LOGW(TAG, "ID : %02X", id);
/* ******************************** ******************************** */
ret_val |= qma7981_write_byte(QMA7981_REG_PWR_MANAGE, 0xC0); /* Exit sleep mode*/
vTaskDelay(pdMS_TO_TICKS(20));
ret_val |= qma7981_write_byte(QMA7981_REG_RANGE, QMA_RANGE_2G); /* Set range */
ret_val |= qma7981_write_byte(QMA7981_REG_BAND_WIDTH, QMA_BANDWIDTH_1024_HZ); /* Set bandwidth */
/* ******************************** ******************************** */
return ret_val;
}
esp_err_t qma7981_get_step(uint16_t *data)
{
esp_err_t ret_val = ESP_OK;
uint8_t step_h = 0, step_l = 0;
if (NULL == data)
{
return ESP_ERR_INVALID_ARG;
}
ret_val |= qma7981_read_byte(0x07, &step_l);
ret_val |= qma7981_read_byte(0x08, &step_h);
*data = (step_h << 8) + step_l;
return ret_val;
}
/**
* @brief
*
* @param range
* @return esp_err_t
*/
esp_err_t qma7981_set_range(qma_range_t range)
{
esp_err_t ret_val = qma7981_write_byte(QMA7981_REG_RANGE, range);
qma_range = range;
return ret_val;
}
/**
* @brief
*
* @param x
* @param y
* @param z
* @return esp_err_t
*/
esp_err_t qma7981_get_acce(float *x, float *y, float *z)
{
float multiple = 2;
esp_err_t ret_val = ESP_OK;
struct qma_acce_data_t
{
int16_t x;
int16_t y;
int16_t z;
} data;
switch (qma_range)
{
case QMA_RANGE_2G:
multiple = 2;
break;
case QMA_RANGE_4G:
multiple = 4;
break;
case QMA_RANGE_8G:
multiple = 8;
break;
case QMA_RANGE_16G:
multiple = 16;
break;
case QMA_RANGE_32G:
multiple = 32;
break;
default:
multiple = 2;
break;
}
ret_val |= qma7981_read_bytes(QMA7981_REG_DX_L, 6, &data);
/* QMA7981's range is 14 bit. Adjust data format */
data.x >>= 2;
data.y >>= 2;
data.z >>= 2;
/* Convert to acceleration of gravity */
*x = data.x / (float)(1 << 13) * multiple;
*y = data.y / (float)(1 << 13) * multiple;
*z = data.z / (float)(1 << 13) * multiple;
return ret_val;
}