diff --git a/Source/BSP/Inc/imu.h b/Source/BSP/Inc/imu.h
index 71ac265..9746de6 100644
--- a/Source/BSP/Inc/imu.h
+++ b/Source/BSP/Inc/imu.h
@@ -5,18 +5,19 @@
 
 #include "stm32g431xx.h"
 
-#define ICM_ADDR				0x68
+#define ICM_ADDR				    0x68
 #define REG_PWR_MGMT_1			0x06
-#define REG_BANK_SEL			0x7F
+#define REG_BANK_SEL			  0x7F
 #define REG_GYRO_CONFIG_1		0x01
 #define REG_ACCEL_CONFIG		0x14
-#define IMU_DT					0.002f   // 2 ms
+#define IMU_DT					    0.002f   // 2 ms
 
 #define GYRO_FS_SEL_2000		3 << 1
 #define GYRO_DLPFCFG_73			3 << 3
 #define GYRO_FCHOICE_ON			1
 #define GYRO_FCHOICE_OFF		0
 
+#define ACCEL_FS_SEL_4			1 << 1
 #define ACCEL_FS_SEL_8			2 << 1
 #define ACCEL_DLPFCFG_69		3 << 3
 #define ACCEL_FCHOICE_ON		1
@@ -24,8 +25,8 @@
 
 typedef struct
 {
-	float ax, ay, az; // lsb
-	float gx, gy, gz; // lsb
+	int16_t ax, ay, az; // lsb
+	int16_t gx, gy, gz; // lsb
 } imu_raw_t;
 
 void imu_pow_init();
diff --git a/Source/BSP/Inc/imu_processing.h b/Source/BSP/Inc/imu_processing.h
index a92a0f7..0b40e71 100644
--- a/Source/BSP/Inc/imu_processing.h
+++ b/Source/BSP/Inc/imu_processing.h
@@ -4,10 +4,10 @@
 #include "imu.h"
 
 
-#define ACCEL_SENS_SCALE_FACTOR		8192.0f
+#define ACCEL_SENS_SCALE_FACTOR		4096.0f
 #define GYRO_SENS_SCALE_FACTOR		16.4f
-#define PI							3.14159265359f
-#define DEG2RAD						PI / 180.0f
+#define PI							          3.14159265359f
+#define DEG2RAD						        PI / 180.0f
 
 typedef struct
 {
@@ -19,6 +19,7 @@ void imu_processing_init();
 
 void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib);
 void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax);
+uint16_t Rev16(uint16_t v);
 
 void imu_calibrate();
 
diff --git a/Source/BSP/Src/imu.c b/Source/BSP/Src/imu.c
index fa7cf6a..59577ba 100644
--- a/Source/BSP/Src/imu.c
+++ b/Source/BSP/Src/imu.c
@@ -23,6 +23,9 @@ void i2c_gpio_init()
 	GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
 	GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
 	
+  // high speed
+  GPIOB->OSPEEDR |= (2 << (8 * 2)) | (2 << (9 * 2));
+  
 	// enable open-drain
 	GPIOB->OTYPER |= (1 << 8) | (1 << 9);
 	
@@ -36,8 +39,10 @@ void i2c1_init()
 	// enable I2C1
 	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
 	
+  // 0x00303D5B - 100 kHz
+  
 	// 400 kHz @ 16 MHz
-	I2C1->TIMINGR = 0x00303D5B;
+	I2C1->TIMINGR = 0x10802D9BUL;
 	
 	I2C1->CR1 |= I2C_CR1_PE;
 }
@@ -56,7 +61,7 @@ void imu_init()
 	// gyro ~2000 dps, FS_SEL = 3
 	i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON);
 	
-	// accel ~4g, FS_SEL = 1
+	// accel 8g, FS_SEL = 2
 	i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON);
 	
 	// back to bank 0
