嵌入式算法8---空间向量夹角公式及其应用

有些设备正常工作时需按合适的方位安装，比如GPS天线必须朝向天空才能保证信号最佳，温湿度传感器监测口必须朝向被测目标才能及时响应。软件需求是在安装角度异常时提醒用户改变位置。那么设备如何感知当前方位呢？需要一颗加速度传感器硬件支持，辅以算法实现。

1、重力加速度

根据物理常识，地面上任何物体静止时都受到1g的重力加速度，且方向是竖直向下。

因为倾斜角的不同，1g的加速度按向量分解到xyz三轴：
acc_x=1g.sinθ.cosϕ
acc_y=-1g.sinθ.sinϕ
acc_z=1g.cosϕ
符号.代表相乘，读取加速度传感器的xyz三轴数据的细节，不是本文考虑的范畴。

因为传感器的位数精度和量程不同，同样的1g，读寄存器的数值不同，为统一后文描述，假设数值255对应1g的加速度。物体水平静止时加速度值理想情况是（0，0，255），反向水平放置是（0，0，-255）。这个数值等比例的缩小或放大，不影响角度的判断。

2、空间向量夹角

假设期望的正确安装方式下三轴是（x0,y0,z0），实际三轴数据是（x1,y1,z1）。那如何得出当前偏差的角度呢？

已知两空间向量的坐标为a=(x1,y1,z1)，b=(x2,y2,z2)，则两向量的夹角余弦cosθ公式为：

在实际应用中，翻转角度为181度时，按179度处理。本文只考虑0-180度的应用，180度以上的需要额外再计算翻转。

根据空间向量夹角余弦，再反余弦得出在0-180度的角度，即可判断设备安装角度是否正确。

3、代码实现

#include "math.h"
#include "stdio.h"

#define PI  3.1415926

typedef struct
{
   
   
    unsigned short x;
    unsigned short y;
    unsigned short z;
}sensor_data_struct;

static sensor_data_struct ref={
   
   0,0,255};
static sensor_data_struct test={
   
   0,180,180};

//计算当前向量与参考向量的夹角
float get_angle(sensor_data_struct data)
{
   
   
    float cosine;
    float temp,angle;
    cosine=(data.x*ref.x+data.y*ref.y+data.z*ref.z)/ \
            ((sqrt(data.x*data.x+data.y*data.y+data.z*data.z))*\
             (sqrt(ref.x*ref.x+ref.y*ref.y+ref.z*ref.z)));

     temp=acos(cosine);
     angle=(temp*180)/PI;
     return angle;
}

int main(int argc, char *argv[])
{
   
   
    float angle;
    printf("reference vector (%d,%d,%d)\r\n",ref.x,ref.y