coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Lots of development of new features. Radio input, motor output, PID control

Browse Source 
loops, boot-time initialisation, option to run without attached UART.

And.. it flies!
This commit is contained in:
Gavan Fantom
2012-08-30 12:42:38 +00:00
parent 2890007195
commit 3f12132231
18 changed files with 727 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
dcm.c
View File

@@ -8,6 +8,7 @@
#include "matrix.h"
#include "dcm.h"
#include "uart.h"
#include "motor.h"
#define GRAVITY 9.80665f
@@ -27,13 +28,15 @@ float dcm[3*3] = {1, 0, 0,
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)
{
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];
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;
@@ -182,9 +185,12 @@ void dcm_drift_correction(float x, float y, float z)
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));
@@ -202,6 +208,22 @@ void dcm_drift_correction(float x, float y, float z)
#endif
}
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? */
motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z);
}
void dcm_dump(void)
{
putstr("dcm: ");