一、本节内容
1. 读取BMI160角加速度信息
2. 处理数据进行采样,存储数值
3. 将数值进行对比,检测姿态
二、原理讲解
本节先使用寄存器读取数据,然后使用数据进行处理,通过简单算法推断姿态。
三、库函数
部分库函数和寄存器:
学习MSP432M0手册——第五节systick和UART接收
四、软件代码
void I2Cwrite(uint32_t I2C_TARGET_ADDRESS, uint8_t I2C_TX_PACKET_SIZE, uint8_t *senddataAdd)
I2C通讯下对某一从机的寄存器的写入
void I2Cwrite(uint32_t I2C_TARGET_ADDRESS, uint8_t I2C_TX_PACKET_SIZE, uint8_t *senddataAdd)
{
DL_I2C_fillControllerTXFIFO(I2C_INST, &senddataAdd[0], I2C_TX_PACKET_SIZE);
while (!(
DL_I2C_getControllerStatus(I2C_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
DL_I2C_startControllerTransfer(I2C_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);
while (DL_I2C_getControllerStatus(I2C_INST) &
DL_I2C_CONTROLLER_STATUS_BUSY_BUS)
;
if (DL_I2C_getControllerStatus(I2C_INST) &
DL_I2C_CONTROLLER_STATUS_ERROR) {
__BKPT(0);
}
while (!(
DL_I2C_getControllerStatus(I2C_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
delay_cycles(1000);
}
void I2Cread(uint32_t I2C_TARGET_ADDRESS, uint8_t I2C_TX_PACKET_SIZE, uint8_t *senddataAdd, uint8_t I2C_RX_PACKET_SIZE,uint8_t *gRxPacket)
I2C通讯下对某一从机的寄存器的读取
void I2Cread(uint32_t I2C_TARGET_ADDRESS, uint8_t I2C_TX_PACKET_SIZE, uint8_t *senddataAdd, uint8_t I2C_RX_PACKET_SIZE,uint8_t *gRxPacket)
{
DL_I2C_fillControllerTXFIFO(I2C_INST, &senddataAdd[0], I2C_TX_PACKET_SIZE);
while (!(
DL_I2C_getControllerStatus(I2C_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
DL_I2C_startControllerTransfer(I2C_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);
while (DL_I2C_getControllerStatus(I2C_INST) &
DL_I2C_CONTROLLER_STATUS_BUSY_BUS)
;
if (DL_I2C_getControllerStatus(I2C_INST) &
DL_I2C_CONTROLLER_STATUS_ERROR) {
__BKPT(0);
}
while (!(
DL_I2C_getControllerStatus(I2C_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
delay_cycles(1000);
DL_I2C_startControllerTransfer(I2C_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_RX, I2C_RX_PACKET_SIZE);
for (uint8_t i = 0; i < I2C_RX_PACKET_SIZE; i++) {
while (DL_I2C_isControllerRXFIFOEmpty(I2C_INST))
;
gRxPacket[i] = DL_I2C_receiveControllerData(I2C_INST);
}
}
通过向命令寄存器写入0x15,使陀螺仪处于正常工作模式,再根据地址读取对应信息进行公式计算后可得到正确加速度值。(我这边就不放源代码了,有需要私聊我)
对经过处理后的数据,进行采样滤波。
void lvbo()
volatile float max_value_x;//x最大值
volatile float max_value_y;//y最大值
volatile float max_value_z;//z最大值
volatile float min_value_x;//x最小值
volatile float min_value_y;//y最小值
volatile float min_value_z;//z最小值
volatile int16_t step = 0;//步数
volatile int16_t count_i = 0;//循环位
volatile float ave_x;
volatile float ave_y;
volatile float ave_z;
volatile float ACC_X_AVE[5];
volatile float ACC_Y_AVE[5];
volatile float ACC_Z_AVE[5];
volatile float diff_x;//x差值
volatile float diff_y;//y差值
volatile float diff_z;//z差值
void lvbo()
{
ACC_X_AVE[count_i] = acc_x;
ACC_Y_AVE[count_i] = acc_y;
ACC_Z_AVE[count_i] = acc_z;
if(count_i>=2)
{
max_value_x = ACC_X_AVE[0];
max_value_y = ACC_Y_AVE[0];
max_value_z = ACC_Z_AVE[0];
min_value_x = ACC_X_AVE[0];
min_value_y = ACC_Y_AVE[0];
min_value_z = ACC_Z_AVE[0];
// 遍历数组,找出最大最小值_x
for (int count_c = 1; count_c < 2; count_c++)
{
if (ACC_X_AVE[count_c] > max_value_x)
{
max_value_x = ACC_X_AVE[count_c];
}
if (ACC_X_AVE[count_c] < min_value_x)
{
min_value_x = ACC_X_AVE[count_c];
}
}
// 遍历数组,找出最大最小值_y
for (int count_d = 1; count_d < 2; count_d++)
{
if (ACC_Y_AVE[count_d] > max_value_y)
{
max_value_y = ACC_Y_AVE[count_d];
}
if (ACC_Y_AVE[count_d] < min_value_y)
{
min_value_y = ACC_Y_AVE[count_d];
}
}
// 遍历数组,找出最大最小值_z
for (int count_f = 1; count_f < 2; count_f++)
{
if (ACC_Z_AVE[count_f] > max_value_z)
{
max_value_z = ACC_Z_AVE[count_f];
}
if (ACC_Z_AVE[count_f] < min_value_z)
{
min_value_z = ACC_Z_AVE[count_f];
}
}
}
}
经过数据处理后,对比数据检测姿态,在数据合理时,当记录的数据达到峰值的时候计为一步,然后当角加速度一段时间内的差值大于某一数值时计为跌倒。
#define count_high 2.8 //计步器调参
#define count_low -2.8
#define count_dao 10.00 //跌倒报警调参
int detect_step(double threshold)
{
diff_x = max_value_x - min_value_x;
diff_y = max_value_y - min_value_y;
diff_z = max_value_z - min_value_z;
if(diff_x > diff_y && diff_x > diff_z)
{
if(diff_x > threshold && (ACC_X_AVE[1] - ACC_X_AVE[0] > count_high || ACC_X_AVE[1] - ACC_X_AVE[2] < count_low))
{
step++;
}
}
if(diff_y > diff_x && diff_x > diff_z)
{
if(diff_x > threshold && (ACC_Y_AVE[1] - ACC_Y_AVE[0] > count_high || ACC_Y_AVE[1] - ACC_Y_AVE[2] < count_low))
{
step++;
}
}
if(diff_z > diff_y && diff_z > diff_x)
{
if(diff_x > threshold && (ACC_Z_AVE[1] - ACC_Z_AVE[0] > count_high || ACC_Z_AVE[1] - ACC_Z_AVE[2] < count_low))
{
step++;
}
}
}
void zitai_baojing(float acc_Z)
{
int key;
if (diff_x > count_dao || diff_y > count_dao || diff_z > count_dao)
{
while(1)
{
DL_GPIO_setPins(GPIO_baojing_PORT,GPIO_baojing_PIN_0_PIN);
key = (GPIOA->DIN31_0&(0x01<<27));
if(key == 0)
{
break;
}
}
}
else
{
DL_GPIO_clearPins(GPIO_baojing_PORT,GPIO_baojing_PIN_0_PIN);
}
}
主函数需要这样写:
int main(void)
{
SYSCFG_DL_init(); //M0初始化
while (1) {
getAccelerometerValue(); //获取加速度数据
getGyroscopeValue(); //获取角速度数据
delay_cycles(100000);
lvbo();//记步数
detect_step(0.5);
count_i++;
if(count_i == 3)
{
count_i = 0;
ave_x = 0;
ave_y = 0;
ave_z = 0;
}
}
}
一些简单的算法,主要是要学会IIC通讯,和数据运用。