motan: Add support for dumping and analyzing adxl345 data

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 51 insertions, 0 deletions

diff --git a/scripts/motan/data_logger.py b/scripts/motan/data_logger.py
index 3f23b2e8..cd64e465 100755
--- a/scripts/motan/data_logger.py
+++ b/scripts/motan/data_logger.py
@@ -150,6 +150,13 @@ class DataLogger:
         for stepper in motion_report.get("steppers", []):
             self.send_subscribe("stepq:" + stepper,
                                 "motion_report/dump_stepper", {"name": stepper})
+        # Subscribe to additional sensor data
+        config = status["configfile"]["settings"]
+        for cfgname in config.keys():
+            if cfgname == "adxl345" or cfgname.startswith("adxl345 "):
+                aname = cfgname.split()[-1]
+                self.send_subscribe("adxl345:" + aname, "adxl345/dump_adxl345",
+                                    {"sensor": aname})
     def handle_dump(self, msg, raw_msg):
         msg_id = msg["id"]
         self.db.setdefault("subscriptions", {})[msg_id] = msg["result"]
diff --git a/scripts/motan/readlog.py b/scripts/motan/readlog.py
index 68f460be..476abbee 100644
--- a/scripts/motan/readlog.py
+++ b/scripts/motan/readlog.py
@@ -232,6 +232,50 @@ class HandleStepQ:
                 step_data.append((step_time, step_halfpos, step_pos))
 LogHandlers["stepq"] = HandleStepQ
 
+# Extract accelerometer data
+class HandleADXL345:
+    SubscriptionIdParts = 2
+    ParametersMin = ParametersMax = 2
+    DataSets = [
+        ('adxl345(<name>,<axis>)', 'Accelerometer for given axis (x, y, or z)'),
+    ]
+    def __init__(self, lmanager, name, name_parts):
+        self.name = name
+        self.adxl_name = name_parts[1]
+        self.jdispatch = lmanager.get_jdispatch()
+        self.next_accel_time = self.last_accel_time = 0.
+        self.next_accel = self.last_accel = (0., 0., 0.)
+        self.cur_data = []
+        self.data_pos = 0
+        if name_parts[2] not in 'xyz':
+            raise error("Unknown adxl345 data selection '%s'" % (name,))
+        self.axis = 'xyz'.index(name_parts[2])
+    def get_label(self):
+        label = '%s %s acceleration' % (self.adxl_name, 'xyz'[self.axis])
+        return {'label': label, 'units': 'Acceleration\n(mm/s^2)'}
+    def pull_data(self, req_time):
+        axis = self.axis
+        while 1:
+            if req_time <= self.next_accel_time:
+                adiff = self.next_accel[axis] - self.last_accel[axis]
+                tdiff = self.next_accel_time - self.last_accel_time
+                rtdiff = req_time - self.last_accel_time
+                return self.last_accel[axis] + rtdiff * adiff / tdiff
+            if self.data_pos >= len(self.cur_data):
+                # Read next data block
+                jmsg = self.jdispatch.pull_msg(req_time, self.name)
+                if jmsg is None:
+                    return 0.
+                self.cur_data = jmsg['data']
+                self.data_pos = 0
+                continue
+            self.last_accel = self.next_accel
+            self.last_accel_time = self.next_accel_time
+            self.next_accel_time, x, y, z = self.cur_data[self.data_pos]
+            self.next_accel = (x, y, z)
+            self.data_pos += 1
+LogHandlers["adxl345"] = HandleADXL345
+
 
 ######################################################################
 # Log reading