1 files changed, 43 insertions, 19 deletions

diff --git a/src/sensor_adxl345.c b/src/sensor_adxl345.c
index 1e2cb9e5..309b3ff1 100644
--- a/src/sensor_adxl345.c
+++ b/src/sensor_adxl345.c
@@ -27,6 +27,7 @@ enum {
 
 static struct task_wake adxl345_wake;
 
+// Event handler that wakes adxl345_task() periodically
 static uint_fast8_t
 adxl345_event(struct timer *timer)
 {
@@ -56,6 +57,27 @@ adxl_report(struct adxl345 *ax, uint8_t oid)
     ax->sequence++;
 }
 
+// Report buffer and fifo status
+static void
+adxl_status(struct adxl345 *ax, uint_fast8_t oid
+            , uint32_t time1, uint32_t time2, uint_fast8_t fifo)
+{
+    sendf("adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+          " buffered=%c fifo=%c limit_count=%hu"
+          , oid, time1, time2-time1, ax->sequence
+          , ax->data_count, fifo, ax->limit_count);
+}
+
+// Helper code to reschedule the adxl345_event() timer
+static void
+adxl_reschedule_timer(struct adxl345 *ax)
+{
+    irq_disable();
+    ax->timer.waketime = timer_read_time() + ax->rest_ticks;
+    sched_add_timer(&ax->timer);
+    irq_enable();
+}
+
 // Chip registers
 #define AR_POWER_CTL   0x2D
 #define AR_DATAX0      0x32
@@ -74,7 +96,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
     if (ax->data_count + 6 > ARRAY_SIZE(ax->data))
         adxl_report(ax, oid);
     uint_fast8_t fifo_status = msg[8] & ~0x80; // Ignore trigger bit
-    if (fifo_status >= 31 && ax->limit_count != 0xffff)
+    if (fifo_status >= 31)
         ax->limit_count++;
     if (fifo_status > 1 && fifo_status <= 32) {
         // More data in fifo - wake this task again
@@ -83,10 +105,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
         // Sleep until next check time
         sched_del_timer(&ax->timer);
         ax->flags &= ~AX_PENDING;
-        irq_disable();
-        ax->timer.waketime = timer_read_time() + ax->rest_ticks;
-        sched_add_timer(&ax->timer);
-        irq_enable();
+        adxl_reschedule_timer(ax);
     }
 }
 
@@ -97,15 +116,8 @@ adxl_start(struct adxl345 *ax, uint8_t oid)
     sched_del_timer(&ax->timer);
     ax->flags = AX_RUNNING;
     uint8_t msg[2] = { AR_POWER_CTL, 0x08 };
-    uint32_t start1_time = timer_read_time();
     spidev_transfer(ax->spi, 0, sizeof(msg), msg);
-    irq_disable();
-    uint32_t start2_time = timer_read_time();
-    ax->timer.waketime = start2_time + ax->rest_ticks;
-    sched_add_timer(&ax->timer);
-    irq_enable();
-    sendf("adxl345_start oid=%c start1_clock=%u start2_clock=%u"
-          , oid, start1_time, start2_time);
+    adxl_reschedule_timer(ax);
 }
 
 // End measurements
@@ -123,18 +135,18 @@ adxl_stop(struct adxl345 *ax, uint8_t oid)
     uint_fast8_t i;
     for (i=0; i<33; i++) {
         msg[0] = AR_FIFO_STATUS | AM_READ;
-        msg[1] = 0;
+        msg[1] = 0x00;
         spidev_transfer(ax->spi, 1, sizeof(msg), msg);
-        if (!(msg[1] & 0x3f))
+        uint_fast8_t fifo_status = msg[1] & ~0x80;
+        if (!fifo_status)
             break;
-        adxl_query(ax, oid);
+        if (fifo_status <= 32)
+            adxl_query(ax, oid);
     }
     // Report final data
     if (ax->data_count)
         adxl_report(ax, oid);
-    sendf("adxl345_end oid=%c end1_clock=%u end2_clock=%u"
-          " limit_count=%hu sequence=%hu"
-          , oid, end1_time, end2_time, ax->limit_count, ax->sequence);
+    adxl_status(ax, oid, end1_time, end2_time, msg[1]);
 }
 
 void
@@ -160,6 +172,18 @@ DECL_COMMAND(command_query_adxl345,
              "query_adxl345 oid=%c clock=%u rest_ticks=%u");
 
 void
+command_query_adxl345_status(uint32_t *args)
+{
+    struct adxl345 *ax = oid_lookup(args[0], command_config_adxl345);
+    uint8_t msg[2] = { AR_FIFO_STATUS | AM_READ, 0x00 };
+    uint32_t time1 = timer_read_time();
+    spidev_transfer(ax->spi, 1, sizeof(msg), msg);
+    uint32_t time2 = timer_read_time();
+    adxl_status(ax, args[0], time1, time2, msg[1]);
+}
+DECL_COMMAND(command_query_adxl345_status, "query_adxl345_status oid=%c");
+
+void
 adxl345_task(void)
 {
     if (!sched_check_wake(&adxl345_wake))