// File: /workspace/github_c_makefile_star2_O2/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/sensor.c
// Function: ms_open()
int ms_open() 
	{
	dmpReady=1;
	initialized = 0;
	for (int i=0;i<DIM;i++){
		lastval[i]=10;
	}

	// initialize device
	printf("初始化MPU...请等待\n");
	if (mpu_init(NULL) != 0) {
		printf("MPU初始化失败.\n");
		return -1;
	}
	printf("MPU初始化成功.");

	printf("设置MPU传感器...请等待\n");
    dump_init();
	if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
		printf("MPU传感器设置失败.\n");
		return -1;
	}
	printf("MPU传感器设置成功.\n");

	printf("设置陀螺仪灵敏度...请等待\n");
	if (mpu_set_gyro_fsr(2000)!=0) {
		printf("陀螺仪精度设置失败.\n");
		return -1;
	}
	printf("陀螺仪精度设置成功.\n");

	printf("设置加速度计灵敏度...请等待\n");
	if (mpu_set_accel_fsr(2)!=0) {
		printf("加速度计灵敏度设置失败.\n");
		return -1;
	}
	printf("加速度计灵敏度设置成功.\n");

	// verify connection
	printf("执行MPU开机命令...请等待\n");
	mpu_get_power_state(&devStatus);
	printf(devStatus ? "MPU6050 连接成功.\n" : "MPU6050 连接失败. %u\n",devStatus);

	//fifo config
	printf("设置MPU FIFO...请等待\n");
	if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
		printf("MPU fifo 设置失败.\n");
		return -1;
	}
	printf("MPU fifo 设置成功.\n");


	// load and configure the DMP
	printf("读取DMP固件...请等待\n");
	if (dmp_load_motion_driver_firmware()!=0) {
		printf("DMP固件打开失败.\n");
		return -1;
	}
	printf("DMP固件打开成功.\n");


	printf("正在激活DMP...请等待\n");
	if (mpu_set_dmp_state(1)!=0) {
		printf("DMP激活失败.\n");
		return -1;
	}
	printf("DMP激活成功.\n");

	//dmp_set_orientation()
	//if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
	printf("设置DMP...请等待\n");
	if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
		printf("DMP特征提取失败.\n");
		return -1;
	}
	printf("DMP特征提取成功.\n");

	printf("设置DMP FIFO采样率...请等待\n");
	if (dmp_set_fifo_rate(rate)!=0) {
		printf("DMP FIFO采样率设置失败.\n");
		return -1;
	}
	printf("DMP FIFO采样率设置成功.\n");


	printf("重置FIFO消息队列...请等待\n");
	if (mpu_reset_fifo()!=0) {
		printf("FIFO消息队列重置失败.\n");
		return -1;
	}
	printf("FIFO消息队列重置成功.\n");

	printf("检测中... ");
	do {
		delay_ms(1000/rate);  //dmp will habve 4 (5-1) packets based on the fifo_rate
		r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount);
	} while (r!=0 || fifoCount<5); //packtets!!!
	printf("完成.\n");

	initialized = 1;
	return 0;
}

