Use the timer to kick off WMP readings regularly.

i2c.c

@@ -1,6 +1,7 @@
 
 #include "i2c.h"
 #include "interrupt.h"
+#include "event.h"
 
 #define I2CBASE  0xE001C000
 
@@ -108,6 +109,7 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void)
 				i2c_transaction = NULL;
 				IREG(I2CONSET) = STOFLAG;
 				IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG;
+				event_set(EVENT_I2C_COMPLETE);
 			}
 		}
 		break;
@@ -138,6 +140,7 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void)
 			i2c_transaction = NULL;
 			IREG(I2CONSET) = STOFLAG;
 			IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG;
+			event_set(EVENT_I2C_COMPLETE);
 		}
 		break;
 
@@ -150,6 +153,7 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void)
 		i2c_transaction = NULL;
 		IREG(I2CONSET) = STOFLAG;
 		IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG;
+		event_set(EVENT_I2C_COMPLETE);
 		break;
 
 	/* We don't handle slave mode */

main.c

@@ -159,6 +159,11 @@ void average_sample(void)
 	putstr(")\r\n");
 }
 
+void timer_event_handler(void)
+{
+	wmp_start_sample();
+}
+
 void menu_handler(void);
 
 int main(void) {
@@ -170,6 +175,10 @@ int main(void) {
 
 	event_register(EVENT_UART_INPUT, menu_handler);
 
+	event_register(EVENT_I2C_COMPLETE, wmp_event_handler);
+
+	event_register(EVENT_TIMER, timer_event_handler);
+
 	putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n");
 
 	putstr("prompt> ");
@@ -268,7 +277,7 @@ void menu_handler(void)
 			break;
 		case 'P':
 			putstr("Initialising timer... ");
-			timer_set_period(10000*TIMER_MS);
+			timer_set_period(10*TIMER_MS);
 			reply("done");
 			break;
 		case 'E':

timer.c

@@ -58,7 +58,6 @@ void init_timer(void)
 	TWREG(PC) = 0;
 
 	TREG(TCR) = TCR_ENABLE;
-	event_register(EVENT_TIMER, timer_event_handler);
 }
 
 unsigned int timer_read(void)
@@ -74,9 +73,10 @@ void timer_delay_clocks(unsigned int clocks)
 
 void timer_set_period(unsigned int period)
 {
+	interrupt_register(TIMER0, timer_interrupt_handler);
 	TWREG(MR0) = period;
 	TWREG(MCR) = MR0I | MR0R;
-	interrupt_register(TIMER0, timer_interrupt_handler);
+	TWREG(TC) = 0;
 }
 
 void __attribute__((interrupt("IRQ"))) timer_interrupt_handler(void)
@@ -87,15 +87,8 @@ void __attribute__((interrupt("IRQ"))) timer_interrupt_handler(void)
 
 	if (ir & (1<<0)) {
 		/* Match channel 0 */
-		putstr(" *timer0* ");
-	}
-
 		event_set(EVENT_TIMER);
+	}
 
 	interrupt_clear();
 }
-
-void timer_event_handler(void)
-{
-	putstr(" *t0event* ");
-}

wmp.c

@@ -1,10 +1,12 @@
 
 #include "wmp.h"
 #include "i2c.h"
+#include "uart.h"
 
 unsigned char wmp_init_command[2] = {0xfe, 0x04};
 
 i2c_result wmp_result;
+unsigned int wmp_generation;
 
 struct i2c_transaction wmp_init_transaction = {
 	(0x53 << 1) + 0, /* write */
@@ -105,6 +107,8 @@ bool wmp_sample(void)
 	if (wmp_result != I2C_SUCCESS)
 		return FALSE;
 
+	wmp_result = I2C_IN_PROGRESS;
+
 	wmp_yaw   = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0];
 	wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1];
 	wmp_roll  = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2];
@@ -117,3 +121,36 @@ bool wmp_sample(void)
 	return TRUE;
 }
 
+bool wmp_start_sample(void)
+{
+	return i2c_start_transaction(&wmp_sample_transaction);
+}
+
+void wmp_event_handler(void)
+{
+	if (wmp_result != I2C_SUCCESS)
+		return;
+
+	wmp_result = I2C_IN_PROGRESS;
+
+	wmp_yaw   = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0];
+	wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1];
+	wmp_roll  = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2];
+
+	/* XXX We don't take into account the fast/slow mode flag here */
+	wmp_yaw_fast = !(wmp_sample_data[3] & 0x2);
+	wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
+	wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
+
+	wmp_generation++;
+	if ((wmp_generation % 100) == 0) {
+		putstr("(");
+		puthex(wmp_roll);
+		putstr(", ");
+		puthex(wmp_pitch);
+		putstr(", ");
+		puthex(wmp_yaw);
+		putstr(")\r\n");
+
+	}
+}

wmp.h

@@ -16,5 +16,7 @@ extern bool wmp_roll_fast;
 bool wmp_init(void);
 bool wmp_sample(void);
 bool wmp_read_calibration_data(void);
+bool wmp_start_sample(void);
+void wmp_event_handler(void);
 
 #endif /* __WMP_H */