8位2g加速度数据为例
typedef struct
{
int8_t x;
int8_t y;
int8_t z;
}accel_xyz_data_f;
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"
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:
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:
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:
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);