// Assembly:
andl $0x0, 0x7eb9(%rip)      # 0x915c
movl $0x1, 0x7eb3(%rip)      # 0x9160
xorl %eax, %eax
leaq 0x7eae(%rip), %rcx      # 0x9164
cmpq $0xc, %rax
je 0x12c9
movl $0x41200000, (%rax,%rcx) # imm = 0x41200000
addq $0x4, %rax
jmp 0x12b6
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
leaq 0x3e2a(%rip), %rdi      # 0x5103
callq 0x1110
xorl %edi, %edi
callq 0x196b
testb %al, %al
je 0x12f5
leaq 0x4144(%rip), %rdi      # 0x5434
jmp 0x14dd
leaq 0x3db4(%rip), %rdi      # 0x50b0
xorl %eax, %eax
callq 0x1040
leaq 0x3e12(%rip), %rdi      # 0x511c
callq 0x1110
callq 0x3a88
pushq $0x78
popq %rdi
callq 0x2049
testb %al, %al
je 0x132c
leaq 0x40f3(%rip), %rdi      # 0x541a
jmp 0x14dd
leaq 0x3e08(%rip), %rdi      # 0x513b
callq 0x1110
leaq 0x3e16(%rip), %rdi      # 0x5155
callq 0x1110
movl $0x7d0, %edi            # imm = 0x7D0
callq 0x1bf9
testb %al, %al
je 0x135e
leaq 0x40a4(%rip), %rdi      # 0x53fd
jmp 0x14dd
leaq 0x3e15(%rip), %rdi      # 0x517a
callq 0x1110
leaq 0x3e26(%rip), %rdi      # 0x5197
callq 0x1110
pushq $0x2
popq %rdi
callq 0x1c8f
testb %al, %al
je 0x138e
leaq 0x4051(%rip), %rdi      # 0x53da
jmp 0x14dd
leaq 0x3e2a(%rip), %rdi      # 0x51bf
callq 0x1110
leaq 0x3e41(%rip), %rdi      # 0x51e2
callq 0x1110
leaq 0x7d23(%rip), %rdi      # 0x90d0
callq 0x28e9
movzbl 0x7d17(%rip), %esi      # 0x90d0
testl %esi, %esi
leaq 0x3d19(%rip), %rax      # 0x50db
leaq 0x3cfb(%rip), %rdi      # 0x50c4
cmoveq %rax, %rdi
xorl %eax, %eax
callq 0x1040
leaq 0x3e29(%rip), %rdi      # 0x5204
callq 0x1110
pushq $0x78
popq %rdi
callq 0x1e90
testb %al, %al
je 0x13f8
leaq 0x3fd0(%rip), %rdi      # 0x53c3
jmp 0x14dd
leaq 0x3e20(%rip), %rdi      # 0x521f
callq 0x1110
leaq 0x3e2b(%rip), %rdi      # 0x5236
callq 0x1110
callq 0x3ab8
testb %al, %al
je 0x1425
leaq 0x3f8c(%rip), %rdi      # 0x53ac
jmp 0x14dd
leaq 0x3e26(%rip), %rdi      # 0x5252
callq 0x1110
leaq 0x3e31(%rip), %rdi      # 0x5269
callq 0x1110
pushq $0x1
popq %rdi
callq 0x2fff
testb %al, %al
je 0x1455
leaq 0x3f4b(%rip), %rdi      # 0x539b
jmp 0x14dd
leaq 0x3e29(%rip), %rdi      # 0x5285
callq 0x1110
leaq 0x3e2e(%rip), %rdi      # 0x5296
callq 0x1110
movl $0x170, %edi            # imm = 0x170
callq 0x41bf
testb %al, %al
je 0x1484
leaq 0x3f02(%rip), %rdi      # 0x5384
jmp 0x14dd
leaq 0x3e21(%rip), %rdi      # 0x52ac
callq 0x1110
leaq 0x3e2c(%rip), %rdi      # 0x52c3
callq 0x1110
movzbl 0x7c15(%rip), %edi      # 0x90b8
callq 0x3e17
testb %al, %al
je 0x14b5
leaq 0x3eb2(%rip), %rdi      # 0x5365
jmp 0x14dd
leaq 0x3e2b(%rip), %rdi      # 0x52e7
callq 0x1110
leaq 0x3e3e(%rip), %rdi      # 0x5306
callq 0x1110
callq 0x2526
testb %al, %al
je 0x14ef
leaq 0x3e6a(%rip), %rdi      # 0x5347
callq 0x1110
pushq $-0x1
popq %rax
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq
leaq 0x3e33(%rip), %rdi      # 0x5329
callq 0x1110
leaq 0x3bf3(%rip), %rdi      # 0x50f5
xorl %eax, %eax
callq 0x1040
leaq 0x7c16(%rip), %rbx      # 0x9126
leaq 0x7c09(%rip), %r14      # 0x9120
leaq 0x7c12(%rip), %r15      # 0x9130
leaq 0x7c4b(%rip), %r12      # 0x9170
leaq 0x7ba5(%rip), %r13      # 0x90d1
movzbl 0x7b85(%rip), %ecx      # 0x90b8
movw $0x3e8, %ax             # imm = 0x3E8
xorl %edx, %edx
divw %cx
movzwl %ax, %eax
imull $0x3e8, %eax, %edi      # imm = 0x3E8
callq 0x10e0
movq %rbx, %rdi
movq %r14, %rsi
movq %r15, %rdx
movq %r12, %rcx
movq %r13, %r8
callq 0x4560
movzbl %al, %eax
movl %eax, 0x7bf1(%rip)      # 0x9158
testb %al, %al
jne 0x152c
cmpb $0x5, 0x7b5f(%rip)      # 0x90d1
jb 0x152c
leaq 0x3ee1(%rip), %rdi      # 0x545c
callq 0x1110
movl $0x1, 0x7bd2(%rip)      # 0x915c
xorl %eax, %eax
jmp 0x14e5