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

Add missing file and tidy up somewhat.

Browse Source
This commit is contained in:
Gavan Fantom
2014-11-23 20:20:48 +00:00
parent 8b3ff9feba
commit 4f171f3023
12 changed files with 136 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
dcm.c
View File

@@ -42,7 +42,7 @@ 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;
vec3f omega;
float delta_t = 0.005;
@@ -54,15 +54,15 @@ void dcm_log(unsigned int magic)
log_put_float(dcm[i]);
}
void dcm_update(float x, float y, float z)
void dcm_update(vec3f gyro)
{
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];
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 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,
@@ -77,12 +77,12 @@ void dcm_update(float x, float y, float z)
/* dcm_log(LOG_MAGIC_DCM_UPDATE); */
}
void dcm_setvector(float x, float y, float z)
void dcm_setvector(vec3f zvec)
{
/* We're given the Z axis */
dcm[6] = x;
dcm[7] = y;
dcm[8] = z;
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;
@@ -171,7 +171,7 @@ bool dcm_renormalise(float *v)
return TRUE;
}
void dcm_drift_correction(float x, float y, float z)
void dcm_drift_correction(vec3f accel)
{
float weight;
float error[3];
@@ -180,7 +180,10 @@ void dcm_drift_correction(float x, float y, float z)
#if DCM_WEIGHT
float mag;
mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY;
mag = (1.0/fisqrt(accel.x*accel.x+
accel.y*accel.y+
accel.z*accel.z))
/ GRAVITY;
mag = 1-mag;
if (mag < 0.0)
@@ -198,9 +201,9 @@ void dcm_drift_correction(float x, float y, float z)
/* 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;
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) &&
@@ -251,7 +254,8 @@ void dcm_drift_correction(float x, float y, float z)
/* Maximum angle to the horizontal for arming: 30 degrees */
#define ATTITUDE_THRESHOLD (0.5)
void dcm_attitude_error(float roll, float pitch, float yaw)
/* x = roll, y = pitch, z = yaw */
void dcm_attitude_error(vec3f target)
{
/* dcm[6] = sine of pitch */
/* dcm[7] = sine of roll */
@@ -272,7 +276,8 @@ void dcm_attitude_error(float roll, float pitch, float yaw)
status_set_ready(STATUS_MODULE_ATTITUDE, FALSE);
}
motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z);
vec3f measured = {dcm[6], -dcm[7], -omega.z};
motor_pid_update(target, measured);
}
void dcm_dump(void)