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"); }