Add missing file and tidy up somewhat.

dcm.c

@@ -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)

dcm.h

@@ -4,11 +4,11 @@
 
 extern float delta_t;
 
-void dcm_update(float omega_x, float omega_y, float omega_z);
+void dcm_update(vec3f gyro);
 void dcm_normalise(void);
 bool dcm_renormalise(float *v);
 void dcm_dump(void);
 void dcm_send_packet(void);
-void dcm_setvector(float x, float y, float z);
-void dcm_drift_correction(float x, float y, float z);
-void dcm_attitude_error(float x, float y, float z);
+void dcm_setvector(vec3f zvec);
+void dcm_drift_correction(vec3f accel);
+void dcm_attitude_error(vec3f target);

hmc5883l.c

@@ -85,16 +85,6 @@ bool hmc5883l_start_sample(void)
 
 void hmc5883l_event_handler(void)
 {
-#if 0
-	signed short int xi, yi, zi;
-	signed short int tempi;
-	signed short int rolli, pitchi, yawi;
-
-	float x, y, z;
-	float temp;
-	float roll, pitch, yaw;
-#endif
-
 	if (hmc5883l_result != I2C_SUCCESS)
 		return;
 
@@ -106,23 +96,6 @@ void hmc5883l_event_handler(void)
 	if (hmc5883l_state)
 	    return;
 
-#if 0
-	xi = (hmc5883l_sample_data[0] << 8) + hmc5883l_sample_data[1];
-	yi = (hmc5883l_sample_data[2] << 8) + hmc5883l_sample_data[3];
-	zi = (hmc5883l_sample_data[4] << 8) + hmc5883l_sample_data[5];
-
-	tempi = (hmc5883l_sample_data[6] << 8) + hmc5883l_sample_data[7];
-
-	rolli  = (hmc5883l_sample_data[ 8] << 8)+hmc5883l_sample_data[ 9];
-	pitchi = (hmc5883l_sample_data[10] << 8)+hmc5883l_sample_data[11];
-	yawi   = (hmc5883l_sample_data[12] << 8)+hmc5883l_sample_data[13];
-
-	sensors_write_gyro_data(roll, pitch, yaw);
-	sensors_write_accel_data(x, y, z);
-	sensors_write_temp_data(temp);
-
-#endif
-
 	log_put_uint(LOG_SIGNATURE_MAGNETOMETER);
 	log_put_array((char *)hmc5883l_sample_data, 6);
 }

motor.c

@@ -27,10 +27,12 @@ float throttle = 0.0f;
  * Perform a PID loop iteration.
  * roll and pitch are absolute values
  * yaw is, currently, a rate.
+ * For this function only, coordinate convention is:
+ *   x = roll
+ *   y = pitch
+ *   z = yaw
  */
