// File: /workspace/github_c_makefile_star2_O0/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:
pushq %rbp
movq %rsp, %rbp
subq $0x10, %rsp
movl $0x1, 0xadfe(%rip)      # 0xc160
movl $0x0, 0xadf0(%rip)      # 0xc15c
movl $0x0, -0x8(%rbp)
cmpl $0x3, -0x8(%rbp)
jge 0x139c
movslq -0x8(%rbp), %rcx
leaq 0xade0(%rip), %rax      # 0xc164
movss 0x6cd8(%rip), %xmm0     # 0x8064
movss %xmm0, (%rax,%rcx,4)
movl -0x8(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x8(%rbp)
jmp 0x1373
leaq 0x6cfd(%rip), %rdi      # 0x80a0
movb $0x0, %al
callq 0x1040
xorl %eax, %eax
movl %eax, %edi
callq 0x1f40
movzbl %al, %eax
cmpl $0x0, %eax
je 0x13d5
leaq 0x6cf8(%rip), %rdi      # 0x80ba
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6cf3(%rip), %rdi      # 0x80cf
movb $0x0, %al
callq 0x1040
leaq 0x6cf9(%rip), %rdi      # 0x80e3
movb $0x0, %al
callq 0x1040
callq 0x5520
movl $0x78, %edi
callq 0x2aa0
movzbl %al, %eax
cmpl $0x0, %eax
je 0x1422
leaq 0x6cf4(%rip), %rdi      # 0x8103
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6cf5(%rip), %rdi      # 0x811e
movb $0x0, %al
callq 0x1040
leaq 0x6d02(%rip), %rdi      # 0x8139
movb $0x0, %al
callq 0x1040
movl $0x7d0, %edi            # imm = 0x7D0
callq 0x23e0
movzbl %al, %eax
cmpl $0x0, %eax
je 0x146a
leaq 0x6d08(%rip), %rdi      # 0x815f
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d0c(%rip), %rdi      # 0x817d
movb $0x0, %al
callq 0x1040
leaq 0x6d1c(%rip), %rdi      # 0x819b
movb $0x0, %al
callq 0x1040
movl $0x2, %edi
callq 0x24c0
movzbl %al, %eax
cmpl $0x0, %eax
je 0x14b2
leaq 0x6d25(%rip), %rdi      # 0x81c4
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d2f(%rip), %rdi      # 0x81e8
movb $0x0, %al
callq 0x1040
leaq 0x6d45(%rip), %rdi      # 0x820c
movb $0x0, %al
callq 0x1040
leaq 0xabfb(%rip), %rdi      # 0xc0d0
callq 0x3980
movb 0xabf0(%rip), %cl       # 0xc0d0
leaq 0x6d5f(%rip), %rdi      # 0x8246
leaq 0x6d41(%rip), %rax      # 0x822f
cmpb $0x0, %cl
cmovneq %rax, %rdi
movzbl 0xabd4(%rip), %esi      # 0xc0d0
movb $0x0, %al
callq 0x1040
leaq 0x6d56(%rip), %rdi      # 0x8260
movb $0x0, %al
callq 0x1040
movl $0x78, %edi
callq 0x2780
movzbl %al, %eax
cmpl $0x0, %eax
je 0x153d
leaq 0x6d52(%rip), %rdi      # 0x827c
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d50(%rip), %rdi      # 0x8294
movb $0x0, %al
callq 0x1040
leaq 0x6d5a(%rip), %rdi      # 0x82ac
movb $0x0, %al
callq 0x1040
callq 0x5570
movzbl %al, %eax
cmpl $0x0, %eax
je 0x1580
leaq 0x6d5c(%rip), %rdi      # 0x82c9
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d5a(%rip), %rdi      # 0x82e1
movb $0x0, %al
callq 0x1040
leaq 0x6d64(%rip), %rdi      # 0x82f9
movb $0x0, %al
callq 0x1040
movl $0x1, %edi
callq 0x41b0
movzbl %al, %eax
cmpl $0x0, %eax
je 0x15c8
leaq 0x6d61(%rip), %rdi      # 0x8316
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d59(%rip), %rdi      # 0x8328
movb $0x0, %al
callq 0x1040
leaq 0x6d5d(%rip), %rdi      # 0x833a
movb $0x0, %al
callq 0x1040
movl $0x170, %edi            # imm = 0x170
callq 0x6280
movzbl %al, %eax
cmpl $0x0, %eax
je 0x1610
leaq 0x6d54(%rip), %rdi      # 0x8351
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d52(%rip), %rdi      # 0x8369
movb $0x0, %al
callq 0x1040
leaq 0x6d5c(%rip), %rdi      # 0x8381
movb $0x0, %al
callq 0x1040
movzbl 0xaa7d(%rip), %eax      # 0xc0b0
movzwl %ax, %edi
callq 0x5af0
movzbl %al, %eax
cmpl $0x0, %eax
je 0x165d
leaq 0x6d5c(%rip), %rdi      # 0x83a6
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d62(%rip), %rdi      # 0x83c6
movb $0x0, %al
callq 0x1040
leaq 0x6d74(%rip), %rdi      # 0x83e6
movb $0x0, %al
callq 0x1040
callq 0x3270
movzbl %al, %eax
cmpl $0x0, %eax
je 0x16a0
leaq 0x6d7d(%rip), %rdi      # 0x840a
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x174b
leaq 0x6d82(%rip), %rdi      # 0x8429
movb $0x0, %al
callq 0x1040
leaq 0x6d93(%rip), %rdi      # 0x8448
movb $0x0, %al
callq 0x1040
movzbl 0xa9ed(%rip), %ecx      # 0xc0b0
movl $0x3e8, %eax            # imm = 0x3E8
cltd
idivl %ecx
imull $0x3e8, %eax, %edi      # imm = 0x3E8
callq 0x10f0
leaq 0xaa49(%rip), %rdi      # 0xc126
leaq 0xaa3c(%rip), %rsi      # 0xc120
leaq 0xaa45(%rip), %rdx      # 0xc130
leaq 0xaa7e(%rip), %rcx      # 0xc170
leaq 0xa9d8(%rip), %r8       # 0xc0d1
callq 0x6740
movzbl %al, %eax
movl %eax, 0xaa51(%rip)      # 0xc158
movb $0x1, %al
cmpl $0x0, 0xaa48(%rip)      # 0xc158
movb %al, -0x9(%rbp)
jne 0x1725
movzbl 0xa9b5(%rip), %eax      # 0xc0d1
cmpl $0x5, %eax
setl %al
movb %al, -0x9(%rbp)
movb -0x9(%rbp), %al
testb $0x1, %al
jne 0x16bc
leaq 0x6d37(%rip), %rdi      # 0x846a
movb $0x0, %al
callq 0x1040
movl $0x1, 0xaa18(%rip)      # 0xc15c
movl $0x0, -0x4(%rbp)
movl -0x4(%rbp), %eax
addq $0x10, %rsp
popq %rbp
retq
nopw %cs:(%rax,%rax)