Add preliminary support for HMC5883L magnetometer and MPL3115A2 altimeter.

Logging only, at this stage.

Makefile

@@ -5,10 +5,11 @@ NAME=quad
 SSRCS=crt0.s
 CSRCS=main.c i2c.c mpu6050.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
 CSRCS+=fisqrt.c stick.c trig.c motor.c led.c watchdog.c panic.c status.c
-CSRCS+=thrust.c sensors.c spi.c sdcard.c log.c
+CSRCS+=thrust.c sensors.c spi.c sdcard.c log.c hmc5883l.c mpl3115a2.c
 
 #PROJOPTS=-DUSE_UART -DSEND_DCM -DSTICK_DEBUG_CALIBRATE
 PROJOPTS=-DTIMER_CPPM -DI2C_FAST
+#PROJOPTS=-DTIMER_CPPM -DI2C_FAST -DUSE_UART
 #PROJOPTS=-DTIMER_CPPM -DUSE_UART -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM
 #PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM
 #PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DUSE_UART -DSEND_DCM

event.h

@@ -5,10 +5,12 @@
 
 #define EVENT_TIMER		0
 #define EVENT_MPU6050_I2C_COMPLETE	1
-#define EVENT_UART_INPUT	2
+#define EVENT_HMC5883L_I2C_COMPLETE	2
-#define EVENT_SDCARD		3
+#define EVENT_MPL3115A2_I2C_COMPLETE	3
+#define EVENT_UART_INPUT	4
+#define EVENT_SDCARD		5
 
-#define EVENT_MAX		3
+#define EVENT_MAX		5
 
 typedef void event_handler(void);
 

hmc5883l.c

@@ -0,0 +1,129 @@
+/* hmc5883l.c */
+
+#include "sensors.h"
+#include "hmc5883l.h"
+#include "i2c.h"
+#include "event.h"
+#include "log.h"
+#include "uart.h"
+
+bool hmc5883l_state;
+
+i2c_result hmc5883l_result;
+
+unsigned char hmc5883l_sample_data[6];
+
+#define LOG_SIGNATURE_MAGNETOMETER 0xDA7AAA75
+
+unsigned char hmc5883l_init_command[] = {0x00, 0x70};
+unsigned char hmc5883l_init_command2[] = {0x01, 0xA0};
+
+struct i2c_transaction hmc5883l_init_transaction2;
+
+struct i2c_transaction hmc5883l_init_transaction = {
+	(0x1E << 1) + 0, /* write */
+	2,
+	hmc5883l_init_command,
+	&hmc5883l_result,
+	EVENT_HMC5883L_I2C_COMPLETE,
+	&hmc5883l_init_transaction2
+};
+
+struct i2c_transaction hmc5883l_init_transaction2 = {
+	(0x1E << 1) + 0, /* write */
+	2,
+	hmc5883l_init_command2,
+	&hmc5883l_result,
+	EVENT_HMC5883L_I2C_COMPLETE,
+	NULL
+};
+
+unsigned char hmc5883l_start_command[] = {0x02, 0x01};
+
+struct i2c_transaction hmc5883l_start_transaction = {
+	(0x1E << 1) + 0, /* write */
+	2,
+	hmc5883l_start_command,
+	&hmc5883l_result,
+	EVENT_HMC5883L_I2C_COMPLETE,
+	NULL
+};
+
+struct i2c_transaction hmc5883l_sample_transaction = {
+	(0x1E << 1) + 1, /* read */
+	6,
+	hmc5883l_sample_data,
+	&hmc5883l_result,
+	EVENT_HMC5883L_I2C_COMPLETE,
+	NULL
+};
+
+void hmc5883l_event_handler(void);
+
+bool hmc5883l_init(void)
+{
+	event_register(EVENT_HMC5883L_I2C_COMPLETE, hmc5883l_event_handler);
+
+	if (!i2c_start_transaction(&hmc5883l_init_transaction))
+		return FALSE;
+	while (i2c_busy()) ;
+
+	hmc5883l_state = 0;
+
+	return (hmc5883l_result == I2C_SUCCESS);
+}
+
+bool hmc5883l_start_sample(void)
+{
+	hmc5883l_state = !hmc5883l_state;
+
+	if (hmc5883l_state)
+	    return i2c_start_transaction(&hmc5883l_start_transaction);
+	else
+	    return i2c_start_transaction(&hmc5883l_sample_transaction);
+}
+
+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;
+
+	hmc5883l_result = I2C_IN_PROGRESS;
+
+	sensors_sample_done();
+
+	/* If we just started a conversion, our job is done for now */
+	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);
+}
+

