// File: /workspace/github_c_makefile_star2_O1/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, 0x8963(%rip)      # 0xa170
je 0x19a3
leaq 0x891c(%rip), %rbx      # 0xa136
leaq 0x890f(%rip), %r14      # 0xa130
leaq 0x8918(%rip), %r15      # 0xa140
leaq 0x8951(%rip), %r12      # 0xa180
leaq 0x88ab(%rip), %r13      # 0xa0e1
movq %rbx, %rdi
movq %r14, %rsi
movq %r15, %rdx
movq %r12, %rcx
movq %r13, %r8
callq 0x4ac2
testb %al, %al
jne 0x1836
cvtsi2ssl 0x88ea(%rip), %xmm0 # 0xa140
movss 0x4802(%rip), %xmm4     # 0x6060
mulss %xmm4, %xmm0
cvtsi2ssl 0x88da(%rip), %xmm3 # 0xa144
mulss %xmm4, %xmm3
cvtsi2ssl 0x88d2(%rip), %xmm2 # 0xa148
mulss %xmm4, %xmm2
cvtsi2ssl 0x88ca(%rip), %xmm1 # 0xa14c
mulss %xmm4, %xmm1
movss %xmm0, 0x8822(%rip)     # 0xa0b0
leaq 0x881b(%rip), %rsi      # 0xa0b0
movss %xmm3, 0x8817(%rip)     # 0xa0b4
movss %xmm2, 0x8813(%rip)     # 0xa0b8
movss %xmm1, 0x880f(%rip)     # 0xa0bc
movaps %xmm2, %xmm4
mulss %xmm0, %xmm4
movaps %xmm3, %xmm5
mulss %xmm1, %xmm5
subss %xmm4, %xmm5
addss %xmm5, %xmm5
movss %xmm5, 0x8891(%rip)     # 0xa15c
leaq 0x888a(%rip), %rdx      # 0xa15c
movaps %xmm2, %xmm4
mulss %xmm1, %xmm4
movaps %xmm0, %xmm5
mulss %xmm3, %xmm5
addss %xmm4, %xmm5
addss %xmm5, %xmm5
movss %xmm5, 0x8870(%rip)     # 0xa160
mulss %xmm3, %xmm3
mulss %xmm0, %xmm0
subss %xmm3, %xmm0
mulss %xmm2, %xmm2
subss %xmm2, %xmm0
mulss %xmm1, %xmm1
addss %xmm0, %xmm1
movss %xmm1, 0x8850(%rip)     # 0xa164
leaq 0x8869(%rip), %r12      # 0xa184
movq %r12, %rdi
callq 0x1af3
leaq 0x8826(%rip), %rdi      # 0xa150
callq 0x26c8
xorps %xmm0, %xmm0
cvtsi2ssl 0x8816(%rip), %xmm0 # 0xa150
mulss 0x4722(%rip), %xmm0     # 0x6064
movss %xmm0, 0x8846(%rip)     # 0xa190
leaq 0x8803(%rip), %r15      # 0xa154
movq %r15, %rdi
callq 0x3c0f
xorl %eax, %eax
movsd 0x471d(%rip), %xmm0     # 0x6080
xorps %xmm1, %xmm1
cvtss2sd (%rax,%r12), %xmm1
mulsd %xmm0, %xmm1
cvtsd2ss %xmm1, %xmm1
movss %xmm1, (%rax,%r12)
addq $0x4, %rax
cmpq $0xc, %rax
jne 0x1963
movss 0x87f8(%rip), %xmm0     # 0xa184
movss 0x46d4(%rip), %xmm1     # 0x6068
ucomiss %xmm0, %xmm1
jbe 0x19b9
addss 0x46d3(%rip), %xmm0     # 0x6074
jmp 0x19de
leaq 0x4a9e(%rip), %rdi      # 0x6448
callq 0x1110
movl $0xffffffff, %eax       # imm = 0xFFFFFFFF
jmp 0x1a7d
movss 0x46ab(%rip), %xmm1     # 0x606c
addss %xmm0, %xmm1
movss 0x46a3(%rip), %xmm2     # 0x6070
cmpltss %xmm0, %xmm2
andps %xmm2, %xmm1
andnps %xmm0, %xmm2
orps %xmm1, %xmm2
movaps %xmm2, %xmm0
movss %xmm0, 0x879e(%rip)     # 0xa184
movss 0x879a(%rip), %xmm0     # 0xa188
xorps 0x46ab(%rip), %xmm0     # 0x60a0
movss %xmm0, 0x878b(%rip)     # 0xa188
movl $0x3, %ecx
xorl %eax, %eax
movsd 0x467c(%rip), %xmm0     # 0x6088
movsd 0x467c(%rip), %xmm1     # 0x6090
leaq 0x8779(%rip), %rdx      # 0xa194
leaq 0x877e(%rip), %rsi      # 0xa1a0
leaq 0x8783(%rip), %rdi      # 0xa1ac
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 0x1a2c
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq