最近写的一份驱动,核心逻辑60行,其中大部分还是废话。
#ifndef __LSM6DS3_H_INCLUDED__
#define __LSM6DS3_H_INCLUDED__
#include "i2c_bus.h"
struct lsm6ds3_info_t
{
struct i2c_bus_t *i2c_bus;
uint8_t i2c_addr;
// private
uint8_t error;
struct vsfsm_t xfer_sm;
struct vsfsm_pt_t xfer_pt;
};
void lsm6ds3_init(struct lsm6ds3_info_t *info);
#endif // __LSM6DS3_H_INCLUDED__ 复制代码
#include "vsf.h"
#include "usrapp.h"
#define LSM6DS3_ACC_GYRO_FUNC_CFG_ACCESS 0X01
#define LSM6DS3_ACC_GYRO_SENSOR_SYNC_TIME 0X04
#define LSM6DS3_ACC_GYRO_FIFO_CTRL1 0X06
#define LSM6DS3_ACC_GYRO_FIFO_CTRL2 0X07
#define LSM6DS3_ACC_GYRO_FIFO_CTRL3 0X08
#define LSM6DS3_ACC_GYRO_FIFO_CTRL4 0X09
#define LSM6DS3_ACC_GYRO_FIFO_CTRL5 0X0A
#define LSM6DS3_ACC_GYRO_ORIENT_CFG_G 0X0B
#define LSM6DS3_ACC_GYRO_INT1_CTRL 0X0D
#define LSM6DS3_ACC_GYRO_INT2_CTRL 0X0E
#define LSM6DS3_ACC_GYRO_WHO_AM_I_REG 0X0F
#define LSM6DS3_ACC_GYRO_CTRL1_XL 0X10
#define LSM6DS3_ACC_GYRO_CTRL2_G 0X11
#define LSM6DS3_ACC_GYRO_CTRL3_C 0X12
#define LSM6DS3_ACC_GYRO_CTRL4_C 0X13
#define LSM6DS3_ACC_GYRO_CTRL5_C 0X14
#define LSM6DS3_ACC_GYRO_CTRL6_G 0X15
#define LSM6DS3_ACC_GYRO_CTRL7_G 0X16
#define LSM6DS3_ACC_GYRO_CTRL8_XL 0X17
#define LSM6DS3_ACC_GYRO_CTRL9_XL 0X18
#define LSM6DS3_ACC_GYRO_CTRL10_C 0X19
#define LSM6DS3_ACC_GYRO_MASTER_CONFIG 0X1A
#define LSM6DS3_ACC_GYRO_WAKE_UP_SRC 0X1B
#define LSM6DS3_ACC_GYRO_TAP_SRC 0X1C
#define LSM6DS3_ACC_GYRO_D6D_SRC 0X1D
#define LSM6DS3_ACC_GYRO_STATUS_REG 0X1E
#define LSM6DS3_ACC_GYRO_OUT_TEMP_L 0X20
#define LSM6DS3_ACC_GYRO_OUT_TEMP_H 0X21
#define LSM6DS3_ACC_GYRO_OUTX_L_G 0X22
#define LSM6DS3_ACC_GYRO_OUTX_H_G 0X23
#define LSM6DS3_ACC_GYRO_OUTY_L_G 0X24
#define LSM6DS3_ACC_GYRO_OUTY_H_G 0X25
#define LSM6DS3_ACC_GYRO_OUTZ_L_G 0X26
#define LSM6DS3_ACC_GYRO_OUTZ_H_G 0X27
#define LSM6DS3_ACC_GYRO_OUTX_L_XL 0X28
#define LSM6DS3_ACC_GYRO_OUTX_H_XL 0X29
#define LSM6DS3_ACC_GYRO_OUTY_L_XL 0X2A
#define LSM6DS3_ACC_GYRO_OUTY_H_XL 0X2B
#define LSM6DS3_ACC_GYRO_OUTZ_L_XL 0X2C
#define LSM6DS3_ACC_GYRO_OUTZ_H_XL 0X2D
#define LSM6DS3_ACC_GYRO_SENSORHUB1_REG 0X2E
#define LSM6DS3_ACC_GYRO_SENSORHUB2_REG 0X2F
#define LSM6DS3_ACC_GYRO_SENSORHUB3_REG 0X30
#define LSM6DS3_ACC_GYRO_SENSORHUB4_REG 0X31
#define LSM6DS3_ACC_GYRO_SENSORHUB5_REG 0X32
#define LSM6DS3_ACC_GYRO_SENSORHUB6_REG 0X33
#define LSM6DS3_ACC_GYRO_SENSORHUB7_REG 0X34
#define LSM6DS3_ACC_GYRO_SENSORHUB8_REG 0X35
#define LSM6DS3_ACC_GYRO_SENSORHUB9_REG 0X36
#define LSM6DS3_ACC_GYRO_SENSORHUB10_REG 0X37
#define LSM6DS3_ACC_GYRO_SENSORHUB11_REG 0X38
#define LSM6DS3_ACC_GYRO_SENSORHUB12_REG 0X39
#define LSM6DS3_ACC_GYRO_FIFO_STATUS1 0X3A
#define LSM6DS3_ACC_GYRO_FIFO_STATUS2 0X3B
#define LSM6DS3_ACC_GYRO_FIFO_STATUS3 0X3C
#define LSM6DS3_ACC_GYRO_FIFO_STATUS4 0X3D
#define LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_L 0X3E
#define LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_H 0X3F
#define LSM6DS3_ACC_GYRO_TIMESTAMP0_REG 0X40
#define LSM6DS3_ACC_GYRO_TIMESTAMP1_REG 0X41
#define LSM6DS3_ACC_GYRO_TIMESTAMP2_REG 0X42
#define LSM6DS3_ACC_GYRO_TIMESTAMP_L 0X49
#define LSM6DS3_ACC_GYRO_TIMESTAMP_H 0X4A
#define LSM6DS3_ACC_GYRO_STEP_COUNTER_L 0X4B
#define LSM6DS3_ACC_GYRO_STEP_COUNTER_H 0X4C
#define LSM6DS3_ACC_GYRO_SENSORHUB13_REG 0X4D
#define LSM6DS3_ACC_GYRO_SENSORHUB14_REG 0X4E
#define LSM6DS3_ACC_GYRO_SENSORHUB15_REG 0X4F
#define LSM6DS3_ACC_GYRO_SENSORHUB16_REG 0X50
#define LSM6DS3_ACC_GYRO_SENSORHUB17_REG 0X51
#define LSM6DS3_ACC_GYRO_SENSORHUB18_REG 0X52
#define LSM6DS3_ACC_GYRO_FUNC_SRC 0X53
#define LSM6DS3_ACC_GYRO_TAP_CFG1 0X58
#define LSM6DS3_ACC_GYRO_TAP_THS_6D 0X59
#define LSM6DS3_ACC_GYRO_INT_DUR2 0X5A
#define LSM6DS3_ACC_GYRO_WAKE_UP_THS 0X5B
#define LSM6DS3_ACC_GYRO_WAKE_UP_DUR 0X5C
#define LSM6DS3_ACC_GYRO_FREE_FALL 0X5D
#define LSM6DS3_ACC_GYRO_MD1_CFG 0X5E
#define LSM6DS3_ACC_GYRO_MD2_CFG 0X5F
#define VSFSM_EVT_LSM6DS3_XFER_DONE (VSFSM_EVT_USER_LOCAL + 1)
#define VSFSM_EVT_LSM6DS3_POLL (VSFSM_EVT_USER_LOCAL + 2)
static void lsm6ds3_callback(void *param, int32_t err)
{
struct lsm6ds3_info_t *info = param;
if (err == 0)
info->error = 0;
else
info->error++;
vsfsm_post_evt_pending(&info->xfer_sm, VSFSM_EVT_LSM6DS3_XFER_DONE);
}
static vsf_err_t lsm6ds3_thread(struct vsfsm_pt_t *pt, vsfsm_evt_t evt)
{
struct lsm6ds3_info_t *info = pt->user_data;
struct i2c_bus_t *i2c_bus = info->i2c_bus;
vsfsm_pt_begin(pt);
vsfsm_pt_delay(pt, 100);
// IC probe
vsfhal_i2c_config_cb(i2c_bus->index, info, lsm6ds3_callback);
i2c_bus->msg[0].flag = VSFHAL_I2C_WRITE;
i2c_bus->msg[0].buf = i2c_bus->temp;
i2c_bus->msg[0].len = 2;
i2c_bus->temp[0] = LSM6DS3_ACC_GYRO_FUNC_CFG_ACCESS;
i2c_bus->temp[1] = 0x0;
do
{
vsfhal_i2c_xfer(i2c_bus->index, info->i2c_addr, i2c_bus->msg, 1);
vsfsm_pt_wfe(pt, VSFSM_EVT_LSM6DS3_XFER_DONE);
} while (info->error && (info->error < 3));
if (info->error)
return VSFERR_FAIL;
// Enable BDU and IF_INC
i2c_bus->temp[0] = LSM6DS3_ACC_GYRO_CTRL3_C;
i2c_bus->temp[1] = 0x44;
vsfhal_i2c_xfer(i2c_bus->index, info->i2c_addr, i2c_bus->msg, 1);
vsfsm_pt_wfe(pt, VSFSM_EVT_LSM6DS3_XFER_DONE);
// Disable FIFO
i2c_bus->temp[0] = LSM6DS3_ACC_GYRO_FIFO_CTRL5;
i2c_bus->temp[1] = 0x0;
vsfhal_i2c_xfer(i2c_bus->index, info->i2c_addr, i2c_bus->msg, 1);
vsfsm_pt_wfe(pt, VSFSM_EVT_LSM6DS3_XFER_DONE);
// Set rate: 416Hz; FS_XL: +- 8G;
i2c_bus->temp[0] = LSM6DS3_ACC_GYRO_CTRL1_XL;
i2c_bus->temp[1] = (0x3 << 2) | (0x6 << 4);
vsfhal_i2c_xfer(i2c_bus->index, info->i2c_addr, i2c_bus->msg, 1);
vsfsm_pt_wfe(pt, VSFSM_EVT_LSM6DS3_XFER_DONE);
// Set rate: 416Hz; FS_G: 2000;
i2c_bus->temp[0] = LSM6DS3_ACC_GYRO_CTRL2_G;
i2c_bus->temp[1] = (0x3 << 2) | (0x6 << 4);
vsfhal_i2c_xfer(i2c_bus->index, info->i2c_addr, i2c_bus->msg, 1);
vsfsm_pt_wfe(pt, VSFSM_EVT_LSM6DS3_XFER_DONE);
// Read data
i2c_bus->msg[0].len = 1;
i2c_bus->temp[0] = LSM6DS3_ACC_GYRO_OUTX_L_G;
i2c_bus->msg[1].flag = VSFHAL_I2C_READ;
i2c_bus->msg[1].buf = i2c_bus->temp + 4;
i2c_bus->msg[1].len = 12;
vsftimer_create(pt->sm, 4, -1, VSFSM_EVT_LSM6DS3_POLL);
do
{
vsfsm_pt_wfe(pt, VSFSM_EVT_LSM6DS3_POLL);
vsfhal_i2c_xfer(i2c_bus->index, info->i2c_addr, i2c_bus->msg, 2);
vsfsm_pt_wfe(pt, VSFSM_EVT_LSM6DS3_XFER_DONE);
gyro_acc_data_input(i2c_bus->msg[1].buf, 12);
} while (1);
vsfsm_pt_end(pt);
return VSFERR_NONE;
}
void lsm6ds3_init(struct lsm6ds3_info_t *info)
{
info->xfer_pt.thread = lsm6ds3_thread;
info->xfer_pt.user_data = info;
vsfsm_pt_init(&info->xfer_sm, &info->xfer_pt);
} 复制代码