The unified diff between revisions [23a3e9a5..] and [08a35a66..] 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 [08a35a6680cdf8985cfb16fa6779ee6db7202a9c] # # patch "dcm.c" # from [35a44d5b49a5c9bf56678ae7af0b611fde405621] # to [a94a48f5dabd3ba779aabfdb936b6c019c7b0531] # ============================================================ --- dcm.c 35a44d5b49a5c9bf56678ae7af0b611fde405621 +++ dcm.c a94a48f5dabd3ba779aabfdb936b6c019c7b0531 @@ -8,14 +8,29 @@ #include "matrix.h" #include "dcm.h" #include "uart.h" +#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 #define ERROR_LIMIT 1.17f +/* 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 */ @@ -27,18 +42,28 @@ 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 delta_t = 0.01; +vec3f omega; -void dcm_update(float x, float y, float z) +float delta_t = 0.005; + +void dcm_log(unsigned int magic) { - 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]; + int i; + log_put_uint(magic); + for (i = 0; i < 9; i++) + log_put_float(dcm[i]); +} - float tx = delta_t * omega_x; - float ty = delta_t * omega_y; - float tz = delta_t * omega_z; +void dcm_update(vec3f gyro) +{ + omega.x = gyro.x + omega_i[0] + omega_p[0]; + omega.y = gyro.y + omega_i[1] + omega_p[1]; + omega.z = gyro.z + omega_i[2] + omega_p[2]; + float tx = delta_t * omega.x; + float ty = delta_t * omega.y; + float tz = delta_t * omega.z; + float update_matrix[3*3] = { 0, -tz, ty, tz, 0, -tx, -ty, tx, 0}; @@ -48,14 +73,16 @@ 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) +void dcm_setvector(vec3f zvec) { /* We're given the Z axis */ - dcm[6] = x; - dcm[7] = y; - dcm[8] = z; + dcm[6] = zvec.x; + dcm[7] = zvec.y; + dcm[8] = zvec.z; /* Second row = cross product of unit X and third rows */ dcm[3] = 0.0; @@ -144,15 +171,20 @@ bool dcm_renormalise(float *v) return TRUE; } -void dcm_drift_correction(float x, float y, float z) +void dcm_drift_correction(vec3f accel) { - float mag; float weight; float error[3]; int i; - mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY; +#if DCM_WEIGHT + float mag; + mag = (1.0/fisqrt(accel.x*accel.x+ + accel.y*accel.y+ + accel.z*accel.z)) + / GRAVITY; + mag = 1-mag; if (mag < 0.0) mag = -mag; @@ -163,13 +195,25 @@ 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 */ - error[0] = dcm[7]*z - dcm[8]*y; - error[1] = dcm[8]*x - dcm[6]*z; - error[2] = dcm[6]*y - dcm[7]*x; + error[0] = dcm[7]*accel.z - dcm[8]*accel.y; + error[1] = dcm[8]*accel.x - dcm[6]*accel.z; + error[2] = dcm[6]*accel.y - dcm[7]*accel.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 +226,14 @@ 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)); putstr("\r\n"); +#endif + #if 0 putstr("p: "); putint_s((int)(omega_p[0] * 100000.0f)); @@ -202,6 +251,35 @@ void dcm_drift_correction(float x, float #endif } +/* Maximum angle to the horizontal for arming: 30 degrees */ +#define ATTITUDE_THRESHOLD (0.5) + +/* x = roll, y = pitch, z = yaw */ +void dcm_attitude_error(vec3f target) +{ + /* 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); + } + + vec3f measured = {dcm[6], -dcm[7], -omega.z}; + motor_pid_update(target, measured); +} + void dcm_dump(void) { putstr("dcm: ");