// File: /workspace/github_c_makefile_star2_O2/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/inv_mpu_lib/inv_mpu.c
// Function: get_st_biases(int*, int*, unsigned char)
static int get_st_biases(int32_t *gyro, int32_t *accel, uint8_t hw_test)
{
	uint8_t data[MAX_PACKET_LENGTH];
	uint8_t packet_count, ii;
	uint16_t fifo_count;

	data[0] = 0x01;
	data[1] = 0;
	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
		return 1;
	delay_ms(200);
	data[0] = 0;
	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
		return 1;
	data[0] = BIT_FIFO_RST | BIT_DMP_RST;
	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
		return 1;
	delay_ms(15);
	data[0] = st.test->reg_lpf;
	if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
		return 1;
	data[0] = st.test->reg_rate_div;
	if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
		return 1;
	if (hw_test)
		data[0] = st.test->reg_gyro_fsr | 0xE0;
	else
		data[0] = st.test->reg_gyro_fsr;
	if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
		return 1;

	if (hw_test)
		data[0] = st.test->reg_accel_fsr | 0xE0;
	else
		data[0] = test.reg_accel_fsr;
	if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
		return 1;
	if (hw_test)
		delay_ms(200);

	/* Fill FIFO for test.wait_ms milliseconds. */
	data[0] = BIT_FIFO_EN;
	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
		return 1;

	data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
		return 1;
	delay_ms(test.wait_ms);
	data[0] = 0;
	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
		return 1;

	if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
		return 1;

	fifo_count = (data[0] << 8) | data[1];
	packet_count = fifo_count / MAX_PACKET_LENGTH;
	gyro[0] = gyro[1] = gyro[2] = 0;
	accel[0] = accel[1] = accel[2] = 0;

	for (ii = 0; ii < packet_count; ii++)
	{
		int16_t accel_cur[3], gyro_cur[3];
		if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
			return 1;
		accel_cur[0] = ((int16_t)data[0] << 8) | data[1];
		accel_cur[1] = ((int16_t)data[2] << 8) | data[3];
		accel_cur[2] = ((int16_t)data[4] << 8) | data[5];
		accel[0] += (int32_t)accel_cur[0];
		accel[1] += (int32_t)accel_cur[1];
		accel[2] += (int32_t)accel_cur[2];
		gyro_cur[0] = (((int16_t)data[6] << 8) | data[7]);
		gyro_cur[1] = (((int16_t)data[8] << 8) | data[9]);
		gyro_cur[2] = (((int16_t)data[10] << 8) | data[11]);
		gyro[0] += (int32_t)gyro_cur[0];
		gyro[1] += (int32_t)gyro_cur[1];
		gyro[2] += (int32_t)gyro_cur[2];
	}
#ifdef EMPL_NO_64BIT
	gyro[0] = (int32_t)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
	gyro[1] = (int32_t)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
	gyro[2] = (int32_t)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
	if (has_accel)
	{
		accel[0] = (int32_t)(((float)accel[0]*65536.f) / test.accel_sens /
												 packet_count);
		accel[1] = (int32_t)(((float)accel[1]*65536.f) / test.accel_sens /
												 packet_count);
		accel[2] = (int32_t)(((float)accel[2]*65536.f) / test.accel_sens /
												 packet_count);
		/* Don't remove gravity! */
		accel[2] -= 65536L;
	}
#else
	gyro[0] = (int32_t)(((int64_t)gyro[0]<<16) / test.gyro_sens / packet_count);
	gyro[1] = (int32_t)(((int64_t)gyro[1]<<16) / test.gyro_sens / packet_count);
	gyro[2] = (int32_t)(((int64_t)gyro[2]<<16) / test.gyro_sens / packet_count);
	accel[0] = (int32_t)(((int64_t)accel[0]<<16) / test.accel_sens /
											 packet_count);
	accel[1] = (int32_t)(((int64_t)accel[1]<<16) / test.accel_sens /
											 packet_count);
	accel[2] = (int32_t)(((int64_t)accel[2]<<16) / test.accel_sens /
											 packet_count);
	/* Don't remove gravity! */
	if (accel[2] > 0L)
		accel[2] -= 65536L;
	else
		accel[2] += 65536L;
#endif

	return 0;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x18, %rsp
movl %edx, %r15d
movq %rsi, %rbx
movq %rdi, %r14
leaq 0x8(%rsp), %rcx
movw $0x1, (%rcx)
movq 0x613b(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x6129(%rip), %rax      # 0x9208
movzbl 0x15(%rax), %esi
pushq $0x2
popq %rdx
callq 0x4b0e
testl %eax, %eax
je 0x30f7
pushq $0x1
popq %rbp
jmp 0x33b9
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10e0
leaq 0x8(%rsp), %rcx
movb $0x0, (%rcx)
movq 0x6100(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x60ee(%rip), %rax      # 0x9208
movzbl 0x11(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movq 0x60d9(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x60c7(%rip), %rax      # 0x9208
movzbl 0x5(%rax), %esi
pushq $0x1
popq %rbp
leaq 0x8(%rsp), %rcx
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movq 0x60ad(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x609b(%rip), %rax      # 0x9208
movzbl 0x15(%rax), %esi
pushq $0x1
popq %rbp
leaq 0x8(%rsp), %rcx
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movq 0x6081(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x606f(%rip), %rax      # 0x9208
movzbl 0x1a(%rax), %esi
pushq $0x1
popq %rbp
leaq 0x8(%rsp), %rcx
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movq 0x6055(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x6043(%rip), %rax      # 0x9208
movzbl 0x4(%rax), %esi
pushq $0x1
popq %rbp
leaq 0x8(%rsp), %rcx
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
leaq 0x8(%rsp), %rcx
movb $0xc, (%rcx)
movq 0x6021(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x600f(%rip), %rax      # 0x9208
movzbl 0x4(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movl $0x3a98, %edi           # imm = 0x3A98
callq 0x10e0
movq 0x5ff8(%rip), %rax      # 0x9218
movb 0x9(%rax), %al
leaq 0x8(%rsp), %rcx
movb %al, (%rcx)
movq 0x5fdf(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5fcd(%rip), %rax      # 0x9208
movzbl 0x2(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movq 0x5fc0(%rip), %rax      # 0x9218
movb 0x8(%rax), %al
leaq 0x8(%rsp), %rcx
movb %al, (%rcx)
movq 0x5fa7(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5f95(%rip), %rax      # 0x9208
movzbl 0x1(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movq 0x5f88(%rip), %rax      # 0x9218
movzbl 0xa(%rax), %eax
movl %eax, %ecx
orb $-0x20, %cl
testb %r15b, %r15b
movzbl %cl, %edx
cmovel %eax, %edx
leaq 0x8(%rsp), %rcx
movb %dl, (%rcx)
movq 0x5f60(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5f4e(%rip), %rax      # 0x9208
movzbl 0x6(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
testb %r15b, %r15b
je 0x32e3
movq 0x5f3c(%rip), %rax      # 0x9218
movb 0xb(%rax), %al
orb $-0x20, %al
jmp 0x32e9
movb 0x5efe(%rip), %al       # 0x91e7
leaq 0x8(%rsp), %rcx
movb %al, (%rcx)
movq 0x5f19(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5f07(%rip), %rax      # 0x9208
movzbl 0x7(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
testb %r15b, %r15b
je 0x3326
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10e0
leaq 0x8(%rsp), %rcx
movb $0x40, (%rcx)
movq 0x5edb(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5ec9(%rip), %rax      # 0x9208
movzbl 0x4(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
leaq 0x8(%rsp), %rcx
movb $0x78, (%rcx)
movq 0x5eb0(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5e9e(%rip), %rax      # 0x9208
movzbl 0x5(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
jne 0x33b9
movzwl 0x5e65(%rip), %eax      # 0x91e8
imull $0x3e8, %eax, %edi      # imm = 0x3E8
callq 0x10e0
leaq 0x8(%rsp), %rcx
movb $0x0, (%rcx)
movq 0x5e73(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5e61(%rip), %rax      # 0x9208
movzbl 0x5(%rax), %esi
pushq $0x1
popq %rbp
movl %ebp, %edx
callq 0x4b0e
testl %eax, %eax
je 0x33ca
movl %ebp, %eax
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x5e3f(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5e2d(%rip), %rax      # 0x9208
movzbl 0xc(%rax), %esi
pushq $0x2
popq %rdx
leaq 0x8(%rsp), %rcx
callq 0x4847
cmpb $-0x1, %al
je 0x33b9
leaq 0x8(%rsp), %rax
movzwl (%rax), %eax
rolw $0x8, %ax
movw $0xc, %cx
xorl %edx, %edx
divw %cx
movl %eax, %r12d
andl $0x0, 0x8(%r14)
andq $0x0, (%r14)
andl $0x0, 0x8(%rbx)
andq $0x0, (%rbx)
xorl %r15d, %r15d
movzbl %r12b, %eax
movl %eax, 0x14(%rsp)
pushq $0xc
popq %r13
cmpb %r12b, %r15b
jae 0x34de
movq 0x5dd8(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x5dc6(%rip), %rax      # 0x9208
movzbl 0xd(%rax), %esi
movl %r13d, %edx
leaq 0x8(%rsp), %rcx
callq 0x4847
cmpb $-0x1, %al
je 0x33b9
movzbl 0x8(%rsp), %eax
shll $0x8, %eax
movzbl 0x9(%rsp), %ecx
cwtl
orl %ecx, %eax
movzbl 0xa(%rsp), %ecx
shll $0x8, %ecx
movzbl 0xb(%rsp), %edx
movswl %cx, %ecx
orl %edx, %ecx
movzbl 0xc(%rsp), %edx
shll $0x8, %edx
movzbl 0xd(%rsp), %esi
movswl %dx, %edx
addl %eax, (%rbx)
orl %esi, %edx
addl %ecx, 0x4(%rbx)
addl %edx, 0x8(%rbx)
movzbl 0xe(%rsp), %eax
shll $0x8, %eax
movzbl 0xf(%rsp), %ecx
cwtl
orl %ecx, %eax
movzbl 0x10(%rsp), %ecx
shll $0x8, %ecx
movzbl 0x11(%rsp), %edx
movswl %cx, %ecx
orl %edx, %ecx
movzbl 0x12(%rsp), %edx
shll $0x8, %edx
movzbl 0x13(%rsp), %esi
movswl %dx, %edx
addl %eax, (%r14)
orl %esi, %edx
addl %ecx, 0x4(%r14)
addl %edx, 0x8(%r14)
incb %r15b
jmp 0x3428
movslq (%r14), %rax
shlq $0x10, %rax
movl 0x5cf1(%rip), %ecx      # 0x91dc
cqto
idivq %rcx
movzwl 0x14(%rsp), %ecx
cqto
idivq %rcx
movl %eax, (%r14)
movslq 0x4(%r14), %rax
shlq $0x10, %rax
movl 0x5cd1(%rip), %esi      # 0x91dc
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, 0x4(%r14)
movslq 0x8(%r14), %rax
shlq $0x10, %rax
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, 0x8(%r14)
movslq (%rbx), %rax
shlq $0x10, %rax
movl 0x5ca4(%rip), %esi      # 0x91e0
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, (%rbx)
movslq 0x4(%rbx), %rax
shlq $0x10, %rax
movl 0x5c8a(%rip), %esi      # 0x91e0
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, 0x4(%rbx)
movslq 0x8(%rbx), %rax
shlq $0x10, %rax
movl 0x5c6f(%rip), %esi      # 0x91e0
cqto
idivq %rsi
cqto
idivq %rcx
xorl %ecx, %ecx
testl %eax, %eax
setle %cl
shll $0x11, %ecx
addl %ecx, %eax
addl $0xffff0000, %eax       # imm = 0xFFFF0000
movl %eax, 0x8(%rbx)
xorl %ebp, %ebp
jmp 0x33b9