gyro: Fix non-blocking start

The I2C bus may not be free when starting the gyro. Add two additional states.

1 files changed, 31 insertions, 23 deletions

diff --git a/gyro.c b/gyro.c
index 943f654..a533e3f 100644
--- a/gyro.c
+++ b/gyro.c
@@ -26,7 +26,7 @@ static volatile int16_t zval = 0;
 static int32_t zaccum = 0;
 /* calculated zticks */
 static int16_t zticks = 0;
-static enum {STOPPED = 0, STARTING, STOPPING, READING, IDLE} state = STOPPED;
+static enum {STOPPED = 0, START_REQUEST, STARTING, STOP_REQUEST, STOPPING, READING, IDLE} state = STOPPED;
 
 /* data ready interrupt
  */
@@ -50,33 +50,13 @@ void gyroInit () {
 }
 
 void gyroStart () {
-	/* configuration:
-	 * disable power-down-mode, enable z
-	 * defaults
-	 * high-active, push-pull, drdy on int2
-	 * select 2000dps
-	 */
-	static uint8_t data[] = {0b00001100, 0b0, 0b00001000, 0b00110000};
-
 	assert (state == STOPPED);
-
-	const bool ret = twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
-			sizeof (data)/sizeof (*data));
-	assert (ret);
-	state = STARTING;
+	state = START_REQUEST;
 }
 
 void gyroStop () {
-	/* enable power-down mode */
-	static uint8_t data[] = {0b00000000};
-
-	/* XXX: there might be a race-condition here */
 	assert (state == IDLE);
-
-	const bool ret = twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
-			sizeof (data)/sizeof (*data));
-	assert (ret);
-	state = STOPPING;
+	state = STOP_REQUEST;
 }
 
 /*	calculate ticks for z rotation
@@ -102,6 +82,22 @@ static void gyroProcessTicks () {
  */
 bool gyroProcess () {
 	switch (state) {
+		case START_REQUEST:
+			if (twr.status == TWST_OK) {
+				/* configuration:
+				 * disable power-down-mode, enable z
+				 * defaults
+				 * high-active, push-pull, drdy on int2
+				 * select 2000dps
+				 */
+				static uint8_t data[] = {0b00001100, 0b0, 0b00001000, 0b00110000};
+				const bool ret = twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
+						sizeof (data)/sizeof (*data));
+				assert (ret);
+				state = STARTING;
+			}
+			break;
+
 		case STARTING:
 			if (shouldWakeup (WAKE_I2C)) {
 				disableWakeup (WAKE_I2C);
@@ -109,6 +105,18 @@ bool gyroProcess () {
 			}
 			break;
 
+		case STOP_REQUEST:
+			if (twr.status == TWST_OK) {
+				/* enable power-down mode */
+				static uint8_t data[] = {0b00000000};
+
+				const bool ret = twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
+						sizeof (data)/sizeof (*data));
+				assert (ret);
+				state = STOPPING;
+			}
+			break;
+
 		case STOPPING:
 			if (shouldWakeup (WAKE_I2C)) {
 				disableWakeup (WAKE_I2C);