// File: /workspace/github_c_makefile_star2_O1/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:
movl $0x1, 0x8c66(%rip)      # 0xa170
movl $0x0, 0x8c58(%rip)      # 0xa16c
xorl %eax, %eax
leaq 0x8c57(%rip), %rcx      # 0xa174
movl $0x41200000, (%rax,%rcx) # imm = 0x41200000
addq $0x4, %rax
cmpq $0xc, %rax
jne 0x151d
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
leaq 0x4bc5(%rip), %rdi      # 0x6103
callq 0x1110
xorl %edi, %edi
callq 0x1c8e
testb %al, %al
je 0x155a
leaq 0x4edf(%rip), %rdi      # 0x6434
jmp 0x174a
leaq 0x4b4f(%rip), %rdi      # 0x60b0
xorl %eax, %eax
callq 0x1040
leaq 0x4bad(%rip), %rdi      # 0x611c
callq 0x1110
callq 0x3efc
movl $0x78, %edi
callq 0x2398
testb %al, %al
je 0x1593
leaq 0x4e8c(%rip), %rdi      # 0x641a
jmp 0x174a
leaq 0x4ba1(%rip), %rdi      # 0x613b
callq 0x1110
leaq 0x4baf(%rip), %rdi      # 0x6155
callq 0x1110
movl $0x7d0, %edi            # imm = 0x7D0
callq 0x1f36
testb %al, %al
je 0x15c5
leaq 0x4e3d(%rip), %rdi      # 0x63fd
jmp 0x174a
leaq 0x4bae(%rip), %rdi      # 0x617a
callq 0x1110
leaq 0x4bbf(%rip), %rdi      # 0x6197
callq 0x1110
movl $0x2, %edi
callq 0x1fd8
testb %al, %al
je 0x15f7
leaq 0x4de8(%rip), %rdi      # 0x63da
jmp 0x174a
leaq 0x4bc1(%rip), %rdi      # 0x61bf
callq 0x1110
leaq 0x4bd8(%rip), %rdi      # 0x61e2
callq 0x1110
leaq 0x8aca(%rip), %rdi      # 0xa0e0
callq 0x2c57
movzbl 0x8abe(%rip), %esi      # 0xa0e0
testl %esi, %esi
leaq 0x4ab0(%rip), %rax      # 0x60db
leaq 0x4a92(%rip), %rdi      # 0x60c4
cmoveq %rax, %rdi
xorl %eax, %eax
callq 0x1040
leaq 0x4bc0(%rip), %rdi      # 0x6204
callq 0x1110
movl $0x78, %edi
callq 0x21dd
testb %al, %al
je 0x1663
leaq 0x4d65(%rip), %rdi      # 0x63c3
jmp 0x174a
leaq 0x4bb5(%rip), %rdi      # 0x621f
callq 0x1110
leaq 0x4bc0(%rip), %rdi      # 0x6236
callq 0x1110
callq 0x3f29
testb %al, %al
je 0x1690
leaq 0x4d21(%rip), %rdi      # 0x63ac
jmp 0x174a
leaq 0x4bbb(%rip), %rdi      # 0x6252
callq 0x1110
leaq 0x4bc6(%rip), %rdi      # 0x6269
callq 0x1110
movl $0x1, %edi
callq 0x33b2
testb %al, %al
je 0x16c2
leaq 0x4cde(%rip), %rdi      # 0x639b
jmp 0x174a
leaq 0x4bbc(%rip), %rdi      # 0x6285
callq 0x1110
leaq 0x4bc1(%rip), %rdi      # 0x6296
callq 0x1110
movl $0x170, %edi            # imm = 0x170
callq 0x4676
testb %al, %al
je 0x16f1
leaq 0x4c95(%rip), %rdi      # 0x6384
jmp 0x174a
leaq 0x4bb4(%rip), %rdi      # 0x62ac
callq 0x1110
leaq 0x4bbf(%rip), %rdi      # 0x62c3
callq 0x1110
movzbl 0x89b0(%rip), %edi      # 0xa0c0
callq 0x4298
testb %al, %al
je 0x1722
leaq 0x4c45(%rip), %rdi      # 0x6365
jmp 0x174a
leaq 0x4bbe(%rip), %rdi      # 0x62e7
callq 0x1110
leaq 0x4bd1(%rip), %rdi      # 0x6306
callq 0x1110
callq 0x289f
testb %al, %al
je 0x175e
leaq 0x4bfd(%rip), %rdi      # 0x6347
callq 0x1110
movl $0xffffffff, %eax       # imm = 0xFFFFFFFF
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq
leaq 0x4bc4(%rip), %rdi      # 0x6329
callq 0x1110
leaq 0x4984(%rip), %rdi      # 0x60f5
xorl %eax, %eax
callq 0x1040
leaq 0x89b7(%rip), %rbx      # 0xa136
leaq 0x89aa(%rip), %r14      # 0xa130
leaq 0x89b3(%rip), %r15      # 0xa140
leaq 0x89ec(%rip), %r12      # 0xa180
leaq 0x8946(%rip), %r13      # 0xa0e1
movzbl 0x891e(%rip), %ecx      # 0xa0c0
movl $0x3e8, %eax            # imm = 0x3E8
xorl %edx, %edx
divl %ecx
imull $0x3e8, %eax, %edi      # imm = 0x3E8
callq 0x10e0
movq %rbx, %rdi
movq %r14, %rsi
movq %r15, %rdx
movq %r12, %rcx
movq %r13, %r8
callq 0x4ac2
movzbl %al, %eax
movl %eax, 0x8995(%rip)      # 0xa168
testb %al, %al
jne 0x179b
cmpb $0x5, 0x8903(%rip)      # 0xa0e1
jb 0x179b
leaq 0x4c75(%rip), %rdi      # 0x645c
callq 0x1110
movl $0x1, 0x8976(%rip)      # 0xa16c
xorl %eax, %eax
jmp 0x1754