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: 'sensors.c'

#
# old_revision [bfc9e27f5c40da31ae4269394aaf0545e5856a70]
# new_revision [3dc5e7ac4bcb952cc267892653dd78ed095d8778]
#
# patch "sensors.c"
#  from [ba01cef6cd239a12b9171dccb25fd02092ca037c]
#    to [178f9bbaac429772d293d7749bb23c32fbed0c98]
#
============================================================
--- sensors.c	ba01cef6cd239a12b9171dccb25fd02092ca037c
+++ sensors.c	178f9bbaac429772d293d7749bb23c32fbed0c98
@@ -35,20 +35,17 @@ static unsigned int sensors_generation;
 static unsigned int sensors_discard;
 static unsigned int sensors_generation;
 
-float sensors_gyro_roll;
-float sensors_gyro_pitch;
-float sensors_gyro_yaw;
+/* x = roll
+   y = pitch
+   z = yaw
+ */
+vec3f sensors_gyro;
+vec3f gyro_zero;
 
 float sensors_temp;
 
-float sensors_accel_x;
-float sensors_accel_y;
-float sensors_accel_z;
+vec3f sensors_accel;
 
-float gyro_yaw_zero;
-float gyro_pitch_zero;
-float gyro_roll_zero;
-
 void sensors_write_log(void);
 void sensors_process(void);
 
@@ -103,24 +100,20 @@ bool sensors_start_sample(void)
 	return sensors_next_sample();
 }
 
-void sensors_write_gyro_data(float roll, float pitch, float yaw)
+void sensors_write_gyro_data(vec3f gyro)
 {
 #if 0
-	sensors_gyro_roll = roll - gyro_roll_zero;
-	sensors_gyro_pitch = pitch - gyro_pitch_zero;
-	sensors_gyro_yaw = yaw - gyro_yaw_zero;
+	sensors_gyro.x = gyro.x - gyro_zero.x;
+	sensors_gyro.y = gyro.y - gyro_zero.y;
+	sensors_gyro.z = gyro.z - gyro_zero.z;
 #else
-	sensors_gyro_roll = roll;
-	sensors_gyro_pitch = pitch;
-	sensors_gyro_yaw = yaw;
+	sensors_gyro = gyro;
 #endif
 }
 
-void sensors_write_accel_data(float x, float y, float z)
+void sensors_write_accel_data(vec3f accel)
 {
-	sensors_accel_x = x;
-	sensors_accel_y = y;
-	sensors_accel_z = z;
+	sensors_accel = accel;
 }
 
 void sensors_write_temp_data(float temp)
@@ -136,12 +129,12 @@ void sensors_write_log(void)
 {
 #if 0
 	log_put_uint(LOG_SIGNATURE_SENSORS);
-	log_put_float(sensors_accel_x);
-	log_put_float(sensors_accel_y);
-	log_put_float(sensors_accel_z);
-	log_put_float(sensors_gyro_roll);
-	log_put_float(sensors_gyro_pitch);
-	log_put_float(sensors_gyro_yaw);
+	log_put_float(sensors_accel.x);
+	log_put_float(sensors_accel.y);
+	log_put_float(sensors_accel.z);
+	log_put_float(sensors_gyro.x);
+	log_put_float(sensors_gyro.y);
+	log_put_float(sensors_gyro.z);
 	log_put_float(sensors_temp);
 #else
 	/* XXX this just about comes out in the right place, but by luck */
@@ -163,20 +156,19 @@ void sensors_process(void)
 {
 	if (sensors_update) {
 #if 1
-		dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll,
-		    -sensors_gyro_yaw);
+		vec3f dcm_gyro = {-sensors_gyro.y, -sensors_gyro.x, -sensors_gyro.z};
 #else
-		dcm_update(0.0, 0.0, 0.0);
+		vec3f dcm_gyro = {0.0, 0.0, 0.0};
 #endif
+		dcm_update(dcm_gyro);
 		if (!status_armed()) {
-			if ( (abs(sensors_gyro_roll)  < GYRO_RATE_THRESHOLD) &&
-			     (abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) &&
-			     (abs(sensors_gyro_yaw)   < GYRO_RATE_THRESHOLD)) {
+			if ( (abs(sensors_gyro.x)  < GYRO_RATE_THRESHOLD) &&
+			     (abs(sensors_gyro.y)  < GYRO_RATE_THRESHOLD) &&
+			     (abs(sensors_gyro.z)  < GYRO_RATE_THRESHOLD)) {
 			    status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE);
 			} else {
 			    status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE);
 			}
-
 		}
 
 		sensors_generation++;
@@ -192,17 +184,17 @@ void sensors_process(void)
 		if (sensors_discard) {
 			sensors_discard--;
 		} else {
-			gyro_yaw_zero += sensors_gyro_yaw;
-			gyro_pitch_zero += sensors_gyro_pitch;
-			gyro_roll_zero += sensors_gyro_roll;
+			gyro_zero.z += sensors_gyro.z;
+			gyro_zero.y += sensors_gyro.y;
+			gyro_zero.x += sensors_gyro.x;
 			sensors_generation++;
 			if (sensors_generation >= GYRO_ZERO_COUNT) {
 				sensors_zero = FALSE;
 				sensors_update = TRUE;
 				sensors_generation = 0;
-				gyro_yaw_zero /= GYRO_ZERO_COUNT;
-				gyro_pitch_zero /= GYRO_ZERO_COUNT;
-				gyro_roll_zero /= GYRO_ZERO_COUNT;
+				gyro_zero.z /= GYRO_ZERO_COUNT;
+				gyro_zero.y /= GYRO_ZERO_COUNT;
+				gyro_zero.x /= GYRO_ZERO_COUNT;
 				putstr("Zero finished\r\n");
 				status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
 			}
@@ -211,13 +203,12 @@ void sensors_process(void)
 	watchdog_kick(WATCHDOG_GYRO);
 
 #if 1
-	dcm_drift_correction(-sensors_accel_y, -sensors_accel_x,
-		-sensors_accel_z);
+	vec3f dcm_accel = {-sensors_accel.y, -sensors_accel.x, -sensors_accel.z};
 #endif
 #if 0
-	dcm_drift_correction(sensors_accel_x, sensors_accel_y,
-		sensors_accel_z);
+	vec3f dcm_accel = {sensors_accel.x, sensors_accel.y, sensors_accel.z};
 #endif
+	dcm_drift_correction(dcm_accel);
 
 	watchdog_kick(WATCHDOG_ACCEL);
 	stick_input();
@@ -226,19 +217,19 @@ void sensors_dump(void)
 void sensors_dump(void)
 {
 	putstr("(");
-	putint_s((int)(sensors_accel_x * 1000.0));
+	putint_s((int)(sensors_accel.x * 1000.0));
 	putstr(",");
-	putint_s((int)(sensors_accel_y * 1000.0));
+	putint_s((int)(sensors_accel.y * 1000.0));
 	putstr(",");
-	putint_s((int)(sensors_accel_z * 1000.0));
+	putint_s((int)(sensors_accel.z * 1000.0));
 	putstr(")");
 
 	putstr("(");
-	putint_s((int)(sensors_gyro_roll  * 1000.0));
+	putint_s((int)(sensors_gyro.x * 1000.0));
 	putstr(",");
-	putint_s((int)(sensors_gyro_pitch * 1000.0));
+	putint_s((int)(sensors_gyro.y * 1000.0));
 	putstr(",");
-	putint_s((int)(sensors_gyro_yaw   * 1000.0));
+	putint_s((int)(sensors_gyro.z * 1000.0));
 	putstr(")");
 
 	putstr("(");