diff --git a/src/main.cpp b/src/main.cpp
index e4b1fde..1a55b56 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -77,15 +77,15 @@ float Mxyz[3];
 // These are the free parameters in the Mahony filter and fusion scheme,
 // Kp for proportional feedback, Ki for integral
 // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
-#define Kp 30.0
-#define Ki 0.0
+#define Kp 8.0
+#define Ki 0.1
 
 // globals for AHRS loop timing
 
 unsigned long now = 0, last = 0; //micros() timers
 float deltat = 0;  //loop time in seconds
 unsigned long now_ms, last_ms = 0; //millis() timers
-unsigned long update_ms = 200; //send updates every "update_ms" milliseconds
+unsigned long update_ms = 20; //send updates every "update_ms" milliseconds
 
 
 // Vector to hold quaternion