-void motor_pid_update(float troll, float mroll,
-		      float tpitch, float mpitch,
-		      float tyaw, float myaw)
+void motor_pid_update(vec3f target, vec3f measured)
 {
 	float derivative[3];
 	float out[3];
@@ -40,16 +42,16 @@ void motor_pid_update(float troll, float mroll,
 	float min_motor;
 	int i;
 
-	roll  = troll  - mroll;
-	pitch = tpitch - mpitch;
-	yaw   = tyaw   - myaw;
+	roll  = target.x - measured.x;
+	pitch = target.y - measured.y;
+	yaw   = target.z - measured.z;
 
 #if 0
         if ((stick_counter % 100) == 0) {
 		putstr("{");
-		putint_s((int)(tyaw * 10000));
+		putint_s((int)(target.z * 10000));
 		putstr(", ");
-		putint_s((int)(myaw * 10000));
+		putint_s((int)(measured.z * 10000));
 		putstr("}\r\n");
 	}
 #endif
@@ -67,13 +69,13 @@ void motor_pid_update(float troll, float mroll,
 	}
 
 	/* The measurements are the opposite sign to the error */
-	derivative[0] = (-mroll  - last[0]) / delta_t;
-	derivative[1] = (-mpitch - last[1]) / delta_t;
-	derivative[2] = (-myaw   - last[2]) / delta_t;
+	derivative[0] = (-measured.x - last[0]) / delta_t;
+	derivative[1] = (-measured.y - last[1]) / delta_t;
+	derivative[2] = (-measured.z - last[2]) / delta_t;
 
-	last[0] = -mroll;
-	last[1] = -mpitch;
-	last[2] = -myaw;
+	last[0] = -measured.x;
+	last[1] = -measured.y;
+	last[2] = -measured.z;
 
 	out[0] = roll  * Kp   + integral[0] * Ki   + derivative[0] * Kd;
 	out[1] = pitch * Kp   + integral[1] * Ki   + derivative[1] * Kd;

motor.h

@@ -1,9 +1,12 @@
 /* motor.h */
 
+#ifndef __MOTOR_H
+#define __MOTOR_H
+#include "types.h"
+
 extern float temp_kp;
 
-void motor_pid_update(float troll, float mroll,
-		      float tpitch, float mpitch,
-		      float tyaw, float myaw);
+void motor_pid_update(vec3f target, vec3f measured);
 void motor_kill(void);
 void motor_set_throttle(float t);
+#endif /* __MOTOR_H */

mpl3115a2.c

@@ -55,23 +55,12 @@ bool mpl3115a2_init(void)
 	mpl3115a2_state = 0;
 
 	return TRUE;
-
-#if 0
-	if (!i2c_start_transaction(&mpl3115a2_init_transaction))
-		return FALSE;
-	while (i2c_busy()) ;
-
-
-	return (mpl3115a2_result == I2C_SUCCESS);
-#endif
 }
 
 bool mpl3115a2_start_sample(void)
 {
 	mpl3115a2_state = !mpl3115a2_state;
 
-	putstr("X");
-
 	if (mpl3115a2_state)
 	    return i2c_start_transaction(&mpl3115a2_start_transaction);
 	else
@@ -80,16 +69,6 @@ bool mpl3115a2_start_sample(void)
 
 void mpl3115a2_event_handler(void)
 {
-#if 0
-	signed short int xi, yi, zi;
-	signed short int tempi;
-	signed short int rolli, pitchi, yawi;
-
-	float x, y, z;
-	float temp;
-	float roll, pitch, yaw;
-#endif
-
 	if (mpl3115a2_result != I2C_SUCCESS)
 		return;
 
@@ -101,32 +80,7 @@ void mpl3115a2_event_handler(void)
 	if (mpl3115a2_state)
 	    return;
 
-#if 0
-	xi = (mpl3115a2_sample_data[0] << 8) + mpl3115a2_sample_data[1];
-	yi = (mpl3115a2_sample_data[2] << 8) + mpl3115a2_sample_data[3];
-	zi = (mpl3115a2_sample_data[4] << 8) + mpl3115a2_sample_data[5];
-
-	tempi = (mpl3115a2_sample_data[6] << 8) + mpl3115a2_sample_data[7];
-
-	rolli  = (mpl3115a2_sample_data[ 8] << 8)+mpl3115a2_sample_data[ 9];
-	pitchi = (mpl3115a2_sample_data[10] << 8)+mpl3115a2_sample_data[11];
-	yawi   = (mpl3115a2_sample_data[12] << 8)+mpl3115a2_sample_data[13];
-
-	sensors_write_gyro_data(roll, pitch, yaw);
-	sensors_write_accel_data(x, y, z);
-	sensors_write_temp_data(temp);
-
-#endif
-
 	log_put_uint(LOG_SIGNATURE_ALTIMETER);
 	log_put_array((char *)mpl3115a2_sample_data, 5);
-	putstr("Y");
-
-#if 0
-	puthex(mpl3115a2_sample_data[0]);
-	puthex(mpl3115a2_sample_data[1]);
-	puthex(mpl3115a2_sample_data[2]);
-	putstr("\n");
-#endif
 }
 

mpu6050.c

@@ -214,9 +214,8 @@ void mpu6050_event_handler(void)
 	signed short int tempi;
 	signed short int rolli, pitchi, yawi;
 
-	float x, y, z;
+	vec3f accel, gyro;
 	float temp;
-	float roll, pitch, yaw;
 
 	CHECKPOINT(9);
 
@@ -252,18 +251,18 @@ void mpu6050_event_handler(void)
 		yawi   -= gyro_zero_yaw;
 	}
 
-	x = ((float)xi) * ACCEL_SCALE;
-	y = ((float)yi) * ACCEL_SCALE;
-	z = ((float)zi) * ACCEL_SCALE;
+	accel.x = ((float)xi) * ACCEL_SCALE;
+	accel.y = ((float)yi) * ACCEL_SCALE;
+	accel.z = ((float)zi) * ACCEL_SCALE;
 
 	temp = ((float)tempi) * TEMP_SCALE + TEMP_OFFSET;
 
-	roll  = ((float)rolli)  * GYRO_SCALE;
-	pitch = ((float)pitchi) * GYRO_SCALE;
-	yaw   = ((float)yawi)   * GYRO_SCALE;
+	gyro.x = ((float)rolli)  * GYRO_SCALE;
+	gyro.y = ((float)pitchi) * GYRO_SCALE;
+	gyro.z = ((float)yawi)   * GYRO_SCALE;
 
-	sensors_write_gyro_data(roll, pitch, yaw);
-	sensors_write_accel_data(x, y, z);
+	sensors_write_gyro_data(gyro);
+	sensors_write_accel_data(accel);
 	sensors_write_temp_data(temp);
 
 	log_put_array((char *)mpu6050_sample_data, 14);

sensors.c

@@ -35,19 +35,16 @@ static bool sensors_update;
 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;
-
-float gyro_yaw_zero;
-float gyro_pitch_zero;
-float gyro_roll_zero;
+vec3f sensors_accel;
 
 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_process(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("(");

sensors.h

@@ -0,0 +1,21 @@
+/* sensors.h */
+
+#ifndef __SENSORS_H
+#define __SENSORS_H
+
+#include "types.h"
+
+/* functions to be called from above */
+bool sensors_init(void);
+bool sensors_start_sample(void);
+void sensors_dump(void);
+
+/* functions to be called from below */
+void sensors_sample_done(void);
+/* x = roll, y = pitch, z = yaw */
+void sensors_write_gyro_data(vec3f gyro_data);
+void sensors_write_accel_data(vec3f accel_data);
+void sensors_write_temp_data(float temp);
+void sensors_start_zero(void);
+
+#endif /* __SENSORS_H */

stick.c

@@ -59,9 +59,9 @@ float stick_yaw = 0.0f;
 
 unsigned int stick_counter;
 
-void stick_update(float x, float y, float z)
+void stick_update(vec3f stick)
 {
-	float tz = delta_t * z;
+	float tz = delta_t * stick.z;
 
 	stick_yaw += tz;
 
@@ -72,16 +72,16 @@ void stick_update(float x, float y, float z)
 	    stick_yaw -= TWO_PI;
 
 #if 0
-	z = stick_yaw;
+	stick.z = stick_yaw;
 #endif
 
-	x = sine(x);
-	y = sine(y);
+	stick.x = sine(stick.x);
+	stick.y = sine(stick.y);
 #if 0
-	z = 1.0/fisqrt(1-x*x-y*y);
+	stick.z = 1.0/fisqrt(1-stick.x*stick.x-stick.y*stick.y);
 #endif
 
-	dcm_attitude_error(x, y, z);
+	dcm_attitude_error(stick);
 }
 
 #ifdef STICK_DEBUG_CALIBRATE
@@ -168,9 +168,7 @@ void stick_input(void) {
 	watchdog_kick(WATCHDOG_STICK);
 
 	/* So the controls are back to front. Let's fix that. */
-	x = -x;
-	y = -y;
-	z = -z;
+	vec3f stick = {-x, -y, -z};
 
 #if 0
 	if ((stick_counter % 100) == 0) {
@@ -181,10 +179,10 @@ void stick_input(void) {
 #endif
 
 #if 1
-	stick_update(x, y, z);
+	stick_update(stick);
 #else
 	if ((stick_counter % 100) == 0)
-		stick_update(x, y, z);
+		stick_update(stick);
 #endif
 
 /*

stick.h

@@ -7,4 +7,4 @@ extern bool armed;
 extern unsigned int stick_counter;
 
 void stick_input(void);
-void stick_update(float x, float y, float omega_z);
+void stick_update(vec3f stick);

types.h

@@ -9,4 +9,8 @@ typedef int bool;
 
 typedef unsigned int size_t;
 
+typedef struct {
+    float x, y, z;
+} vec3f;
+
 #endif /* __TYPES_H */