Let's have an LED driving module so that we can output different patterns for

status indications etc.

Makefile

@@ -4,7 +4,7 @@ NAME=quad
 
 SSRCS=crt0.s
 CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
-CSRCS+=fisqrt.c stick.c trig.c motor.c
+CSRCS+=fisqrt.c stick.c trig.c motor.c led.c
 
 #PROJOPTS=-DUSE_UART -DSEND_DCM
 

led.c

@@ -0,0 +1,51 @@
+/* led.c */
+
+#include "led.h"
+#include "timer.h"
+
+#define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014))
+
+led_pattern *led_current_pattern;
+led_pattern *led_current_pointer;
+unsigned int led_next_time;
+bool led_next_state;
+
+led_pattern led_pattern_active[] = {250, 250, 0};
+
+void led_set(bool on)
+{
+	if (on)
+		FP0XVAL |= 0x04000000;
+	else
+		FP0XVAL &= ~0x04000000;
+}
+
+void led_update(void)
+{
+	unsigned int time = timer_read();
+
+	/* This should never be possible, but let's just be sure. */
+	if (!led_current_pattern)
+		return;
+	if (!led_current_pointer)
+		return;
+
+	if (((signed int) (((signed int)led_next_time)-time)) > 0)
+		return;
+
+	led_set(led_next_state);
+	led_next_state = !led_next_state;
+	led_next_time += *led_current_pointer * TIMER_MS;
+	led_current_pointer++;
+	if (*led_current_pointer == 0)
+		led_current_pointer = led_current_pattern;
+}
+
+void led_set_pattern(led_pattern *pattern)
+{
+	led_current_pattern = pattern;
+	led_current_pointer = pattern;
+	led_next_state = TRUE;
+	led_next_time = timer_read();
+	led_update();
+}

led.h

@@ -0,0 +1,11 @@
+/* led.h */
+
+#include "timer.h"
+
+typedef unsigned int led_pattern;
+
+extern led_pattern led_pattern_active[];
+
+void led_set(bool on);
+void led_update(void);
+void led_set_pattern(led_pattern *pattern);

main.c

@@ -7,6 +7,7 @@
 #include "interrupt.h"
 #include "event.h"
 #include "stick.h"
+#include "led.h"
 
 #define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
 #define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
@@ -196,11 +197,11 @@ int main(void) {
 
 	putstr("prompt> ");
 
-	FP0XVAL &= ~0x04000000;
+	led_set(FALSE);
 	timer_delay_ms(1000);
-	FP0XVAL |= 0x04000000;
+	led_set(TRUE);
 	timer_delay_ms(1000);
-	FP0XVAL &= ~0x04000000;
+	led_set(FALSE);
 	if (!wmp_init())
 		putstr("WMP initialisation failed\r\n");
 
@@ -208,11 +209,11 @@ int main(void) {
 	timer_set_period(5*TIMER_MS);
 	wmp_start_zero();
 
-	FP0XVAL |= 0x04000000;
+	led_set_pattern(led_pattern_active);
 
 	/* Good luck! */
 	while (1) {
-		FP0XVAL ^= 0x04000000;
+		led_update();
 		event_dispatch();
 	}
 

timer.c

@@ -128,7 +128,7 @@ void init_timer(void)
 
 unsigned int timer_read(void)
 {
-	return TWREG(TC);
+	return T1WREG(TC);
 }
 
 void timer_delay_clocks(unsigned int clocks)