三轴加速度计值检测碰撞和摔倒

一.思路

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;
    }
}