Overflow fix

rs2k · Apr 17, 2016 · 6ae0fac · 6ae0fac
1 parent 6d8296c
commit 6ae0fac
Showing 1 changed file with 31 additions and 6 deletions.
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
@@ -313,18 +313,43 @@ bool mpuGyroRead(int16_t *gyroADC)
 
     static int16_t last_data0[3] = {0, 0, 0};
     static int16_t last_data1[3] = {0, 0, 0};
+    int16_t current_data[3] = {0, 0, 0};
 
-    gyroADC[0] = ((int16_t)((data[0] << 8) | data[1]) + last_data0[0] + last_data1[0]) / 3;
-    gyroADC[1] = ((int16_t)((data[2] << 8) | data[3]) + last_data0[1] + last_data1[1]) / 3;
-    gyroADC[2] = ((int16_t)((data[4] << 8) | data[5]) + last_data0[2] + last_data1[2]) / 3;
+	current_data[0] = (int16_t)((data[0] << 8) | data[1]);
+	current_data[1] = (int16_t)((data[2] << 8) | data[3]);
+	current_data[2] = (int16_t)((data[4] << 8) | data[5]);
+
+	if ( (last_data0[0] - current_data[0]) >= 32767) {
+		current_data[0] = 32766;
+	}
+	if ( (last_data0[1] - current_data[1]) >= 32767) {
+		current_data[1] = 32766;
+	}
+	if ( (last_data0[2] - current_data[2]) >= 32767) {
+		current_data[2] = 32766;
+	}
+
+	if ( (-last_data0[0] + current_data[0]) >= 32767) {
+		current_data[0] = -32766;
+	}
+	if ( (-last_data0[1] + current_data[1]) >= 32767) {
+		current_data[1] = -32766;
+	}
+	if ( (-last_data0[2] + current_data[2]) >= 32767) {
+		current_data[2] = -32766;
+	}
+
+    gyroADC[0] = ((int16_t)(current_data[0] + last_data0[0] + last_data1[0])) / 3;
+    gyroADC[1] = ((int16_t)(current_data[1] + last_data0[1] + last_data1[1])) / 3;
+    gyroADC[2] = ((int16_t)(current_data[2] + last_data0[2] + last_data1[2])) / 3;
 
     last_data0[0] = last_data1[0];
     last_data0[1] = last_data1[1];
     last_data0[2] = last_data1[2];
 
-    last_data1[0] = (int16_t)((data[0] << 8) | data[1]);
-    last_data1[1] = (int16_t)((data[2] << 8) | data[3]);
-    last_data1[2] = (int16_t)((data[4] << 8) | data[5]);
+    last_data1[0] = current_data[0];
+    last_data1[1] = current_data[1];
+    last_data1[2] = current_data[2];
 
     return true;
 }