// File: /workspace/github_c_makefile_star2_O0/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 %rbp
movq %rsp, %rbp
subq $0x30, %rsp
cmpl $0x0, 0xa9f1(%rip)      # 0xc160
jne 0x178b
leaq 0x6cde(%rip), %rdi      # 0x8456
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x19f0
jmp 0x178d
leaq 0xa992(%rip), %rdi      # 0xc126
leaq 0xa985(%rip), %rsi      # 0xc120
leaq 0xa98e(%rip), %rdx      # 0xc130
leaq 0xa9c7(%rip), %rcx      # 0xc170
leaq 0xa921(%rip), %r8       # 0xc0d1
callq 0x6740
movzbl %al, %eax
cmpl $0x0, %eax
je 0x17bf
jmp 0x178d
leaq -0x14(%rbp), %rdi
leaq 0xa966(%rip), %rsi      # 0xc130
callq 0x1d10
movq -0x14(%rbp), %rax
movq %rax, 0xa9a6(%rip)      # 0xc180
movq -0xc(%rbp), %rax
movq %rax, 0xa9a3(%rip)      # 0xc188
leaq 0xa960(%rip), %rdi      # 0xc14c
leaq 0xa98d(%rip), %rsi      # 0xc180
callq 0x1a00
leaq 0xa975(%rip), %rdi      # 0xc174
leaq 0xa97a(%rip), %rsi      # 0xc180
leaq 0xa93f(%rip), %rdx      # 0xc14c
callq 0x1b20
leaq 0xa927(%rip), %rdi      # 0xc140
callq 0x2f80
cvtsi2ssl 0xa91a(%rip), %xmm0 # 0xc140
movss 0x683a(%rip), %xmm1     # 0x8068
divss %xmm1, %xmm0
movss %xmm0, 0xa956(%rip)     # 0xc190
leaq 0xa903(%rip), %rdi      # 0xc144
callq 0x5010
movl $0x0, -0x18(%rbp)
cmpl $0x3, -0x18(%rbp)
jge 0x188e
movslq -0x18(%rbp), %rcx
leaq 0xa916(%rip), %rax      # 0xc174
movss (%rax,%rcx,4), %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x6829(%rip), %xmm1     # 0x8098
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
leaq 0xa8f6(%rip), %rax      # 0xc174
movss %xmm0, (%rax,%rcx,4)
movl -0x18(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x18(%rbp)
jmp 0x184d
movss 0x67d6(%rip), %xmm0     # 0x806c
ucomiss 0xa8d7(%rip), %xmm0     # 0xc174
jbe 0x18b6
movss 0x67cd(%rip), %xmm0     # 0x8074
addss 0xa8c5(%rip), %xmm0     # 0xc174
movss %xmm0, -0x20(%rbp)
jmp 0x18fd
movss 0xa8b6(%rip), %xmm0     # 0xc174
movss 0x67aa(%rip), %xmm1     # 0x8070
ucomiss %xmm1, %xmm0
jbe 0x18e6
movss 0xa8a1(%rip), %xmm0     # 0xc174
movss 0x6799(%rip), %xmm1     # 0x8074
subss %xmm1, %xmm0
movss %xmm0, -0x24(%rbp)
jmp 0x18f3
movss 0xa886(%rip), %xmm0     # 0xc174
movss %xmm0, -0x24(%rbp)
movss -0x24(%rbp), %xmm0
movss %xmm0, -0x20(%rbp)
movss -0x20(%rbp), %xmm0
movss %xmm0, 0xa86a(%rip)     # 0xc174
movss 0xa866(%rip), %xmm0     # 0xc178
cvtss2sd %xmm0, %xmm0
movsd 0x6762(%rip), %xmm1     # 0x8080
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xa84a(%rip)     # 0xc178
movl $0x0, -0x1c(%rbp)
cmpl $0x3, -0x1c(%rbp)
jge 0x19e9
movslq -0x1c(%rbp), %rdx
movl %edx, %esi
movl $0x2, %eax
movl %eax, %ecx
subl %esi, %ecx
movslq %ecx, %rsi
leaq 0xa7ce(%rip), %rcx      # 0xc126
movswl (%rcx,%rsi,2), %ecx
cvtsi2ss %ecx, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x671c(%rip), %xmm1     # 0x8088
divsd %xmm1, %xmm0
movsd 0x6718(%rip), %xmm1     # 0x8090
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
leaq 0xa80d(%rip), %rcx      # 0xc194
movss %xmm0, (%rcx,%rdx,4)
movslq -0x1c(%rbp), %rdx
movl %edx, %esi
movl %eax, %ecx
subl %esi, %ecx
movslq %ecx, %rsi
leaq 0xa780(%rip), %rcx      # 0xc120
movswl (%rcx,%rsi,2), %ecx
cvtsi2ss %ecx, %xmm0
leaq 0xa7f1(%rip), %rcx      # 0xc1a0
movss %xmm0, (%rcx,%rdx,4)
movl -0x1c(%rbp), %ecx
subl %ecx, %eax
movslq %eax, %rcx
leaq 0xa781(%rip), %rax      # 0xc144
movswl (%rax,%rcx,2), %eax
cvtsi2ss %eax, %xmm0
movslq -0x1c(%rbp), %rcx
leaq 0xa7d6(%rip), %rax      # 0xc1ac
movss %xmm0, (%rax,%rcx,4)
movl -0x1c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x1c(%rbp)
jmp 0x1935
movl $0x0, -0x4(%rbp)
movl -0x4(%rbp), %eax
addq $0x30, %rsp
popq %rbp
retq
nopl (%rax)