The unified diff between revisions [65df00aa..] 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 [65df00aa2705ce33fd74f4dd706d2879fe99b2b0] # new_revision [be147b11caac304fda1579ac71017eecc3bb79e0] # # add_file "dcm.c" # content [90d94d47a262c72d472ee7aca36bb55e2b328e66] # ============================================================ --- /dev/null +++ dcm.c 90d94d47a262c72d472ee7aca36bb55e2b328e66 @@ -0,0 +1,308 @@ +/* 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" + +#define GRAVITY 9.80665f + +#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 + + +/* 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}; + +float omega_x, omega_y, omega_z; + +float delta_t = 0.01; + +void dcm_update(float x, float y, float z) +{ + 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; + 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(); +} + +void dcm_setvector(float x, float y, float z) +{ + /* We're given the Z axis */ + dcm[6] = x; + dcm[7] = y; + dcm[8] = 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(float x, float y, float z) +{ + float mag; + float weight; + float error[3]; + int i; + + mag = (1.0/fisqrt(x*x+y*y+z*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; + + /* 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; + + 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); + } + +#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) + +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: "); + 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"); +}