✨ add imu
yuanjiong
parent
0a8c19b129
commit
f1a7252c62
|
|
@ -0,0 +1,198 @@
|
||||||
|
/**
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,61 @@
|
||||||
|
/**
|
||||||
|
* @file qma7981.h
|
||||||
|
* @brief
|
||||||
|
* @version 0.1
|
||||||
|
* @date 2021-09-01
|
||||||
|
*
|
||||||
|
* @copyright Copyright (c) 2021
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "esp_err.h"
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
QMA_RANGE_2G = 0b0001,
|
||||||
|
QMA_RANGE_4G = 0b0010,
|
||||||
|
QMA_RANGE_8G = 0b0100,
|
||||||
|
QMA_RANGE_16G = 0b1000,
|
||||||
|
QMA_RANGE_32G = 0b1111,
|
||||||
|
} qma_range_t; /* Others will be 2G */
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
QMA_BANDWIDTH_128_HZ = 0b111,
|
||||||
|
QMA_BANDWIDTH_256_HZ = 0b110,
|
||||||
|
QMA_BANDWIDTH_1024_HZ = 0b101,
|
||||||
|
} qma_bandwidth_t;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief
|
||||||
|
*
|
||||||
|
* @return esp_err_t
|
||||||
|
*/
|
||||||
|
esp_err_t qma7981_init(void);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief
|
||||||
|
*
|
||||||
|
* @param range
|
||||||
|
* @return esp_err_t
|
||||||
|
*/
|
||||||
|
esp_err_t qma7981_set_range(qma_range_t range);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief
|
||||||
|
*
|
||||||
|
* @param x
|
||||||
|
* @param y
|
||||||
|
* @param z
|
||||||
|
* @return esp_err_t
|
||||||
|
*/
|
||||||
|
esp_err_t qma7981_get_acce(float *x, float *y, float *z);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
Loading…
Reference in New Issue