MPU6050计算竖直平面的物体倾角

/********************************* (C) COPYRIGHT 2019 曼珠沙华 **************************************************** * File Name : angle.c * Author : @曼珠沙华 * Date : 2019/12/02 * Description : 倾斜角度计算 * * Includes ------------------------------------------------------------------*/ #include "angle.h" static float angle_offset; //加速度的原始角度 static int offset_ax = 0,offset_ay = 0; //加速度的漂移值 /*********************************************************************************************** ** name: test_offset ** function: 测试加速度的原始角度与漂移 **parameter: void ** return: void ** date: 2019/11/29 ** create: @曼珠沙华 ************************************************************************************************/ void test_offset(void){ int tempx = 0, tempy = 0; u16 i; short ax,ay,az; for(i = 0; i < 500; i++){ //取500次的XY轴加速值，进行累加，消除飘移 MPU_Get_Accelerometer(&ax,&ay,&az); //通过mpu6050拿取加速度的值 delay_ms(10); //延时10ms tempx += ax; tempy += ay; } offset_ax = tempx / 500; offset_ay = tempy / 500; angle_offset = atan2(offset_ay,offset_ax)*180/PI1; //算出最初始的角度 get_angle(); //拿取一次角度，判断测量效果 } /*********************************************************************************************** ** name: get_angle ** function: 获取机器的倾斜角度 **parameter: void ** return: 倾斜角度 ** date: 2019/11/29 ** create: @曼珠沙华 ************************************************************************************************/ float get_angle(void){ static int acc_x[5] = {0}; //此数组用于存放多个X或Y轴的加速度，类似滤波，主要为了防止加速度的突变造成测量不准确 static int acc_y[5] = {0}; //可以适当加减该数组的成员数，增加成员数能使角度变化更细腻， //减少成员数会使角度变化反应更迅速，需自己去测试一个中间值 int x_num = 0,y_num = 0; float angle; short ax,ay,az; u8 i; for(i = 0; i < 5; i++){ //如果数组没有拿到5个值，就先拿满五个值 这儿都是第一次才会直接拿五个 if(acc_x[i] == 0){ MPU_Get_Accelerometer(&ax,&ay,&az); acc_x[i] = ax; acc_y[i] = ay; } } for(i = 0; i < 5; i++){ //开始累加5个值 x_num += acc_x[i]; y_num += acc_y[i]; } for(i = 0; i < 4; i++){ //拿掉成员0的值，其余的值依次前进一位 acc_x[i] = acc_x[i + 1]; acc_y[i] = acc_y[i + 1]; } ax = (x_num) / 5; ay = (y_num) / 5; angle = atan2(ay,ax) * 180 / PI1; //计算角度 zuo'p if(ay >= 0){ //判断角度的正反（即往左还是往右偏），此代码中左偏为负，右偏为正 angle = (angle - fabs(angle_offset)); //此时的值减去初始值，就是当前的倾斜角度 } else{ if(angle_offset < 0){ //防止360°反偏 angle = (angle - angle_offset); } else{ angle = (angle + angle_offset); } } acc_x[4] = 0; //成员4赋值0 acc_y[4] = 0; x_num = 0; y_num = 0; printf("angle = %f\n",angle); //打印观察 return angle; } /************************************* (C) COPYRIGHT 2019 @曼珠沙华 **********************************************/ /********************************************* END OF FILE ***************************************************/