The unified diff between revisions [bfc9e27f..] and [3dc5e7ac..] 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 [bfc9e27f5c40da31ae4269394aaf0545e5856a70] # new_revision [3dc5e7ac4bcb952cc267892653dd78ed095d8778] # # patch "dcm.c" # from [62d414448bb944679d6e5a3e693736501a28a009] # to [a94a48f5dabd3ba779aabfdb936b6c019c7b0531] # ============================================================ --- dcm.c 62d414448bb944679d6e5a3e693736501a28a009 +++ dcm.c a94a48f5dabd3ba779aabfdb936b6c019c7b0531 @@ -42,7 +42,7 @@ float omega_i[3] = {0.0, 0.0, 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 /* 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 #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 /* 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 /* 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, floa 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)