mpu6050_algorithm/mpu6050_test.c at master · chaokw/mpu6050_algorithm · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
/**************************************************************
*	Include File Section
**************************************************************/
#include <string.h>
#include <stdio.h>
//#include "Board_uart.h"
#include "mpu6050_etootle_imu.h"      //欧拉角
#include "mpu6050_position_data.h"     //位移


signed short int ax, ay, az, gx, gy, gz;   //存储从蓝牙接收到的6轴数据， ax,ay,az为加速度，gx,gy,gz为陀螺仪
int32 accel_xyz_data[3][ACC_FILTER_COUNT];  //chaokw
int16 accel_pos = 0;

int main()
{

	signed short int accel[3];
 	int32 vel[2][3]={0},disp[2][3] = {0},accel_ave[3],accel_res[2][3]={0};
	int16 i;

	BS004_Load_Filter_Parameter();
  	BS004_Load_Calibration_Parameter();

	while(1)
	{

		// 姿态计算(欧拉角)
		BS004_Get_MPU6050_Data(ax, ay, az, gx, gy, gz);
		BS004_Quad_Calculation();

		// 位移计算
#if 0  //chaokw 20170515
		accel[0] = ax;
		accel[1] = ay;
		accel[2] = az;
		insert_AccelData(accel);

		sigma_Filter(acc_xyz_data,accel_xyz_data,accel_pos,15,4);
		accel_ave[0] = accel_xyz_data[0][accel_pos];
		accel_ave[1] = accel_xyz_data[1][accel_pos];
		accel_ave[2] = accel_xyz_data[2][accel_pos];

		accel_pos++;
		if (accel_pos == ACC_FILTER_COUNT)
		{
			accel_pos = 0;
		}

		extern float quat[4];
		accel_BConvertToN(accel_res[1],accel_ave,quat);

		for(i = 0; i < 3; i++)
		{
			if(accel_res[1][i] < ACCEL_WINDOW_H && accel_res[1][i] > ACCEL_WINDOW_L)
			accel_res[1][i] = 0;
		}

		accel_res[1][2] -= 14890;
		position(accel_res,vel,disp);

		locate_x = (disp[0][0] - disp[1][0])*10/2048;
		locate_y = (disp[1][1] - disp[0][1])*10/2048;

		movement_End_Check(accel_res[1],vel);

		for(i = 0; i < 3; i++)
		{
			vel[0][i] = vel[1][i];
			disp[0][i] = disp[1][i];
			accel_res[0][i] = accel_res[1][i];
		}
#else

		BS004_Position_Calculation(ax, ay, az);

#endif


 		//串口打印计算出的姿态和位移
 		char strTemp[128];
		sprintf(strTemp, "pitch:%f, roll:%f, yaw:%f, x:%f, y:%f\r\n",bs004_imu_pitch, bs004_imu_roll, bs004_imu_yaw, locate_x, locate_y);
		UART_WriteTransport((uint8*)strTemp, strlen(strTemp));

		sleep(5);

	}
}