Below is the file 'dcm.c' from this revision. You can also download the file.
/* dcm.c */ #ifdef WE_HAVE_SQRT #include <math.h> #else #include "fisqrt.h" #endif #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 */ float dcm[3*3] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; float omega_p[3] = {0.0, 0.0, 0.0}; float omega_i[3] = {0.0, 0.0, 0.0}; vec3f omega; 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(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}; float temp_matrix[3*3]; matrix_multiply(temp_matrix, dcm, update_matrix, 3, 3, 3); matrix_add(dcm, dcm, temp_matrix, 3, 3); dcm_normalise(); /* dcm_log(LOG_MAGIC_DCM_UPDATE); */ } void dcm_setvector(vec3f zvec) { /* We're given the Z axis */ 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; dcm[4] = -dcm[8]; dcm[5] = dcm[7]; /* First row = cross product of third and second rows */ dcm[0] = dcm[7]*dcm[5] - dcm[8]*dcm[4]; dcm[1] = dcm[8]*dcm[3] - dcm[6]*dcm[5]; dcm[2] = dcm[6]*dcm[4] - dcm[7]*dcm[3]; /* Second row = cross product of third and first rows */ dcm[3] = dcm[7]*dcm[2] - dcm[8]*dcm[1]; dcm[4] = dcm[8]*dcm[0] - dcm[6]*dcm[2]; dcm[5] = dcm[6]*dcm[1] - dcm[7]*dcm[0]; dcm_renormalise(dcm+0); dcm_renormalise(dcm+3); dcm_renormalise(dcm+6); #if 0 dcm_normalise(); #endif } void dcm_normalise(void) { float error; float tmp[6]; int i; /* dot product of first two rows */ error = dcm[0]*dcm[3] + dcm[1]*dcm[4] + dcm[2]*dcm[5]; /* printf("error is %f\n", error); */ tmp[0] = dcm[3]; tmp[1] = dcm[4]; tmp[2] = dcm[5]; tmp[3] = dcm[0]; tmp[4] = dcm[1]; tmp[5] = dcm[2]; for (i = 0; i < 6; i++) dcm[i] = dcm[i] - (tmp[i] * (0.5f * error)); /* third row = cross product of first two rows */ dcm[6] = dcm[1]*dcm[5] - dcm[2]*dcm[4]; dcm[7] = dcm[2]*dcm[3] - dcm[0]*dcm[5]; dcm[8] = dcm[0]*dcm[4] - dcm[1]*dcm[3]; if (!(dcm_renormalise(dcm+0) && dcm_renormalise(dcm+3) && dcm_renormalise(dcm+6))) { /* Shit. I've been shot. */ dcm[0] = dcm[4] = dcm[8] = 1.0f; dcm[1] = dcm[2] = dcm[3] = 0.0f; dcm[5] = dcm[6] = dcm[7] = 0.0f; } } bool dcm_renormalise(float *v) { float f = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; /* printf("f is %f\n", f); */ if (f < 1.5625f && f > 0.64f) { f = 0.5 * (3 - f); } else if (f < 100.0f && f > 0.01f) { #ifdef WE_HAVE_SQRT f = 1.0 / sqrt(f); #else f = fisqrt(f); #endif /* XXX log this event? */ putstr("sqrt\r\n"); } else { putstr("problem\r\n"); return FALSE; } v[0] = v[0] * f; v[1] = v[1] * f; v[2] = v[2] * f; return TRUE; } void dcm_drift_correction(vec3f accel) { float weight; float error[3]; int i; #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; weight = 1 - 3*mag; if (weight < 0.0) 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]*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; if (error[i] < -ERROR_LIMIT) error[i] = -ERROR_LIMIT; } for (i = 0; i < 3; i++) { omega_p[i] = error[i] * (KP_ROLLPITCH * weight); 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)); putstr(", "); putint_s((int)(omega_p[1] * 100000.0f)); putstr(", "); putint_s((int)(omega_p[2] * 100000.0f)); putstr(" i: "); putint_s((int)(omega_i[0] * 100000.0f)); putstr(", "); putint_s((int)(omega_i[1] * 100000.0f)); putstr(", "); putint_s((int)(omega_i[2] * 100000.0f)); putstr("\r\n"); #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: "); putint_s((int)(dcm[0] * 100000.0f)); putstr(", "); putint_s((int)(dcm[1] * 100000.0f)); putstr(", "); putint_s((int)(dcm[2] * 100000.0f)); putstr("\r\n "); putint_s((int)(dcm[3] * 100000.0f)); putstr(", "); putint_s((int)(dcm[4] * 100000.0f)); putstr(", "); putint_s((int)(dcm[5] * 100000.0f)); putstr("\r\n "); putint_s((int)(dcm[6] * 100000.0f)); putstr(", "); putint_s((int)(dcm[7] * 100000.0f)); putstr(", "); putint_s((int)(dcm[8] * 100000.0f)); putstr("\r\n"); } void puthexfloat(float f) { union { float f; unsigned int i; } u; u.f = f; puthex(u.i); } void dcm_send_packet(void) { putstr("D:("); puthexfloat(dcm[0]); putstr(","); puthexfloat(dcm[1]); putstr(","); puthexfloat(dcm[2]); putstr(","); puthexfloat(dcm[3]); putstr(","); puthexfloat(dcm[4]); putstr(","); puthexfloat(dcm[5]); putstr(","); puthexfloat(dcm[6]); putstr(","); puthexfloat(dcm[7]); putstr(","); puthexfloat(dcm[8]); putstr(")\r\n"); }