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

• 计算静态时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_daa_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度