@@ -127,8 +132,8 @@ void imu_read_raw(imu_raw_t* data)
 	data->ay = (buf[2] << 8) | buf[3];
 	data->az = (buf[4] << 8) | buf[5];
 	
-	data->gx = (buf[6] << 8) | buf[7];
-	data->gy = (buf[8] << 8) | buf[9];
+	data->gx = (buf[6] << 8)  | buf[7];
+	data->gy = (buf[8] << 8)  | buf[9];
 	data->gz = (buf[10] << 8) | buf[11];
 }
 
diff --git a/Source/BSP/Src/imu_processing.c b/Source/BSP/Src/imu_processing.c
index e7ba4ae..5dff213 100644
--- a/Source/BSP/Src/imu_processing.c
+++ b/Source/BSP/Src/imu_processing.c
@@ -4,13 +4,13 @@
 
 
 
-static float gyro_bias_x = 0.0f;
+/*static float gyro_bias_x = 0.0f;
 static float gyro_bias_y = 0.0f;
 static float gyro_bias_z = 0.0f;
 
 static float accel_bias_x = 0.0f;
 static float accel_bias_y = 0.0f;
-static float accel_bias_z = 0.0f;
+static float accel_bias_z = 0.0f;*/
 
 static biquad_t accel_x_lpf;
 static biquad_t accel_y_lpf;
@@ -20,8 +20,8 @@ static biquad_t gyro_x_lpf;
 static biquad_t gyro_y_lpf;
 static biquad_t gyro_z_lpf;
 
-const float accel_min = -8192.0f;
-const float accel_max = 8192.0f;
+const float accel_min = -4096.0f;
+const float accel_max = 4096.0f;
 
 static int16_t x_calib;
 static int16_t y_calib;
@@ -46,25 +46,15 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
 	
 	imu_read_raw(&raw);
 	
-	raw.ax = biquad_apply(&accel_x_lpf, raw.ax);
-	raw.ay = biquad_apply(&accel_y_lpf, raw.ay);
-	raw.az = biquad_apply(&accel_z_lpf, raw.az);
+	raw.ax = (uint16_t) biquad_apply(&accel_x_lpf, raw.ax);
+	raw.ay = (uint16_t) biquad_apply(&accel_y_lpf, raw.ay);
+	raw.az = (uint16_t) biquad_apply(&accel_z_lpf, raw.az);
 	
-	raw.gx = biquad_apply(&gyro_x_lpf, raw.gx);
-	raw.gy = biquad_apply(&gyro_y_lpf, raw.gy);
-	raw.gz = biquad_apply(&gyro_z_lpf, raw.gz);
+	raw.gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw.gx);
+	raw.gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw.gy);
+	raw.gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw.gz);
 	
-	if (allowed_calib) imu_calib(&raw, 50, 8192, 1000);
-	
-	/*float alpha = 0.01;
-	
-	out->ax = out->ax * (1.0f - alpha) + raw.ax * alpha;
-	out->ay = out->ay * (1.0f - alpha) + raw.ay * alpha;
-	out->az = out->az * (1.0f - alpha) + raw.az * alpha;
-	
-	out->gx = out->gx * (1.0f - alpha) + raw.gx * alpha;
-	out->gy = out->gy * (1.0f - alpha) + raw.gy * alpha;
-	out->gz = out->gz * (1.0f - alpha) + raw.gz * alpha;*/
+	if (allowed_calib) imu_calib(&raw, 50, 4096, 500);
 	
 	out->ax = normalize(raw.ax, 1.0f, accel_min, accel_max);
 	out->ay = normalize(raw.ay, 1.0f, accel_min, accel_max);
@@ -75,21 +65,38 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
 	out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
 	
 	////////////////////////////////////////////////////////////
