// File: /workspace/github_c_makefile_star2_O2/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/sensor.c
// Function: ms_update()
int ms_update() {
	if (!dmpReady) {
		printf("错误: DMP未准备完成.\n");
		return -1;
	}

	while (dmp_read_fifo(g,a,_q,&sensors,&fifoCount)!=0); //gyro and accel can be null because of being disabled in the efeatures
	q = _q;
	GetGravity(&gravity, &q);
	GetYawPitchRoll(ypr, &q, &gravity);

	mpu_get_temperature(&t);
	temp=(float)t/65536L;

	mpu_get_compass_reg(c);

	//scaling for degrees output
	for (int i=0;i<DIM;i++){
		ypr[i]*=180/M_PI;
	}

	//unwrap yaw when it reaches 180
	ypr[0] = wrap_180(ypr[0]);

	//change sign of Pitch, MPU is attached upside down
	ypr[1]*=-1.0;

	//0=gyroX, 1=gyroY, 2=gyroZ
	//swapped to match Yaw,Pitch,Roll
	//Scaled from deg/s to get tr/s
	for (int i=0;i<DIM;i++){
		gyro[i]   = (float)(g[DIM-i-1])/131.0/360.0;
		accel[i]   = (float)(a[DIM-i-1]);
		compass[i] = (float)(c[DIM-i-1]);
	}

	return 0;
}

// Assembly:
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x10, %rsp
cmpl $0x0, 0x7bbb(%rip)      # 0x9160
je 0x16ad
leaq 0x7b74(%rip), %rbx      # 0x9126
leaq 0x7b67(%rip), %r14      # 0x9120
leaq 0x7b70(%rip), %r15      # 0x9130
leaq 0x7ba9(%rip), %r12      # 0x9170
leaq 0x7b03(%rip), %r13      # 0x90d1
movq %rbx, %rdi
movq %r14, %rsi
movq %r15, %rdx
movq %r12, %rcx
movq %r13, %r8
callq 0x4560
testb %al, %al
jne 0x15ce
leaq 0x7b43(%rip), %rsi      # 0x9130
movq %rsp, %r15
movq %r15, %rdi
callq 0x18b4
movups (%r15), %xmm0
movups %xmm0, 0x7aa5(%rip)     # 0x90a8
leaq 0x7a9e(%rip), %r15      # 0x90a8
leaq 0x7b3b(%rip), %r13      # 0x914c
movq %r13, %rdi
movq %r15, %rsi
callq 0x1787
leaq 0x7b51(%rip), %r12      # 0x9174
movq %r12, %rdi
movq %r15, %rsi
movq %r13, %rdx
callq 0x17f8
leaq 0x7b08(%rip), %rdi      # 0x9140
callq 0x2363
cvtsi2ssl 0x7afb(%rip), %xmm0 # 0x9140
mulss 0x3a13(%rip), %xmm0     # 0x5060
movss %xmm0, 0x7b2b(%rip)     # 0x9180
leaq 0x7ae8(%rip), %r15      # 0x9144
movq %r15, %rdi
callq 0x37ec
xorl %eax, %eax
movsd 0x3a0a(%rip), %xmm0     # 0x5078
cmpq $0xc, %rax
je 0x168e
cvtss2sd (%rax,%r12), %xmm1
mulsd %xmm0, %xmm1
cvtsd2ss %xmm1, %xmm1
movss %xmm1, (%rax,%r12)
addq $0x4, %rax
jmp 0x166e
movss 0x7ade(%rip), %xmm0     # 0x9174
movss 0x39c6(%rip), %xmm1     # 0x5064
ucomiss %xmm0, %xmm1
jbe 0x16ca
addss 0x39c5(%rip), %xmm0     # 0x5070
jmp 0x16ef
leaq 0x3d94(%rip), %rdi      # 0x5448
callq 0x1110
pushq $-0x1
popq %rax
addq $0x10, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq
movss 0x3996(%rip), %xmm1     # 0x5068
addss %xmm0, %xmm1
movss 0x398e(%rip), %xmm2     # 0x506c
cmpltss %xmm0, %xmm2
andps %xmm2, %xmm1
andnps %xmm0, %xmm2
orps %xmm1, %xmm2
movaps %xmm2, %xmm0
movss %xmm0, 0x7a7d(%rip)     # 0x9174
movss 0x7a79(%rip), %xmm0     # 0x9178
xorps 0x398a(%rip), %xmm0     # 0x5090
movss %xmm0, 0x7a6a(%rip)     # 0x9178
pushq $0x3
popq %rcx
xorl %eax, %eax
movsd 0x3965(%rip), %xmm0     # 0x5080
movsd 0x3965(%rip), %xmm1     # 0x5088
leaq 0x7a5a(%rip), %rdx      # 0x9184
leaq 0x7a5f(%rip), %rsi      # 0x9190
leaq 0x7a64(%rip), %rdi      # 0x919c
xorl %r8d, %r8d
subq $0x1, %rcx
jb 0x16bc
movswl (%rbx,%rcx,2), %r9d
cvtsi2sd %r9d, %xmm2
divsd %xmm0, %xmm2
divsd %xmm1, %xmm2
cvtsd2ss %xmm2, %xmm2
movswl (%r14,%rcx,2), %r9d
cvtsi2ss %r9d, %xmm3
movswl (%r15,%rcx,2), %r9d
cvtsi2ss %r9d, %xmm4
movss %xmm2, (%r8,%rdx)
movss %xmm3, (%r8,%rsi)
movss %xmm4, (%r8,%rdi)
addq $0x4, %r8
jmp 0x173b