hmc5883l.h

@@ -0,0 +1,10 @@
+#ifndef __HMC5883L_H
+#define __HMC5883L_H
+
+#include "types.h"
+
+bool hmc5883l_init(void);
+bool hmc5883l_start_sample(void);
+void hmc5883l_write_log(void);
+
+#endif /* __HMC5883L_H */

log.c

@@ -36,8 +36,10 @@ void log_put_byte(char c)
 	/* If the buffer is full, we just discard data.
 	 * Better than overrunning.
 	 */
-	if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart)
+	if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) {
+		putstr("^");
 		return;
+	}
 	log_buffer[log_bufferend++] = c;
 	log_bufferend = log_bufferend % LOG_BUFFERSIZE;
 #if 0

matrix.c

@@ -19,6 +19,7 @@ void matrix_multiply(float *dest, float *m1, float *m2, int r, int c, int n)
 	}
 }
 
+#if 0
 /* dest[r][c] = m1[r][n] * m2[c][n] */
 void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n)
 {
@@ -33,6 +34,7 @@ void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n)
 		}
 	}
 }
+#endif
 
 /* dest[r][c] = m1[r][c] + m2[r][c] */
 void matrix_add(float *dest, float *m1, float *m2, int r, int c)
@@ -64,6 +66,7 @@ void dump_matrix(float *m, int r, int c)
 }
 #endif
 
+#if 0
 /* Invert src into dst.
  * NOTE: destroys the src matrix
  */
@@ -145,3 +148,4 @@ void matrix_invert(float *dst, float *src, int size)
 
 	/* src should now be the identiy matrix while dst holds the answer */
 }
+#endif

mpl3115a2.c

@@ -0,0 +1,132 @@
+/* mpl3115a2.c */
+
+#include "sensors.h"
+#include "mpl3115a2.h"
+#include "i2c.h"
+#include "event.h"
+#include "log.h"
+#include "uart.h"
+
+bool mpl3115a2_state;
+
+i2c_result mpl3115a2_result;
+
+unsigned char mpl3115a2_sample_data[5];
+
+#define LOG_SIGNATURE_ALTIMETER 0xDA7AAA70
+
+unsigned char mpl3115a2_start_command[] = {0x26, 0x82};
+unsigned char mpl3115a2_read_address[] = {0x01};
+
+struct i2c_transaction mpl3115a2_start_transaction = {
+	(0x60 << 1) + 0, /* write */
+	sizeof(mpl3115a2_start_command),
+	mpl3115a2_start_command,
+	&mpl3115a2_result,
+	EVENT_MPL3115A2_I2C_COMPLETE,
+	NULL
+};
+
+struct i2c_transaction mpl3115a2_sample_transaction2;
+
+struct i2c_transaction mpl3115a2_sample_transaction = {
+	(0x60 << 1) + 0, /* write */
+	sizeof(mpl3115a2_read_address),
+	mpl3115a2_read_address,
+	&mpl3115a2_result,
+	EVENT_MPL3115A2_I2C_COMPLETE,
+	&mpl3115a2_sample_transaction2
+};
+struct i2c_transaction mpl3115a2_sample_transaction2 = {
+	(0x60 << 1) + 1, /* read */
+	5,
+	mpl3115a2_sample_data,
+	&mpl3115a2_result,
+	EVENT_MPL3115A2_I2C_COMPLETE,
+	NULL
+};
+
+void mpl3115a2_event_handler(void);
+
+bool mpl3115a2_init(void)
+{
+	event_register(EVENT_MPL3115A2_I2C_COMPLETE, mpl3115a2_event_handler);
+
+	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
+	    return i2c_start_transaction(&mpl3115a2_sample_transaction);
+}
+
+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;
+
+	mpl3115a2_result = I2C_IN_PROGRESS;
+
+	sensors_sample_done();
+
+	/* If we just started a conversion, our job is done for now */
+	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
+}
+

