diff options
| -rw-r--r-- | gyro.c | 98 | ||||
| -rw-r--r-- | gyro.h | 1 | ||||
| -rw-r--r-- | ui.c | 3 |
3 files changed, 67 insertions, 35 deletions
@@ -26,8 +26,7 @@ static volatile int16_t zval = 0; static int32_t zaccum = 0; /* calculated zticks */ static int16_t zticks = 0; -/* currently reading from i2c */ -static bool reading = false; +static enum {STOPPED = 0, STARTING, STOPPING, READING, IDLE} state = STOPPED; /* data ready interrupt */ @@ -50,7 +49,6 @@ void gyroInit () { PCMSK0 = (1 << PCINT1); } -/* XXX: make nonblocking */ void gyroStart () { /* configuration: * disable power-down-mode, enable z @@ -58,16 +56,27 @@ void gyroStart () { * high-active, push-pull, drdy on int2 * select 2000dps */ - uint8_t data[] = {0b00001100, 0b0, 0b00001000, 0b00110000}; + static uint8_t data[] = {0b00001100, 0b0, 0b00001000, 0b00110000}; - if (!twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data, - sizeof (data)/sizeof (*data))) { - puts ("cannot start write"); - } - sleepwhile (twr.status == TWST_WAIT); - assert (twr.status == TWST_OK); - puts ("gyroStart done"); - disableWakeup (WAKE_I2C); + assert (state == STOPPED); + + const bool ret = twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data, + sizeof (data)/sizeof (*data)); + assert (ret); + state = STARTING; +} + +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; } /* calculate ticks for z rotation @@ -92,31 +101,52 @@ static void gyroProcessTicks () { /* process gyro sensor data, returns true if new data is available */ bool gyroProcess () { - if (reading && shouldWakeup (WAKE_I2C)) { - disableWakeup (WAKE_I2C); - reading = false; - if (twr.status == TWST_OK) { - /* new data transfered, process it */ - /* poor man's noise filter */ - if (abs (zval) > 64) { - zaccum += zval; + switch (state) { + case STARTING: + if (shouldWakeup (WAKE_I2C)) { + disableWakeup (WAKE_I2C); + state = IDLE; } - gyroProcessTicks (); - return true; - } else if (twr.status == TWST_ERR) { - puts ("gyro i2c error"); - } - } else { - if (shouldWakeup (WAKE_GYRO) && twr.status == TWST_OK) { - /* new data available in device buffer and bus is free */ - if (!twRequest (TWM_READ, L3GD20, L3GD20_OUTZ, - (uint8_t *) &zval, sizeof (zval))) { - puts ("cannot start read"); - } else { + break; + + case STOPPING: + if (shouldWakeup (WAKE_I2C)) { + disableWakeup (WAKE_I2C); + state = STOPPED; + } + break; + + case READING: + if (shouldWakeup (WAKE_I2C)) { + disableWakeup (WAKE_I2C); + state = IDLE; + assert (twr.status != TWST_ERR); + if (twr.status == TWST_OK) { + /* new data transfered, process it */ + /* poor man's noise filter */ + if (abs (zval) > 64) { + zaccum += zval; + } + gyroProcessTicks (); + return true; + } + } + break; + + case IDLE: + if (shouldWakeup (WAKE_GYRO) && twr.status == TWST_OK) { + /* new data available in device buffer and bus is free */ + const bool ret = twRequest (TWM_READ, L3GD20, L3GD20_OUTZ, + (uint8_t *) &zval, sizeof (zval)); + assert (ret); /* wakeup source is disabled by isr to prevent race condition */ - reading = true; + state = READING; } - } + break; + + default: + /* ignore */ + break; } return false; @@ -6,6 +6,7 @@ void gyroInit (); void gyroStart (); +void gyroStop (); bool gyroProcess (); void gyroResetAccum (); int32_t gyroGetZAccum (); @@ -156,6 +156,7 @@ static void doSelectFine () { /* stop selection */ accelResetShakeCount (); speakerStart (SPEAKER_BEEP); + gyroStop (); puts ("selectfine->idle"); enterIdle (); @@ -201,6 +202,7 @@ static void doIdle () { } else if (accelGetShakeCount () >= 2) { /* set timer */ accelResetShakeCount (); + gyroStart (); mode = UIMODE_SELECT_COARSE; puts ("idle->select"); speakerStart (SPEAKER_BEEP); @@ -345,7 +347,6 @@ void uiLoop () { for (uint8_t i = 0; i < PWM_LED_COUNT; i++) { pwmSet (i, PWM_ON); } - gyroStart (); pwmSet (0, PWM_OFF); accelStart (); pwmSet (1, PWM_OFF); |