The unified diff between revisions [23a3e9a5..] and [be147b11..] 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 [23a3e9a50b4034343e3bd217d2c225dcaec064dd] # new_revision [be147b11caac304fda1579ac71017eecc3bb79e0] # # patch "dcm.c" # from [35a44d5b49a5c9bf56678ae7af0b611fde405621] # to [90d94d47a262c72d472ee7aca36bb55e2b328e66] # ============================================================ --- dcm.c 35a44d5b49a5c9bf56678ae7af0b611fde405621 +++ dcm.c 90d94d47a262c72d472ee7aca36bb55e2b328e66 @@ -8,6 +8,9 @@ #include "matrix.h" #include "dcm.h" #include "uart.h" +#include "motor.h" +#include "status.h" +#include "abs.h" #define GRAVITY 9.80665f @@ -16,6 +19,10 @@ #define ERROR_LIMIT 1.17f +/* Maximum allowed error for arming */ +#define ERROR_THRESHOLD 0.20f + + /* Implementation of the DCM IMU concept as described by Premerlani * and Bizard */ @@ -27,13 +34,15 @@ float omega_i[3] = {0.0, 0.0, 0.0}; float omega_p[3] = {0.0, 0.0, 0.0}; float omega_i[3] = {0.0, 0.0, 0.0}; +float omega_x, omega_y, omega_z; + float delta_t = 0.01; void dcm_update(float x, float y, float z) { - float omega_x = x + omega_i[0] + omega_p[0]; - float omega_y = y + omega_i[1] + omega_p[1]; - float omega_z = z + omega_i[2] + omega_p[2]; + omega_x = x + omega_i[0] + omega_p[0]; + omega_y = y + omega_i[1] + omega_p[1]; + omega_z = z + omega_i[2] + omega_p[2]; float tx = delta_t * omega_x; float ty = delta_t * omega_y; @@ -170,6 +179,15 @@ void dcm_drift_correction(float x, float error[1] = dcm[8]*x - dcm[6]*z; error[2] = dcm[6]*y - dcm[7]*x; + if (!status_armed()) { + if ((abs(error[0]) < ERROR_THRESHOLD) && + (abs(error[1]) < ERROR_THRESHOLD) && + (abs(error[2]) < ERROR_THRESHOLD)) + status_set_ready(STATUS_MODULE_DCM_ERROR, TRUE); + else + status_set_ready(STATUS_MODULE_DCM_ERROR, FALSE); + } + for (i = 0; i < 3; i++) { if (error[i] > ERROR_LIMIT) error[i] = ERROR_LIMIT; @@ -182,9 +200,12 @@ void dcm_drift_correction(float x, float omega_i[i] += error[i] * (KI_ROLLPITCH * weight); } +#if 0 putstr("w: "); putint_s((int)(weight * 100000.0f)); putstr("\r\n"); +#endif + #if 0 putstr("p: "); putint_s((int)(omega_p[0] * 100000.0f)); @@ -202,6 +223,33 @@ void dcm_drift_correction(float x, float #endif } +/* Maximum angle to the horizontal for arming: 30 degrees */ +#define ATTITUDE_THRESHOLD (0.5) + +void dcm_attitude_error(float roll, float pitch, float yaw) +{ + /* dcm[6] = sine of pitch */ + /* dcm[7] = sine of roll */ + + /* pitch error = pitch - dcm[6] */ + /* roll error = roll - dcm[7] */ + + /* That was the theory. In practice, there appears to be some + confusion over axes. Pitch and roll seem.. reversed. */ + + /* TODO: What if we are upside down? */ + + if (!status_armed()) { + if ((abs(dcm[6]) < ATTITUDE_THRESHOLD) && + (abs(dcm[7]) < ATTITUDE_THRESHOLD)) + status_set_ready(STATUS_MODULE_ATTITUDE, TRUE); + else + status_set_ready(STATUS_MODULE_ATTITUDE, FALSE); + } + + motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z); +} + void dcm_dump(void) { putstr("dcm: ");