4 files changed, 29 insertions, 14 deletions

diff --git a/accel.c b/accel.c
index ed2aa81..6d6d9d2 100644
--- a/accel.c
+++ b/accel.c
@@ -15,6 +15,7 @@
 /* registers */
 #define LIS302DL_WHOAMI 0xf
 #define LIS302DL_CTRLREG1 0x20
+#define LIS302DL_CTRLREG2 0x21
 #define LIS302DL_UNUSED1 0x28
 #define LIS302DL_OUTZ 0x2D
 
@@ -84,6 +85,7 @@ void accelStart () {
 		puts ("cannot start write");
 	}
 	sleepwhile (twr.status == TWST_WAIT);
+	assert (twr.status == TWST_OK);
 	puts ("accelStart done");
 	disableWakeup (WAKE_I2C);
 }
diff --git a/gyro.c b/gyro.c
index fdf2d1f..ac9ec1c 100644
--- a/gyro.c
+++ b/gyro.c
@@ -17,6 +17,7 @@
 #define L3GD20_CTRLREG1 0x20
 #define L3GD20_CTRLREG3 0x22
 #define L3GD20_CTRLREG4 0x23
+#define L3GD20_CTRLREG5 0x24
 #define L3GD20_OUTZ 0x2c
 
 /* raw z value */
@@ -65,6 +66,7 @@ void gyroStart () {
 		puts ("cannot start write");
 	}
 	sleepwhile (twr.status == TWST_WAIT);
+	assert (twr.status == TWST_OK);
 	puts ("gyroStart done");
 	disableWakeup (WAKE_I2C);
 }
diff --git a/main.c b/main.c
index b9bb5ce..a69a045 100644
--- a/main.c
+++ b/main.c
@@ -37,24 +37,10 @@ int main () {
 	gyroInit ();
 	accelInit ();
 	pwmInit ();
-	pwmStart ();
 	set_sleep_mode (SLEEP_MODE_IDLE);
 
-	puts ("initialization done");
-
-	/* global interrupt enable */
 	sei ();
-	gyroStart ();
-	accelStart ();
-
 	uiLoop ();
-
-	timerStop ();
-	pwmStop ();
-
-	puts ("stopped");
-
-	/* global interrupt disable */
 	cli ();
 
 	while (1);
diff --git a/ui.c b/ui.c
index 9175133..b9528a4 100644
--- a/ui.c
+++ b/ui.c
@@ -149,6 +149,8 @@ static void doSelectFine () {
 /*	Idle function, waits for timer start or select commands
  */
 static void doIdle () {
+	pwmSet (horizonLed (0), 1);
+
 	if (horizonChanged) {
 		/* start timer */
 		for (uint8_t i = 0; i < PWM_LED_COUNT; i++) {
@@ -244,6 +246,11 @@ static void doInit () {
 	/* get initial orientation */
 	h = accelGetHorizon ();
 	if (h != HORIZON_NONE) {
+		/* init done */
+		for (uint8_t i = 0; i < PWM_LED_COUNT; i++) {
+			pwmSet (horizonLed (i), PWM_OFF);
+		}
+
 		mode = UIMODE_IDLE;
 		puts ("init->idle");
 
@@ -319,6 +326,21 @@ void uiLoop () {
 	}
 #endif
 
+	/* startup, test all LED’s */
+	pwmStart ();
+	for (uint8_t i = 0; i < PWM_LED_COUNT; i++) {
+		pwmSet (i, PWM_ON);
+	}
+	gyroStart ();
+	pwmSet (0, PWM_OFF);
+	accelStart ();
+	pwmSet (1, PWM_OFF);
+
+	/* make sure data is read even when missing the first interrupt (i.e.
+	 * reboot) */
+	enableWakeup (WAKE_ACCEL);
+	enableWakeup (WAKE_GYRO);
+
 	while (1) {
 		processSensors ();
 
@@ -374,5 +396,8 @@ void uiLoop () {
 				accelval[1], accelval[3], accelval[5]);
 #endif
 	}
+
+	timerStop ();
+	pwmStop ();
 }