// File: /workspace/github_c_makefile_star2_O3/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 %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
movl $0x1, 0x8c71(%rip)      # 0xa174
movl $0x0, 0x8c63(%rip)      # 0xa170
movsd 0x4b4b(%rip), %xmm0     # 0x6060
movsd %xmm0, 0x8c5b(%rip)     # 0xa178
movl $0x41200000, 0x8c59(%rip) # imm = 0x41200000
leaq 0x4be5(%rip), %rdi      # 0x6113
callq 0x1110
xorl %edi, %edi
callq 0x1c21
testb %al, %al
je 0x154a
leaq 0x4eff(%rip), %rdi      # 0x6444
jmp 0x173a
leaq 0x4b6f(%rip), %rdi      # 0x60c0
xorl %eax, %eax
callq 0x1040
leaq 0x4bcd(%rip), %rdi      # 0x612c
callq 0x1110
callq 0x3f20
movl $0x78, %edi
callq 0x233e
testb %al, %al
je 0x1583
leaq 0x4eac(%rip), %rdi      # 0x642a
jmp 0x173a
leaq 0x4bc1(%rip), %rdi      # 0x614b
callq 0x1110
leaq 0x4bcf(%rip), %rdi      # 0x6165
callq 0x1110
movl $0x7d0, %edi            # imm = 0x7D0
callq 0x1ec8
testb %al, %al
je 0x15b5
leaq 0x4e5d(%rip), %rdi      # 0x640d
jmp 0x173a
leaq 0x4bce(%rip), %rdi      # 0x618a
callq 0x1110
leaq 0x4bdf(%rip), %rdi      # 0x61a7
callq 0x1110
movl $0x2, %edi
callq 0x1f6b
testb %al, %al
je 0x15e7
leaq 0x4e08(%rip), %rdi      # 0x63ea
jmp 0x173a
leaq 0x4be1(%rip), %rdi      # 0x61cf
callq 0x1110
leaq 0x4bf8(%rip), %rdi      # 0x61f2
callq 0x1110
leaq 0x8ada(%rip), %rdi      # 0xa0e0
callq 0x2c21
movzbl 0x8ace(%rip), %esi      # 0xa0e0
testl %esi, %esi
leaq 0x4ad0(%rip), %rax      # 0x60eb
leaq 0x4ab2(%rip), %rdi      # 0x60d4
cmoveq %rax, %rdi
xorl %eax, %eax
callq 0x1040
leaq 0x4be0(%rip), %rdi      # 0x6214
callq 0x1110
movl $0x78, %edi
callq 0x2179
testb %al, %al
je 0x1653
leaq 0x4d85(%rip), %rdi      # 0x63d3
jmp 0x173a
leaq 0x4bd5(%rip), %rdi      # 0x622f
callq 0x1110
leaq 0x4be0(%rip), %rdi      # 0x6246
callq 0x1110
callq 0x3f4d
testb %al, %al
je 0x1680
leaq 0x4d41(%rip), %rdi      # 0x63bc
jmp 0x173a
leaq 0x4bdb(%rip), %rdi      # 0x6262
callq 0x1110
leaq 0x4be6(%rip), %rdi      # 0x6279
callq 0x1110
movl $0x1, %edi
callq 0x3394
testb %al, %al
je 0x16b2
leaq 0x4cfe(%rip), %rdi      # 0x63ab
jmp 0x173a
leaq 0x4bdc(%rip), %rdi      # 0x6295
callq 0x1110
leaq 0x4be1(%rip), %rdi      # 0x62a6
callq 0x1110
movl $0x170, %edi            # imm = 0x170
callq 0x469b
testb %al, %al
je 0x16e1
leaq 0x4cb5(%rip), %rdi      # 0x6394
jmp 0x173a
leaq 0x4bd4(%rip), %rdi      # 0x62bc
callq 0x1110
leaq 0x4bdf(%rip), %rdi      # 0x62d3
callq 0x1110
movzbl 0x89c0(%rip), %edi      # 0xa0c0
callq 0x42bc
testb %al, %al
je 0x1712
leaq 0x4c65(%rip), %rdi      # 0x6375
jmp 0x173a
leaq 0x4bde(%rip), %rdi      # 0x62f7
callq 0x1110
leaq 0x4bf1(%rip), %rdi      # 0x6316
callq 0x1110
callq 0x2845
testb %al, %al
je 0x174e
leaq 0x4c1d(%rip), %rdi      # 0x6357
callq 0x1110
movl $0xffffffff, %eax       # imm = 0xFFFFFFFF
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq
leaq 0x4be4(%rip), %rdi      # 0x6339
callq 0x1110
leaq 0x49a4(%rip), %rdi      # 0x6105
xorl %eax, %eax
callq 0x1040
leaq 0x89c7(%rip), %rbx      # 0xa136
leaq 0x89ba(%rip), %r14      # 0xa130
leaq 0x89c3(%rip), %r15      # 0xa140
leaq 0x8a00(%rip), %r12      # 0xa184
leaq 0x8956(%rip), %r13      # 0xa0e1
movzbl 0x892e(%rip), %ecx      # 0xa0c0
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 0x4b3e
movzbl %al, %eax
movl %eax, 0x89a6(%rip)      # 0xa16c
testb %al, %al
jne 0x178b
cmpb $0x5, 0x8910(%rip)      # 0xa0e1
jb 0x178b
leaq 0x4c92(%rip), %rdi      # 0x646c
callq 0x1110
movl $0x1, 0x8987(%rip)      # 0xa170
xorl %eax, %eax
jmp 0x1744