gyro: Async stop request

Fixes assertion failure in gyroStop()

1 files changed, 14 insertions, 17 deletions

diff --git a/gyro.c b/gyro.c
index cfadebf..aa0e8ed 100644
--- a/gyro.c
+++ b/gyro.c
@@ -30,11 +30,11 @@ static int16_t zticks = 0;
 #define STOPPED 0
 #define START_REQUEST 1
 #define STARTING 2
-#define STOP_REQUEST 3
 #define STOPPING 4
 #define READING 5
 #define IDLE 6
 static uint8_t state = STOPPED;
+static bool shouldStop = false;
 
 /* data ready interrupt
  */
@@ -60,11 +60,11 @@ void gyroInit () {
 void gyroStart () {
 	assert (state == STOPPED);
 	state = START_REQUEST;
+	shouldStop = false;
 }
 
 void gyroStop () {
-	assert (state == IDLE);
-	state = STOP_REQUEST;
+	shouldStop = true;
 }
 
 /*	calculate ticks for z rotation
@@ -113,18 +113,6 @@ bool gyroProcess () {
 			}
 			break;
 
-		case STOP_REQUEST: {
-			/* enable power-down mode */
-			static uint8_t data[] = {0b00000000};
-
-			const bool ret = twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
-					sizeof (data)/sizeof (*data));
-			if (ret) {
-				state = STOPPING;
-			}
-			break;
-		}
-
 		case STOPPING:
 			if (shouldWakeup (WAKE_I2C)) {
 				disableWakeup (WAKE_I2C);
@@ -149,12 +137,21 @@ bool gyroProcess () {
 			break;
 
 		case IDLE:
-			/* new data available in device buffer and bus is free */
-			if (shouldWakeup (WAKE_GYRO) && twRequest (TWM_READ, L3GD20,
+			if (shouldStop) {
+				/* enable power-down mode */
+				static uint8_t data[] = {0b00000000};
+
+				if (twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
+						sizeof (data)/sizeof (*data))) {
+					state = STOPPING;
+				}
+			} else if (shouldWakeup (WAKE_GYRO) && twRequest (TWM_READ, L3GD20,
 						L3GD20_OUTZ, (uint8_t *) &zval, sizeof (zval))) {
+				/* new data available in device buffer and bus is free */
 				/* wakeup source is disabled by isr to prevent race condition */
 				state = READING;
 			}
+
 			break;
 
 		default: