You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
333 lines
6.6 KiB
333 lines
6.6 KiB
/* 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}; |
|
|
|
float omega_x, omega_y, omega_z; |
|
|
|
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]; |
|
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(); |
|
|
|
/* dcm_log(LOG_MAGIC_DCM_UPDATE); */ |
|
} |
|
|
|
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 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; |
|
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]*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); |
|
} |
|
|
|
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) |
|
|
|
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"); |
|
}
|
|
|