-	/*out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
-	out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
-	out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
+  
+  /*raw.ax = Rev16(raw.ax);
+  raw.ay = Rev16(raw.ay);
+  raw.az = Rev16(raw.az);
+  
+  raw.gx = Rev16(raw.gx);
+  raw.gy = Rev16(raw.gy);
+  raw.gz = Rev16(raw.gz);
+  
+	out->ax = raw.ax - accel_bias_x;
+	out->ay = raw.ay - accel_bias_y;
+	out->az = raw.az - accel_bias_z;
 	
-	out->ax = biquad_apply(&accel_x_lpf, out->ax);
+	out->gx = raw.gx - gyro_bias_x;
+	out->gy = raw.gy - gyro_bias_y;
+	out->gz = raw.gz - gyro_bias_z;
+  
+  out->ax = biquad_apply(&accel_x_lpf, out->ax);
 	out->ay = biquad_apply(&accel_y_lpf, out->ay);
 	out->az = biquad_apply(&accel_z_lpf, out->az);
 	
-	out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
-	out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
-	out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
-	
 	out->gx = biquad_apply(&gyro_x_lpf, out->gx);
 	out->gy = biquad_apply(&gyro_y_lpf, out->gy);
-	out->gz = biquad_apply(&gyro_z_lpf, out->gz);*/
+	out->gz = biquad_apply(&gyro_z_lpf, out->gz);
+  
+  out->ax /= ACCEL_SENS_SCALE_FACTOR;
+  out->ay /= ACCEL_SENS_SCALE_FACTOR;
+  out->az /= ACCEL_SENS_SCALE_FACTOR;
+  
+  out->gx /= GYRO_SENS_SCALE_FACTOR;
+  out->gy /= GYRO_SENS_SCALE_FACTOR;
+  out->gz /= GYRO_SENS_SCALE_FACTOR;*/
 }
 
 void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
@@ -118,7 +125,13 @@ void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
 	acc = acc * (1.0f - alpha) + len * alpha;
 }
 
-void imu_calibrate()
+uint16_t Rev16(uint16_t v)
+{
+  asm("REV16 %1, %0" : "=r" (v) : "r" (v));
+  return v;
+}
+
+/*void imu_calibrate()
 {
 	const int samples = 1000;
 	
@@ -136,25 +149,25 @@ void imu_calibrate()
 	{
 		imu_read_raw(&imu);
 		
-		sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
-		sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
-		sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
+		sum_ax += imu.ax;
+		sum_ay += imu.ay;
+		sum_az += imu.az;
 		
-		sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
-		sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
-		sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
+		sum_gx += imu.gx;
+		sum_gy += imu.gy;
+		sum_gz += imu.gz;
 		
 		for (volatile uint16_t d = 0; d < 5000; ++d);
 	}
 	
 	accel_bias_x = sum_ax / samples;
 	accel_bias_y = sum_ay / samples;
-	accel_bias_z = (sum_az / samples) + 1.0f;
+	accel_bias_z = sum_az / samples;
 	
 	gyro_bias_x = sum_gx / samples;
 	gyro_bias_y = sum_gy / samples;
 	gyro_bias_z = sum_gz / samples;
-}
+}*/
 
 float normalize(float value, float scale, float min, float max)
 {
diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c
index a18f290..5481f23 100644
--- a/Source/INS/IRS.c
+++ b/Source/INS/IRS.c
@@ -27,19 +27,17 @@ void IRS_update(IRS* irs, float dt)
 	irs->q = QuatSum(&irs->q, &g);
 	irs->q = QuatNormalize(&irs->q, 1.0f);
 	
-    // /gyro update
+  // /gyro update
 	
 	// accel update
-	
-	Vector3 accel = {irs->accel.x, irs->accel.y, irs->accel.z};
-	restoreQuat(irs, &accel);
+	restoreQuat(irs);
 	
 	// /accel update
 }
 
