// File: /workspace/github_cpp_makefile_star2_O2/rachit31[P]CubeSatADCS/KalmanPi.cpp
// Function: initializeGyros()
void initializeGyros()
{
     // 119 Hz Sampling, 500 Degrees/second, 31 Hz LPF
	 i2c.writeByte(0x6b, 0x10, 0b01101011);
	 
	 const double pi = 3.141592653589793;
	 const double gyroToW = 500.0/32767.0*pi/180.0; // radians/second/count
	 /* const double gyroToW = 500/(pow(2,15)-1)*pi/180.0; // radians/second/count */
	 
	 uint8_t buffer[6];
	 int16_t data;
	 
	 SleepTimer myTimer(dtGyro);
	 
	 std::cout << "Hold still to initialize Gyro bias" << std::endl;
	
	 myTimer.initialize();
	
	 for (int i = 0; i < 2*119; i++)
	 {
		 i2c.readBuffer(0x6b, 0x18, buffer, 6);
		
		 data = (buffer[1] << 8) | buffer[0]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 Bw_B[0] += -1.0*gyroToW*data; // computing initial bias, converting to engineering units (radians/second)
		
		 data = (buffer[3] << 8) | buffer[2]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 Bw_B[1] += gyroToW*data; // computing initial bias, converting to engineering units (radians/second)
		
		 data = (buffer[5] << 8) | buffer[4]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 Bw_B[2] += gyroToW*data; // computing initial bias, converting to engineering units (radians/second)
		
		 myTimer.sleep();
	 }
	
	 Bw_B[0] = Bw_B[0]/(2.0*119.0);
	 Bw_B[1] = Bw_B[1]/(2.0*119.0);
	 Bw_B[2] = Bw_B[2]/(2.0*119.0);
	
	 std::cout << "Bw_B[0]: " << Bw_B[0] << std::endl;
	 std::cout << "Bw_B[1]: " << Bw_B[1] << std::endl;
	 std::cout << "Bw_B[2]: " << Bw_B[2] << std::endl;
	 
	 std::cout << "Gyro bias initialized" << std::endl;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
leaq 0x7d68(%rip), %rdi      # 0xa488
pushq $0x10
popq %rdx
pushq $0x6b
popq %rcx
movl %ecx, %esi
callq 0x2dae
leaq -0x50(%rbp), %r15
movsd 0x48cf(%rip), %xmm0     # 0x7008
movq %r15, %rdi
callq 0x3574
movq 0x7890(%rip), %rdi      # 0x9fd8
leaq 0x4919(%rip), %rsi      # 0x7068
callq 0x2220
movq %rax, %rdi
callq 0x2100
movq %r15, %rdi
callq 0x35f2
movl $0xee, %r14d
pushq $0x18
popq %r12
leaq -0x2e(%rbp), %r13
pushq $0x6
popq %rbx
subl $0x1, %r14d
jb 0x2804
leaq 0x7d02(%rip), %rdi      # 0xa488
pushq $0x6b
popq %rsi
movl %r12d, %edx
movq %r13, %rcx
movl %ebx, %r8d
callq 0x2fd2
movswl -0x2e(%rbp), %eax
cvtsi2sd %eax, %xmm0
mulsd 0x4869(%rip), %xmm0     # 0x7010
addsd 0x7c81(%rip), %xmm0     # 0xa430
movswl -0x2c(%rbp), %eax
cvtsi2sd %eax, %xmm1
movsd %xmm0, 0x7c71(%rip)     # 0xa430
movsd 0x4851(%rip), %xmm2     # 0x7018
mulsd %xmm2, %xmm1
addsd 0x7c65(%rip), %xmm1     # 0xa438
movsd %xmm1, 0x7c5d(%rip)     # 0xa438
movswl -0x2a(%rbp), %eax
cvtsi2sd %eax, %xmm0
mulsd %xmm2, %xmm0
addsd 0x7c51(%rip), %xmm0     # 0xa440
movsd %xmm0, 0x7c49(%rip)     # 0xa440
movq %r15, %rdi
callq 0x3604
jmp 0x2775
movsd 0x7c24(%rip), %xmm0     # 0xa430
movsd 0x480c(%rip), %xmm1     # 0x7020
divsd %xmm1, %xmm0
movsd %xmm0, 0x7c10(%rip)     # 0xa430
movsd 0x7c10(%rip), %xmm0     # 0xa438
divsd %xmm1, %xmm0
movsd %xmm0, 0x7c04(%rip)     # 0xa438
movsd 0x7c04(%rip), %xmm0     # 0xa440
divsd %xmm1, %xmm0
movsd %xmm0, 0x7bf8(%rip)     # 0xa440
movq 0x7789(%rip), %rbx      # 0x9fd8
leaq 0x4835(%rip), %rsi      # 0x708b
movq %rbx, %rdi
callq 0x2220
movsd 0x7bca(%rip), %xmm0     # 0xa430
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x4818(%rip), %rsi      # 0x7095
movq %rbx, %rdi
callq 0x2220
movsd 0x7bab(%rip), %xmm0     # 0xa438
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x47fb(%rip), %rsi      # 0x709f
movq %rbx, %rdi
callq 0x2220
movsd 0x7b8c(%rip), %xmm0     # 0xa440
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x47de(%rip), %rsi      # 0x70a9
movq %rbx, %rdi
callq 0x2220
movq %rax, %rdi
callq 0x2100
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq