aboutsummaryrefslogtreecommitdiff
path: root/gyro.c
diff options
context:
space:
mode:
Diffstat (limited to 'gyro.c')
-rw-r--r--gyro.c31
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: