The unified diff between revisions [23a3e9a5..] and [81e4dce2..] is displayed below. It can also be downloaded as a raw diff.

This diff has been restricted to the following files: 'wmp.c'

#
# old_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd]
# new_revision [81e4dce274e79dd9187ed4bd182e1d6fc0fdfb37]
#
# patch "wmp.c"
#  from [1ab4017652548447277e83cf1697442cb7c6949b]
#    to [022e72f07353e280c050ae668cb3c02b318c72c4]
#
============================================================
--- wmp.c	1ab4017652548447277e83cf1697442cb7c6949b
+++ wmp.c	022e72f07353e280c050ae668cb3c02b318c72c4
@@ -1,9 +1,11 @@
+/* wmp.c */
 
 #include "wmp.h"
 #include "i2c.h"
 #include "uart.h"
 #include "dcm.h"
 #include "fisqrt.h"
+#include "stick.h"
 
 #define WMP_ZERO_COUNT 100
 
@@ -115,6 +117,7 @@ bool wmp_zero;
 
 bool wmp_update;
 bool wmp_zero;
+unsigned int wmp_discard;
 
 #define TWO_PI 6.28318531f
 #define DEG_TO_RAD (TWO_PI/360.0f)
@@ -201,24 +204,28 @@ void wmp_process_gyro_sample(void)
 
 		wmp_generation++;
 
-#if 1
-		if ((wmp_generation % 2) == 0)
+#if SEND_DCM
+		if ((wmp_generation % 20) == 0)
 			dcm_send_packet();
 #endif
 
 	} else if (wmp_zero) {
-		wmp_yaw_zero += wmp_yaw;
-		wmp_pitch_zero += wmp_pitch;
-		wmp_roll_zero += wmp_roll;
-		wmp_generation++;
-		if (wmp_generation >= WMP_ZERO_COUNT) {
-			wmp_zero = FALSE;
-			wmp_update = TRUE;
-			wmp_generation = 0;
-			wmp_yaw_zero /= WMP_ZERO_COUNT;
-			wmp_pitch_zero /= WMP_ZERO_COUNT;
-			wmp_roll_zero /= WMP_ZERO_COUNT;
-			putstr("Zero finished\r\n");
+		if (wmp_discard) {
+			wmp_discard--;
+		} else {
+			wmp_yaw_zero += wmp_yaw;
+			wmp_pitch_zero += wmp_pitch;
+			wmp_roll_zero += wmp_roll;
+			wmp_generation++;
+			if (wmp_generation >= WMP_ZERO_COUNT) {
+				wmp_zero = FALSE;
+				wmp_update = TRUE;
+				wmp_generation = 0;
+				wmp_yaw_zero /= WMP_ZERO_COUNT;
+				wmp_pitch_zero /= WMP_ZERO_COUNT;
+				wmp_roll_zero /= WMP_ZERO_COUNT;
+				putstr("Zero finished\r\n");
+			}
 		}
 	}
 }
@@ -268,6 +275,7 @@ void wmp_process_accel_sample(void)
 	 * on that so we'll just fudge it here.
 	 */
 	dcm_drift_correction(x, -y, -z);
+	stick_input();
 }
 
 void wmp_event_handler(void)
@@ -287,6 +295,7 @@ void wmp_start_zero(void)
 {
 	wmp_zero = TRUE;
 	wmp_update = FALSE;
+	wmp_discard = 100;
 	wmp_generation = 0;
 	putstr("Starting zero\r\n");
 }