Quadrotor from scratch
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

/* 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");
}