mpl3115a2.h

@@ -0,0 +1,10 @@
+#ifndef __MPL3115A2_H
+#define __MPL3115A2_H
+
+#include "types.h"
+
+bool mpl3115a2_init(void);
+bool mpl3115a2_start_sample(void);
+void mpl3115a2_write_log(void);
+
+#endif /* __MPL3115A2_H */

mpu6050.c

@@ -57,6 +57,16 @@ struct i2c_transaction mpu6050_whoami_transaction2 = {
 
 unsigned char mpu6050_init_command[] = {0x6B, 0x01};
 unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)};
+unsigned char mpu6050_bypass_init_command[] = {0x37, 0x02};
+
+struct i2c_transaction mpu6050_bypass_init_transaction = {
+	(0x68 << 1) + 0, /* write */
+	2,
+	mpu6050_bypass_init_command,
+	&mpu6050_result,
+	EVENT_MPU6050_I2C_COMPLETE,
+	NULL
+};
 
 struct i2c_transaction mpu6050_accel_init_transaction = {
 	(0x68 << 1) + 0, /* write */
@@ -64,7 +74,7 @@ struct i2c_transaction mpu6050_accel_init_transaction = {
 	mpu6050_accel_init_command,
 	&mpu6050_result,
 	EVENT_MPU6050_I2C_COMPLETE,
-	NULL
+	&mpu6050_bypass_init_transaction
 };
 
 struct i2c_transaction mpu6050_init_transaction = {

sensors.c

@@ -1,6 +1,8 @@
 /* sensors.c */
 
 #include "mpu6050.h"
+#include "hmc5883l.h"
+#include "mpl3115a2.h"
 #include "dcm.h"
 #include "fisqrt.h"
 #include "watchdog.h"
@@ -11,10 +13,19 @@
 #include "log.h"
 #include "stick.h"
 
-bool (*sensor_start_fns[])(void) = {
+bool (*sensor_init_fns[])(void) = {
-	mpu6050_start_sample,
+	mpu6050_init,
+	hmc5883l_init,
+	mpl3115a2_init,
 };
 
+bool (*sensor_start_fns[])(void) = {
+	mpu6050_start_sample,
+	hmc5883l_start_sample,
+	mpl3115a2_start_sample,
+};
+
+#define SENSOR_INIT_FNS (sizeof(sensor_init_fns)/sizeof(sensor_init_fns[0]))
 #define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0]))
 
 static unsigned int next_sensor;
@@ -53,10 +64,13 @@ void sensors_dump(void);
 
 bool sensors_init(void)
 {
+	unsigned int i;
+
 	next_sensor = 0;
 
-	if (!mpu6050_init())
+	for (i = 0; i < SENSOR_INIT_FNS; i++)
-		return FALSE;
+		if (!(sensor_init_fns[i])())
+			return FALSE;
 
 	return TRUE;
 }
@@ -65,7 +79,7 @@ bool sensors_next_sample(void)
 {
 	bool result;
 
-	result = (sensor_start_fns[next_sensor])();
+	result = (sensor_start_fns[next_sensor++])();
 	if (next_sensor >= SENSOR_START_FNS)
 		next_sensor = 0;
 

stick.c

@@ -152,6 +152,10 @@ void stick_input(void) {
 		if (throttle < 0.0)
 			throttle = 0.0;
 	} else {
+		log_put_uint16(0);
+		log_put_uint16(0);
+		log_put_uint16(0);
+		log_put_uint16(0);
 		x = 0.0f;
 		y = 0.0f;
 		z = 0.0f;