-void restoreQuat(IRS* irs, const Vector3* accel)
+void restoreQuat(IRS* irs)
 {
-	float len = lengthV3(accel);
+	float len = lengthV3(&irs->accel);
 	
 	static float quat_acc_alpha = 0.03f;
 	static float quat_acc_max = 0.02f;
@@ -51,7 +49,7 @@ void restoreQuat(IRS* irs, const Vector3* accel)
 	
 	if (gain < 0.0001f) return;
 	
-	Vector3 acc = normalizeV3(accel, 1.0f);
+	Vector3 acc = normalizeV3(&irs->accel, 1.0f);
 	
 	Vector3 est = IRS_getGravity(&irs->q);
 	
diff --git a/Source/INS/IRS.h b/Source/INS/IRS.h
index 4a40770..e958878 100644
--- a/Source/INS/IRS.h
+++ b/Source/INS/IRS.h
@@ -21,7 +21,7 @@ typedef struct
 void IRS_init(IRS* irs);
 
 void IRS_update(IRS* irs, float dt);
-void restoreQuat(IRS* irs, const Vector3* accel);
+void restoreQuat(IRS* irs);
 
 void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw);
 
diff --git a/Source/INS/geometry/quaternion.c b/Source/INS/geometry/quaternion.c
index a4eaddd..3bb8a46 100644
--- a/Source/INS/geometry/quaternion.c
+++ b/Source/INS/geometry/quaternion.c
@@ -5,7 +5,7 @@
 
 Quaternion QuatNormalize(const Quaternion* q, const float gain)
 {
-  Quaternion res = {};
+  Quaternion res = {0.0f, 0.0f, 0.0f, 0.0f};
 	
 	float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
 	
@@ -70,7 +70,7 @@ Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
 
 Quaternion QuatConstProd(const Quaternion* q, const float value)
 {
-	Quaternion res = {.x = q->x * value, .y = q->y * value, .z = q->z * value, .w = q->w * value};
+	Quaternion res = {q->x * value, q->y * value, q->z * value, q->w * value};
 	return res;
 }
 
@@ -83,8 +83,6 @@ Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
 		q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
 		q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
 	};
-	
-	
 	return res;
 }
 
diff --git a/Source/main.c b/Source/main.c
index 8b77777..dfc59d6 100644
--- a/Source/main.c
+++ b/Source/main.c
@@ -40,7 +40,7 @@ int main(void)
 	imu_init();
 	
 	imu_processing_init();
-	imu_calibrate();
+	//imu_calibrate();
 	
 	imu_tim6_init();
 	
@@ -59,7 +59,7 @@ int main(void)
 		
 		if (irs_update_flag)
 		{
-		    irs_update_flag = 0;
+		  irs_update_flag = 0;
 			
 			imu_read_scaled(&imu, &allowed_calib);
 			
@@ -71,34 +71,34 @@ int main(void)
 			irs.accel.y = imu.ay;
 			irs.accel.z = imu.az;
 		
-		    IRS_update(&irs, IMU_DT);
+		  IRS_update(&irs, IMU_DT);
 		}
 		
 		euler = QuatToEuler(&irs.q);
 		
 		if (control_update_flag)
 		{
-		    control_update_flag = 0;
-		
-		    attitude_controller_update(
-		        &ctrl_chs,
-		        &rx_chs_normalized,
-		        &irs.q,
-		        &irs.gyro
-		    );
+      control_update_flag = 0;
+      
+      attitude_controller_update(
+        &ctrl_chs,
+        &rx_chs_normalized,
+        &irs.q,
+        &irs.gyro
+      );
 		}
 		
 		if (rx_chs_normalized.rc_armed)
-	    {
-			allowed_calib = 1;
-			
-	        motors_set_throttle_mix(
-	            rx_chs_normalized.rc_throttle,
-	            &ctrl_chs,
-	            rx_chs_normalized.rc_armed
-	        );
-	    }
-	    else
+	  {
+      allowed_calib = 1;
+      
+      motors_set_throttle_mix(
+        rx_chs_normalized.rc_throttle,
+        &ctrl_chs,
+        rx_chs_normalized.rc_armed
+      );
+	  }
+	  else
 		{
 			motors_turn_off();
 			allowed_calib = 0;