3D加速度传感器计算角度

8位2g加速度数据为例

• 计算静态时X,Y,Z哪个轴朝下

typedef struct
{
   int8_t x;
   int8_t y;
   int8_t z;
}accel_xyz_data_f;


/**
 * @brieaf 计算最后静态的轴指向,0-X,1-Y,2-Z
*/
static uint8_t calc_last_axis(accel_xyz_data_f *accel_xyz_data)
{
	 uint8_t buf[3];
	 buf[0] = accel_xyz_data->x >= 0 ? accel_xyz_data->x : -accel_xyz_data->x;
	 buf[1] = accel_xyz_data->y >= 0 ? accel_xyz_data->y : -accel_xyz_data->y;
	 buf[2] = accel_xyz_data->z >= 0 ? accel_xyz_data->z : -accel_xyz_data->z;
	 uint8_t axis = 0;
	
	 uint8_t max = buf[0];
	 for(uint8_t i=1; i<3; i++)
	 {						 
		 if(buf[i] > max)
			 axis = i; 
			max = buf[i];							 
	 }
	 return axis;
 }

• 测试数据

  accel_xyz_data_f xyz;
  xyz.x = 0;
  xyz.y = 0;
  xyz.z = 64; 
  uint8_t mode = calc_last_axis(&xyz);

• 结果为z轴
• 计算pitch和roll角度，并转换成360度格式

#include "math.h"
/**
 * @brieaf 计算pitch,roll角度
*/
static void calc_pitch_roll_angle(int16_t *pitch,int16_t *roll,accel_xyz_data_f *xyz,uint8_t mode)
{
	int16_t temp_pitch,temp_roll;
	switch(mode)
	{
		case 0:    // 基于X轴
			 temp_pitch = (int16_t)(atan2((float)(xyz->z),xyz->x) * 180 / 3.14159f); 	
			 temp_roll  = (int16_t)(atan2((float)(xyz->y),xyz->x) * 180 / 3.14159f);        //转换为度数
			break;
		case 1:   // 基于Y轴
			 temp_pitch = (int16_t)(atan2((float)(xyz->z),xyz->y) * 180 / 3.14159f); 	
			 temp_roll  = (int16_t)(atan2((float)(xyz->x),xyz->y) * 180 / 3.14159f);        //转换为度数
			break;
		case 2:  // 基于Z轴
			 temp_pitch = (int16_t)(atan2((float)(xyz->y),xyz->z) * 180 / 3.14159f); 	
			 temp_roll  = (int16_t)(atan2((float)(xyz->x),xyz->z) * 180 / 3.14159f);        //转换为度数
			break;
	}
	if(temp_pitch < 0)
		temp_pitch += 360;
	if(temp_roll < 0)
		temp_roll += 360;
	
	*pitch = temp_pitch; 
	*roll = temp_roll;
}

• 测试数据

  accel_xyz_data_f xyz;
  xyz.x = 6;
  xyz.y = -6;
  xyz.z = 62;
  int16_t pitch,roll;
  calc_pitch_roll_angle(&pitch,&roll,&xyz,2);

• 结果pitch为355度，roll为5度