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)