✨ 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