一.思路
1.发生碰撞:依次计算加速度计值之间的差值,当差值累计超过一定值时则发生碰撞。这个方法比较简单,缺点是误差比较大,需要通过不断调整阈值以及次数使结果接近准确
2.发生摔倒:摔倒后为静止态时,才会去检测发生摔倒,运动时不进行检测。分别计算x,y,z三轴与重力加速度之间的夹角,当夹角大于一定度数多次后,判定为摔倒。
二.摔倒计算夹角分析
发生碰撞的计算方式比较简单这里不再赘述。
(1) 当芯片处于静止状态时,x,y重力加速度计值为0,z轴加速度计值为重力加速度g,如下图所示
(2) 当物体倾斜一定的角度后。需要计算x,y,z与水平线之间的夹角也就是α1,β1, γ1; α, β, γ分别是α1,β1, γ1的余角,也是和重力加速度g之间的夹角,根据这个关系可以推导出α1,β1, γ1
这里为了好理解只计算X轴的角度α1;
角的关系:sinα1 = Ax / g -> Ax = gsinα1 (1)
边的关系:g*g = Ax*Ax + gconα1*gconα1 -> gconα1 = sqrt(g*g - Ax*Ax) (2)
(1)比(2) -> tanα1 = Ax / sqrt(g*g - Ax*Ax)
因为在静止时重力加速度的平方等于三轴平方和,即:g*g = Ax*Ax + Ay*Ay + Az*Az
综上tanα1 = Ax / sqrt(Az*Az + Ay*Ay)
其中 α1 、β1 、γ1分别是X、Y、Z轴和水平线的弧度值(反三角函数计算的值是弧度),Ax 、Ay、Az是三个轴上的加速度值。那么弧度值分别为:
α1 = arctan(Ax / sqrt(Ay*Ay + Az*Az))
β1= arctan(Ay / sqrt(Ax*Ax+Az*Az))
γ1= arctan( Az / sqrt(Ax*Ax +Ay*Ay))
又因为弧度= θπR/180,所以
θx = [arctan(Ax / squr(Ay*Ay + Az*Az))] *180/π
θy = [arctan(Ay / squr(Ax*Ax+Az*Az))]*180/π
θz = [arctan( Az / squr(Ax*Ax +Ay*Ay))]*180/π
二.代码实现
碰撞
sensor_event report = {0};
void collision_detect()
{
uint16_t acc_x = 0;
uint16_t acc_y = 0;
uint16_t acc_z = 0;
static uint16_t last_x = 0;
static uint16_t last_y = 0;
static uint16_t last_z = 0;
//发生碰撞的阈值,根据需要修改
static uint16_t detect_threshold = 6000;
//碰撞的次数
static uint8_t detect_count;
uint8_t ret;
sensor_acc_get_data_original(&(report.x_acc), &(report.y_acc), &(report.z_acc));//获取原始值
acc_x = port_abs(report.x_acc - last_x);
acc_y = port_abs(report.y_acc - last_y);
acc_z = port_abs(report.z_acc - last_z);
ret = (acc_x > detect_threshold) + (acc_y > detect_threshold) + (acc_z > detect_threshold);
if (ret > 0) {
detect_count += ret;
}
last_x = report.x_acc;
last_y = report.y_acc;
last_z = report.z_acc;
if ((detect_count >= 3)) {
detect_count = 0;
last_x = 0;
last_y = 0;
last_z = 0;
port_trace("happend collision");
return;
}
}
摔倒的代码
#define g 1024
#define PI 3.14
sensor_event report = {0};
uint8_t tumble_count = 0;
void tumble_handle()
{
uint16_t result = 0;
uint16_t angle_x;
uint16_t angle_y;
uint16_t angle_z;
result = sqrt(report.x_acc * report.x_acc + report.y_acc * report.y_acc + report.z_acc * report.z_acc);
//判断为静止
if (result < g ) {
port_trace("no tumble");
}
angle_x = (atan(report.x_acc / sqrt(report.y_acc * report.y_acc + report.z_acc * report.z_acc))) * 180 / PI;
angle_y = (atan(report.y_acc / sqrt(report.x_acc * report.x_acc + report.z_acc * report.z_acc))) * 180 / PI;
angle_z = (atan(report.z_acc / sqrt(report.x_acc * report.x_acc + report.y_acc * report.y_acc))) * 180 / PI;
if ((port_abs(angle_x) > 45 ) || (port_abs(angle_y) > 45) || (port_abs(angle_z) > 45 )) {
tumble_count ++;
if (tumble_count >= 3) {
port_trace("tumble");
tumble_count = 0;
}
} else {
tumble_count = 0;
}
}