// File: /workspace/github_c_makefile_star2_O3/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
cmpl $0x0, 0x8974(%rip)      # 0xa174
je 0x1965
leaq 0x8929(%rip), %rbx      # 0xa136
leaq 0x891c(%rip), %r14      # 0xa130
leaq 0x8925(%rip), %r15      # 0xa140
leaq 0x8962(%rip), %r12      # 0xa184
leaq 0x88b8(%rip), %r13      # 0xa0e1
movq %rbx, %rdi
movq %r14, %rsi
movq %r15, %rdx
movq %r12, %rcx
movq %r13, %r8
callq 0x4b3e
testb %al, %al
jne 0x1829
cvtdq2ps 0x88f8(%rip), %xmm0     # 0xa140
mulps 0x4821(%rip), %xmm0     # 0x6070
movaps %xmm0, 0x885a(%rip)     # 0xa0b0
leaq 0x8853(%rip), %rsi      # 0xa0b0
movaps 0x481c(%rip), %xmm3     # 0x6080
xorps %xmm0, %xmm3
movaps %xmm0, %xmm2
unpckhpd %xmm0, %xmm2            # xmm2 = xmm2[1],xmm0[1]
movaps %xmm0, %xmm4
shufps $0x55, %xmm0, %xmm4     # xmm4 = xmm4[1,1],xmm0[1,1]
movaps %xmm0, %xmm1
shufps $0xff, %xmm0, %xmm1     # xmm1 = xmm1[3,3],xmm0[3,3]
movaps %xmm0, %xmm5
shufps $0xaa, %xmm0, %xmm5     # xmm5 = xmm5[2,2],xmm0[2,2]
movaps %xmm2, %xmm6
movss %xmm3, %xmm6            # xmm6 = xmm3[0],xmm6[1,2,3]
mulps %xmm5, %xmm6
movaps %xmm0, %xmm3
shufps $0xe1, %xmm0, %xmm3     # xmm3 = xmm3[1,0],xmm0[2,3]
movaps %xmm0, %xmm5
shufps $0xe7, %xmm0, %xmm5     # xmm5 = xmm5[3,1],xmm0[2,3]
mulps %xmm3, %xmm5
addps %xmm6, %xmm5
addps %xmm5, %xmm5
movlps %xmm5, 0x88b5(%rip)     # 0xa160
leaq 0x88ae(%rip), %rdx      # 0xa160
mulss %xmm4, %xmm4
mulss %xmm0, %xmm0
subss %xmm4, %xmm0
mulss %xmm2, %xmm2
subss %xmm2, %xmm0
mulss %xmm1, %xmm1
addss %xmm0, %xmm1
movss %xmm1, 0x8892(%rip)     # 0xa168
leaq 0x88ab(%rip), %r12      # 0xa188
movq %r12, %rdi
callq 0x1aba
leaq 0x8864(%rip), %rdi      # 0xa150
callq 0x266e
xorps %xmm0, %xmm0
cvtsi2ssl 0x8854(%rip), %xmm0 # 0xa150
mulss 0x478c(%rip), %xmm0     # 0x6090
movss %xmm0, 0x8888(%rip)     # 0xa194
leaq 0x8841(%rip), %r15      # 0xa154
movq %r15, %rdi
callq 0x3c04
xorl %eax, %eax
movsd 0x4783(%rip), %xmm0     # 0x60a8
xorps %xmm1, %xmm1
cvtss2sd (%rax,%r12), %xmm1
mulsd %xmm0, %xmm1
cvtsd2ss %xmm1, %xmm1
movss %xmm1, (%rax,%r12)
addq $0x4, %rax
cmpq $0xc, %rax
jne 0x1925
movss 0x883a(%rip), %xmm0     # 0xa188
movss 0x473e(%rip), %xmm1     # 0x6094
ucomiss %xmm0, %xmm1
jbe 0x197b
addss 0x473d(%rip), %xmm0     # 0x60a0
jmp 0x19a0
leaq 0x4aec(%rip), %rdi      # 0x6458
callq 0x1110
movl $0xffffffff, %eax       # imm = 0xFFFFFFFF
jmp 0x1a3f
movss 0x4715(%rip), %xmm1     # 0x6098
addss %xmm0, %xmm1
movss 0x470d(%rip), %xmm2     # 0x609c
cmpltss %xmm0, %xmm2
andps %xmm2, %xmm1
andnps %xmm0, %xmm2
orps %xmm1, %xmm2
movaps %xmm2, %xmm0
movss %xmm0, 0x87e0(%rip)     # 0xa188
movss 0x87dc(%rip), %xmm0     # 0xa18c
xorps 0x46c9(%rip), %xmm0     # 0x6080
movss %xmm0, 0x87cd(%rip)     # 0xa18c
movl $0x3, %ecx
xorl %eax, %eax
movsd 0x46e2(%rip), %xmm0     # 0x60b0
movsd 0x46e2(%rip), %xmm1     # 0x60b8
leaq 0x87bb(%rip), %rdx      # 0xa198
leaq 0x87c0(%rip), %rsi      # 0xa1a4
leaq 0x87c5(%rip), %rdi      # 0xa1b0
xorl %r8d, %r8d
movswl -0x2(%rbx,%rcx,2), %r9d
xorps %xmm2, %xmm2
cvtsi2sd %r9d, %xmm2
divsd %xmm0, %xmm2
divsd %xmm1, %xmm2
cvtsd2ss %xmm2, %xmm2
movss %xmm2, (%r8,%rdx)
movswl -0x2(%r14,%rcx,2), %r9d
xorps %xmm2, %xmm2
cvtsi2ss %r9d, %xmm2
movswl -0x2(%r15,%rcx,2), %r9d
xorps %xmm3, %xmm3
cvtsi2ss %r9d, %xmm3
movss %xmm2, (%r8,%rsi)
movss %xmm3, (%r8,%rdi)
addq $0x4, %r8
decq %rcx
jne 0x19ee
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq