The unified diff between revisions [4f22e7ef..] and [bfc9e27f..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'dcm.c'
# # old_revision [4f22e7ef7d3064e3b51a5b868a4722f3f13c747b] # new_revision [bfc9e27f5c40da31ae4269394aaf0545e5856a70] # # patch "dcm.c" # from [90d94d47a262c72d472ee7aca36bb55e2b328e66] # to [62d414448bb944679d6e5a3e693736501a28a009] # ============================================================ --- dcm.c 90d94d47a262c72d472ee7aca36bb55e2b328e66 +++ dcm.c 62d414448bb944679d6e5a3e693736501a28a009 @@ -11,9 +11,14 @@ #include "motor.h" #include "status.h" #include "abs.h" +#include "log.h" +#if 0 #define GRAVITY 9.80665f +#endif +#define GRAVITY 1.0f + #define KP_ROLLPITCH 0.05967 #define KI_ROLLPITCH 0.00001278 @@ -22,7 +27,10 @@ /* Maximum allowed error for arming */ #define ERROR_THRESHOLD 0.20f +#define LOG_MAGIC_DCM_UPDATE 0x00DC111A +#define LOG_MAGIC_DCM_DRIFT 0x00DC111B + /* Implementation of the DCM IMU concept as described by Premerlani * and Bizard */ @@ -36,8 +44,16 @@ float omega_x, omega_y, omega_z; float omega_x, omega_y, omega_z; -float delta_t = 0.01; +float delta_t = 0.005; +void dcm_log(unsigned int magic) +{ + int i; + log_put_uint(magic); + for (i = 0; i < 9; i++) + log_put_float(dcm[i]); +} + void dcm_update(float x, float y, float z) { omega_x = x + omega_i[0] + omega_p[0]; @@ -57,6 +73,8 @@ void dcm_update(float x, float y, float matrix_add(dcm, dcm, temp_matrix, 3, 3); dcm_normalise(); + +/* dcm_log(LOG_MAGIC_DCM_UPDATE); */ } void dcm_setvector(float x, float y, float z) @@ -155,11 +173,13 @@ void dcm_drift_correction(float x, float void dcm_drift_correction(float x, float y, float z) { - float mag; float weight; float error[3]; int i; +#if DCM_WEIGHT + float mag; + mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY; mag = 1-mag; @@ -172,6 +192,9 @@ void dcm_drift_correction(float x, float weight = 0.0; if (weight > 1.0) weight = 1.0; +#else + weight = 1.0; +#endif /* error = cross product of dcm last row and acceleration vector */ /* third row = cross product of first two rows */ @@ -200,6 +223,8 @@ void dcm_drift_correction(float x, float omega_i[i] += error[i] * (KI_ROLLPITCH * weight); } + dcm_log(LOG_MAGIC_DCM_DRIFT); + #if 0 putstr("w: "); putint_s((int)(weight * 100000.0f));