The unified diff between revisions [d0420ebd..] and [64de686d..] 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 [d0420ebd87c820e33a32b29727989516e15980a8] # new_revision [64de686d701acb9539dc52fe0bff299405612ab0] # # patch "wmp.c" # from [022e72f07353e280c050ae668cb3c02b318c72c4] # to [e6627850f4ae970c9aee0dbb4c6f1933f0cb31c7] # ============================================================ --- wmp.c 022e72f07353e280c050ae668cb3c02b318c72c4 +++ wmp.c e6627850f4ae970c9aee0dbb4c6f1933f0cb31c7 @@ -6,6 +6,9 @@ #include "dcm.h" #include "fisqrt.h" #include "stick.h" +#include "watchdog.h" +#include "status.h" +#include "abs.h" #define WMP_ZERO_COUNT 100 @@ -134,6 +137,9 @@ unsigned int wmp_discard; #define FAST_PITCH_STEP (4 / DEG_TO_RAD) #define FAST_ROLL_STEP (4 / DEG_TO_RAD) +/* The gyro has to stay within this limit in each axis in order to arm */ +#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD) + bool wmp_sample(void) { if (!i2c_start_transaction(&wmp_sample_transaction)) @@ -202,6 +208,16 @@ void wmp_process_gyro_sample(void) dcm_update(roll, pitch, yaw); + if (!status_armed()) { + if ( (abs(roll) < GYRO_RATE_THRESHOLD) && + (abs(pitch) < GYRO_RATE_THRESHOLD) && + (abs(yaw) < GYRO_RATE_THRESHOLD) ) { + status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE); + } else { + status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE); + } + } + wmp_generation++; #if SEND_DCM @@ -225,9 +241,11 @@ void wmp_process_gyro_sample(void) wmp_pitch_zero /= WMP_ZERO_COUNT; wmp_roll_zero /= WMP_ZERO_COUNT; putstr("Zero finished\r\n"); + status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE); } } } + watchdog_kick(WATCHDOG_GYRO); } void wmp_process_accel_sample(void) @@ -275,6 +293,7 @@ void wmp_process_accel_sample(void) * on that so we'll just fudge it here. */ dcm_drift_correction(x, -y, -z); + watchdog_kick(WATCHDOG_ACCEL); stick_input(); }