enode-host 0.1.0__py3-none-any.whl

enode_host/protocol.py

@@ -0,0 +1,311 @@
+import enum
+import struct
+from dataclasses import dataclass
+from typing import Optional
+
+
+class MsgType(enum.IntEnum):
+    COMMAND = 0x10
+    STATUS = 0x20
+    ACK = 0x30
+    DATA = 0x40
+    PPS = 0x41
+    SD_STREAM = 0x50
+    SD_DONE = 0x51
+    GNSS = 0x60
+
+
+class CommandId(enum.IntEnum):
+    START_DAQ = 0x01
+    STOP_DAQ = 0x02
+    SET_MODE = 0x03
+    START_REALTIME_STREAM = 0x04
+    STOP_REALTIME_STREAM = 0x05
+    START_PAST_STREAM = 0x06
+    STOP_PAST_STREAM = 0x07
+    START_SD_STREAM = 0x08
+    STOP_SD_STREAM = 0x09
+    SD_CLEAR = 0x0A
+    SET_DAQ_MODE = 0x0B
+
+
+class Mode(enum.IntEnum):
+    REALTIME = 0
+    PAST = 1
+
+
+class Toggle(enum.IntEnum):
+    STOP = 0
+    START = 1
+
+
+@dataclass
+class StatusReport:
+    node_type: int
+    node_number: int
+    level: int
+    parent_mac: bytes
+    self_mac: bytes
+    rssi: int
+    acc_model: int = 0
+    daq_mode: int = 0
+    daq_on: int = 0
+    stream_status: int = 0
+
+
+@dataclass
+class AccSample:
+    cc: int
+    ax: float
+    ay: float
+    az: float
+
+
+@dataclass
+class AccBatch:
+    node_type: int
+    node_number: int
+    samples: list[AccSample]
+
+
+@dataclass
+class PpsReport:
+    node_type: int
+    node_number: int
+    cc: int
+    epoch: int
+
+
+@dataclass
+class SdStreamChunk:
+    node_type: int
+    node_number: int
+    file_time: int
+    offset: int
+    data: bytes
+
+
+@dataclass
+class SdStreamDone:
+    node_type: int
+    node_number: int
+    file_time: int
+    status: int
+
+
+@dataclass
+class GnssReport:
+    node_type: int
+    node_number: int
+    latitude: float
+    longitude: float
+    fix_mode: int
+    valid: int
+
+
+@dataclass
+class Ack:
+    original_type: int
+    status: int
+    message: str = ""
+
+
+def pack_frame(msg_type: int, payload: bytes) -> bytes:
+    if len(payload) > 0xFFFF:
+        raise ValueError("payload too large")
+    header = struct.pack(">BH", int(msg_type), len(payload)
+    )
+    return header + payload
+
+
+def build_command(command_id: CommandId, payload: bytes = b"") -> bytes:
+    return pack_frame(MsgType.COMMAND, bytes([int(command_id)]) + payload)
+
+
+def build_ack(original_type: int, status: int, message: str = "") -> bytes:
+    msg = message.encode("utf-8")
+    payload = struct.pack(">BBH", int(original_type), int(status), len(msg)) + msg
+    return pack_frame(MsgType.ACK, payload)
+
+
+def parse_ack(payload: bytes) -> Ack:
+    if len(payload) < 4:
+        raise ValueError(f"invalid ack payload length {len(payload)}")
+    original_type, status, msg_len = struct.unpack(">BBH", payload[:4])
+    message = payload[4:4 + msg_len].decode("utf-8", errors="replace")
+    return Ack(original_type=original_type, status=status, message=message)
+
+
+def build_sd_stream_start(hours: int) -> bytes:
+    payload = struct.pack(">H", int(hours))
+    return build_command(CommandId.START_SD_STREAM, payload)
+
+
+def build_sd_stream_stop() -> bytes:
+    return build_command(CommandId.STOP_SD_STREAM, b"")
+
+
+def build_sd_clear() -> bytes:
+    return build_command(CommandId.SD_CLEAR, b"")
+
+
+def build_set_daq_mode(mode: int) -> bytes:
+    return build_command(CommandId.SET_DAQ_MODE, bytes([int(mode) & 0xFF]))
+
+
+def parse_status(payload: bytes) -> StatusReport:
+    if len(payload) not in (16, 18, 19, 20):
+        raise ValueError(f"invalid status payload length {len(payload)}")
+    node_type = payload[0]
+    node_number = payload[1]
+    level = payload[2]
+    parent_mac = payload[3:9]
+    self_mac = payload[9:15]
+    rssi = struct.unpack(">b", payload[15:16])[0]
+    acc_model = payload[16] if len(payload) >= 18 else 0
+    daq_mode = payload[17] if len(payload) >= 18 else 0
+    daq_on = payload[18] if len(payload) >= 19 else 0
+    stream_status = payload[19] if len(payload) >= 20 else 0
+    return StatusReport(
+        node_type=node_type,
+        node_number=node_number,
+        level=level,
+        parent_mac=parent_mac,
+        self_mac=self_mac,
+        rssi=rssi,
+        acc_model=acc_model,
+        daq_mode=daq_mode,
+        daq_on=daq_on,
+        stream_status=stream_status,
+    )
+
+
+def parse_sd_chunk(payload: bytes) -> SdStreamChunk:
+    if len(payload) < 12:
+        raise ValueError(f"invalid sd chunk payload length {len(payload)}")
+    node_type = payload[0]
+    node_number = payload[1]
+    file_time = struct.unpack(">I", payload[2:6])[0]
+    offset = struct.unpack(">I", payload[6:10])[0]
+    size = struct.unpack(">H", payload[10:12])[0]
+    if len(payload) < 12 + size:
+        raise ValueError("sd chunk length mismatch")
+    data = payload[12 : 12 + size]
+    return SdStreamChunk(
+        node_type=node_type,
+        node_number=node_number,
+        file_time=file_time,
+        offset=offset,
+        data=data,
+    )
+
+
+def parse_sd_done(payload: bytes) -> SdStreamDone:
+    if len(payload) != 7:
+        raise ValueError(f"invalid sd done payload length {len(payload)}")
+    node_type = payload[0]
+    node_number = payload[1]
+    file_time = struct.unpack(">I", payload[2:6])[0]
+    status = payload[6]
+    return SdStreamDone(
+        node_type=node_type,
+        node_number=node_number,
+        file_time=file_time,
+        status=status,
+    )
+
+
+def parse_gnss(payload: bytes) -> GnssReport:
+    if len(payload) != 12:
+        raise ValueError(f"invalid gnss payload length {len(payload)}")
+    node_type = payload[0]
+    node_number = payload[1]
+    latitude, longitude = struct.unpack(">ff", payload[2:10])
+    fix_mode = payload[10]
+    valid = payload[11]
+    return GnssReport(
+        node_type=node_type,
+        node_number=node_number,
+        latitude=latitude,
+        longitude=longitude,
+        fix_mode=fix_mode,
+        valid=valid,
+    )
+
+
+_ACC_SAMPLE_STRUCT = struct.Struct(">Qfff")
+_PPS_STRUCT = struct.Struct(">Qq")
+
+
+def build_acc_batch(node_type: int, node_number: int, samples: list[AccSample]) -> bytes:
+    if len(samples) > 255:
+        raise ValueError("too many samples")
+    payload = bytearray()
+    payload.extend(bytes([node_type, node_number, len(samples)]))
+    for sample in samples:
+        payload.extend(_ACC_SAMPLE_STRUCT.pack(sample.cc, sample.ax, sample.ay, sample.az))
+    return pack_frame(MsgType.DATA, bytes(payload))
+
+
+def parse_acc_batch(payload: bytes) -> AccBatch:
+    if len(payload) < 3:
+        raise ValueError("invalid data payload length")
+    node_type = payload[0]
+    node_number = payload[1]
+    count = payload[2]
+    expected = 3 + count * _ACC_SAMPLE_STRUCT.size
+    if len(payload) != expected:
+        raise ValueError(f"invalid data payload length {len(payload)}")
+    samples: list[AccSample] = []
+    offset = 3
+    for _ in range(count):
+        cc, ax, ay, az = _ACC_SAMPLE_STRUCT.unpack_from(payload, offset)
+        samples.append(AccSample(cc=cc, ax=ax, ay=ay, az=az))
+        offset += _ACC_SAMPLE_STRUCT.size
+    return AccBatch(node_type=node_type, node_number=node_number, samples=samples)
+
+
+def build_pps(node_type: int, node_number: int, cc: int, epoch: int) -> bytes:
+    payload = bytes([node_type, node_number]) + _PPS_STRUCT.pack(cc, epoch)
+    return pack_frame(MsgType.PPS, payload)
+
+
+def parse_pps(payload: bytes) -> PpsReport:
+    if len(payload) != 18:
+        raise ValueError(f"invalid PPS payload length {len(payload)}")
+    node_type = payload[0]
+    node_number = payload[1]
+    cc, epoch = _PPS_STRUCT.unpack(payload[2:])
+    return PpsReport(node_type=node_type, node_number=node_number, cc=cc, epoch=epoch)
+
+
+def format_mac(mac: bytes) -> str:
+    return ":".join(f"{b:02X}" for b in mac)
+
+
+def build_set_mode(mode: Mode) -> bytes:
+    return build_command(CommandId.SET_MODE, bytes([int(mode)]))
+
+
+def build_start_daq() -> bytes:
+    return build_command(CommandId.START_DAQ)
+
+
+def build_stop_daq() -> bytes:
+    return build_command(CommandId.STOP_DAQ)
+
+
+def build_realtime_stream(toggle: Toggle) -> bytes:
+    command_id = (
+        CommandId.START_REALTIME_STREAM if toggle == Toggle.START else CommandId.STOP_REALTIME_STREAM
+    )
+    return build_command(command_id)
+
+
+def build_past_stream(toggle: Toggle, start_ms: Optional[int] = None, end_ms: Optional[int] = None) -> bytes:
+    if toggle == Toggle.START:
+        if start_ms is None or end_ms is None:
+            raise ValueError("start_ms and end_ms required when starting past stream")
+        payload = struct.pack(">QQ", int(start_ms), int(end_ms))
+        return build_command(CommandId.START_PAST_STREAM, payload)
+    return build_command(CommandId.STOP_PAST_STREAM)

enode_host/psd_recursive.py

@@ -0,0 +1,139 @@
+# PSD/ASD CALCULATION CLASS: RECURSIVE VERSION 
+
+# AUTHOR: 
+#   K Y KOO (k.y.koo@exeter.ac.uk)
+#
+# HISTORY
+#   - Initial version v1 on 12 Sep 2024
+#
+
+
+from numpy import exp, empty, sqrt, mean, zeros, random, sqrt, arange, sin, stack, pi, append
+from scipy.signal import windows
+from scipy.fft import fft
+from matplotlib.pyplot import figure, clf, semilogy, grid, xlabel, ylabel, legend
+
+
+class PSD_Recursive():
+
+    def __init__(self, nfft, fs):
+        
+        self.nfft = nfft
+        self.freq  = arange(0, fs/2, fs/nfft)
+        self.dt = 1/fs
+        self.X = empty((0, 3), dtype=float)
+        self.Xf = zeros((nfft, 3), dtype=float)
+        self.window = windows.hann(self.nfft) / sqrt(mean(windows.hann(self.nfft)**2))
+        self.Lambda = 0.01
+        self.w = [exp(-self.Lambda), 1 - exp(-self.Lambda)] # exponential weighting factor
+        self.n = 1 # No of terms used to get the weighted averaged PSD
+        self.isUpdated = False
+        self.last_sample_time = None
+        self.last_update_time = None
+        self.sample_dt = self.dt
+
+    def push(self, x, ts=None):
+
+        # append
+        self.X = append(self.X, x, axis = 0)
+        # print(ts[-1])
+        if ts is not None and len(ts) >= 1:
+            # print(ts[-1])
+            self.last_sample_time = ts[-1]
+            if len(ts) >= 2:
+                self.sample_dt = (ts[-1] - ts[-2]).total_seconds()
+
+        # if len() > nfft, perform fft 
+        while self.X.shape[0] > self.nfft:
+            
+            x_ = self.X[:self.nfft, :].T * self.window
+            x_f = fft(x_).T
+            # add the result to the Xf with a forgetting factor exp(-lambda)
+            weights = self.get_weights()
+            self.Xf = self.Xf * weights[0] + abs(x_f)**2 * weights[1]
+            self.X = self.X[self.nfft:, :]
+            self.isUpdated = True
+            if ts is not None and len(ts) > 0:
+                self.last_update_time = ts[-1]
+        # pass
+    
+    def get_weights(self):
+        
+        #          (n-1)/n * exp(-lambda)                 , 1 - (n-1)/n*exp(-lambda)
+        weights = [(self.n - 1)/self.n * exp(-self.Lambda), 1 - (self.n - 1)/self.n * exp(-self.Lambda)]
+        self.n += 1
+        return weights
+    
+    def get_psd(self):
+
+        self.isUpdated = False
+        return self.freq, self.Xf[:int(self.nfft/2), :] * self.dt * 2 / self.nfft
+    
+    def get_asd(self):
+
+        f, psd = self.get_psd()
+        return f, sqrt(psd)
+
+    def seconds_until_next_update(self):
+        # Estimate time until enough samples for next FFT window (data-time based).
+        remaining = max(self.nfft - self.X.shape[0], 0)
+        dt = self.sample_dt if self.sample_dt else self.dt
+        return remaining * dt
+
+    def set_fs(self, fs):
+        try:
+            fs_val = float(fs)
+        except (TypeError, ValueError):
+            return
+        if fs_val <= 0:
+            return
+        self.freq = arange(0, fs_val / 2, fs_val / self.nfft)
+        self.dt = 1 / fs_val
+        self.X = empty((0, 3), dtype=float)
+        self.Xf = zeros((self.nfft, 3), dtype=float)
+        self.window = windows.hann(self.nfft) / sqrt(mean(windows.hann(self.nfft) ** 2))
+        self.n = 1
+        self.isUpdated = False
+        self.last_sample_time = None
+        self.last_update_time = None
+        self.sample_dt = self.dt
+
+    
+        
+
+        
+
+if __name__ == '__main__':
+
+    rng = random.default_rng()
+
+    fs = 10e3
+    N = 1e5
+    amp = 2*sqrt(2)
+    freq = 1234.0
+    noise_power = 0.001 * fs / 2
+    time = arange(N) / fs
+    x = amp*sin(2*pi*freq*time)
+    x += rng.normal(scale=sqrt(noise_power), size=time.shape)
+    y = amp*sin(2*pi*1000*time)
+    z = amp*sin(2*pi*2000*time)
+    y += rng.normal(scale=sqrt(noise_power), size=time.shape)
+    z += rng.normal(scale=sqrt(noise_power), size=time.shape)
+
+    X = stack((x, y, z), axis = 0).T
+
+    NFFT = 1024
+
+    psd = PSD_Recursive(NFFT, fs)
+    for i in range(int(X.shape[0]/2)):
+        psd.push(X[2*i:2*(i+1), :])
+
+    figure()
+    clf()
+    f, psd = psd.get_psd()
+
+    semilogy(f, psd, label=['x', 'y', 'z'])
+    grid(True)
+    xlabel('Frequency (Hz)')
+    ylabel('PSD (g^2/Hz)')
+    legend()

enode_host/queues.py

@@ -0,0 +1,11 @@
+from queue import Queue
+try:
+    from . import config
+except ImportError:
+    import config
+
+Mesh2GUI = Queue(maxsize = 1000)
+Mesh2Storage = Queue(maxsize = 1000)
+RemoCons = []
+for i in range(config.NNODES):
+    RemoCons.append(Queue(maxsize = 10))   

enode_host/resampling.py

@@ -0,0 +1,206 @@
+from numpy import ceil
+import datetime
+from numpy import floor 
+import logging
+import numpy as np
+
+logger = logging.getLogger(__name__)
+
+
+class Resampling:
+    def __init__(self, SamplingFrequency):
+        self.Fs = SamplingFrequency   # Hz
+        self.Ts = int(1000/SamplingFrequency)  # msec
+        self.t0 = -1
+        self.t1 = -1
+        self.tk = -1
+        self.y0 = []
+        self.y1 = []
+
+    def get_fs(self):
+        logger.info("fs={}".format(self.Fs))
+
+    def set_fs(self, sampling_frequency):
+        try:
+            fs = float(sampling_frequency)
+        except (TypeError, ValueError):
+            return
+        if fs <= 0:
+            return
+        self.Fs = fs
+        self.Ts = int(1000 / fs)
+        # Reset state to avoid mixing old timing grid with new Fs
+        self.t0 = -1
+        self.t1 = -1
+        self.tk = -1
+        self.y0 = []
+        self.y1 = []
+    
+    @staticmethod
+    def _as_datetime(value):
+        if isinstance(value, datetime.datetime):
+            return value
+        if hasattr(value, "to_pydatetime"):
+            try:
+                return value.to_pydatetime()
+            except Exception:
+                pass
+        if isinstance(value, np.datetime64):
+            try:
+                ts = value.astype("datetime64[us]").astype("int64") / 1e6
+                return datetime.datetime.fromtimestamp(ts, tz=datetime.timezone.utc)
+            except Exception:
+                return None
+        try:
+            return datetime.datetime.fromtimestamp(float(value), tz=datetime.timezone.utc)
+        except Exception:
+            return None
+
+    def _coerce_state_time(self, value):
+        if value == -1:
+            return -1
+        if isinstance(value, datetime.datetime):
+            return value
+        coerced = self._as_datetime(value)
+        return coerced if coerced is not None else -1
+
+    def get_next_grid_time(self, t):
+        if not isinstance(t, datetime.datetime):
+            t = self._as_datetime(t)
+            if t is None:
+                return None
+        tz = t.tzinfo or datetime.timezone.utc
+        epoch = int(t.timestamp())
+        usec = t.microsecond
+        usec_k = (int(usec / (self.Ts * 1000)) + 1) * (self.Ts * 1000)
+        if usec_k >= 1000000:
+            epoch += 1
+            usec_k -= 1000000
+        tk = datetime.datetime.fromtimestamp(epoch, tz=tz)
+        tk = tk.replace(microsecond = usec_k)
+        return tk
+
+    def push(self, t, y):
+        t_rs = []
+        y_rs = []
+        for t_, y_ in zip(t, y):
+            t_dt = t_ if isinstance(t_, datetime.datetime) else self._as_datetime(t_)
+            if t_dt is None:
+                continue
+            t_ = t_dt
+            self.t0 = self._coerce_state_time(self.t0)
+            self.t1 = self._coerce_state_time(self.t1)
+            self.tk = self._coerce_state_time(self.tk)
+            if self.t0 == -1:
+                self.t0 = t_
+                self.y0 = y_
+                self.tk = self.get_next_grid_time(t_)
+                if self.tk is None:
+                    self.t0 = -1
+                    continue
+                # starting tk logging disabled per request
+            elif self.tk != -1 and t_ < self.tk:
+                self.t0 = t_
+                self.y0 = y_
+            else:
+                self.t1 = t_
+                self.y1 = y_
+                dt = (self.t1 - self.t0).total_seconds()
+                if dt <= 0:
+                    self.t0 = self.t1
+                    self.y0 = self.y1
+                    continue
+                while self.tk != -1 and self.tk < self.t1:
+                    dt0 = (self.tk - self.t0).total_seconds()
+                    dt1 = (self.t1 - self.tk).total_seconds()
+                    yk = [y0i * dt1/dt + y1i * dt0/dt for y0i, y1i in zip(self.y0, self.y1)]
+                    y_rs.append(yk)
+                    # convert timestamp float to datetime
+                    t_rs.append(self.tk)
+                    # self.tk  += datetime.timedelta(milliseconds = self.Ts)
+                    self.tk  = self.get_next_grid_time(self.tk)
+                    if self.tk is None:
+                        self.tk = -1
+                        break
+                self.t0 = self.t1
+                self.y0 = self.y1
+
+        return t_rs, y_rs
+
+
+
+
+
+if __name__ == '__main__':
+
+    # TEST 1 
+    # a = resampling(100)
+    # b = resampling(200)
+    # a.get_fs()
+    # b.get_fs()
+
+    # TEST 2
+    # buf = {}
+    # buf[1] = resampling(10)
+    # buf[2] = resampling(20)
+    # for key in buf.keys():
+    #     buf[key].get_fs()
+
+    # # TEST 3
+    # if 0:
+    #     from numpy import *
+    #     from matplotlib.pyplot import *
+    #     xi = sort(random.rand(210,1) * 0.5, axis=0)
+    #     yi = hstack((sin(2*pi*1*xi), cos(2*pi*1*xi)))
+    #     # yi = sin(2*pi*1*xi)
+    #     Fs = 70
+    #     # print(xi)
+    #     TK = []
+    #     VK = []
+    #     resampler = resampling(Fs)
+    #     for idx in range(int(xi.shape[0]/2)):
+    #         # print(xi[idx, 0])
+    #         t_, v_ = resampler.push(xi[2*idx:2*idx + 1, 0], list(yi[2*idx:2*idx + 1,:]))
+    #         for idx, t in enumerate(t_):
+    #             TK.append(t_[idx])
+    #             VK.append(v_[idx])
+    #     TK = array(TK)
+    #     VK = array(VK)
+    #     # print(TK)
+    #     # ion()
+    #     figure(1)
+    #     # clf()
+    #     plot(xi, yi, '.')
+    #     plot(TK, VK, 'o-')
+    #     show()
+    
+    # TEST 4
+    if 1:
+        import datetime 
+        from numpy import *
+        from matplotlib.pyplot import *
+
+        T = []
+        Y = []
+
+        resampler = resampling(62.5)
+        t = [datetime.datetime(2024,1,1,0,0,0, i*1000) for i in range(0, 100, 20)]
+        y = [[j, j+1] for j in range(5)]
+        
+        tr, yr = resampler.push(t, y)
+        T += tr
+        Y += yr
+
+        for t_ in T:
+            print(t_)
+
+
+        figure(1)
+        # ion()
+        plot(t,y, '.')
+        plot(T, Y, 'o-')
+        show()
+        
+
+        
+    

enode_host/shm_sigproc.py

@@ -0,0 +1,47 @@
+import threading
+import logging
+try:
+    from . import queues
+except ImportError:
+    import queues
+try:
+    from . import resampling
+except ImportError:
+    import resampling
+try:
+    from . import storage
+except ImportError:
+    import storage
+import time
+
+logger = logging.getLogger(__name__)
+
+Resamplers = {}
+Storage = storage.Storage(600) # 600 sec
+
+def update():
+    # this function 
+    # - reads the queue and 
+    # - resamples the measurement
+    # - push resampled data into storage
+
+    while True:
+        if not queues.Mesh2Storage.empty():
+            # logger.info("message received for t, a")
+            item = queues.Mesh2Storage.get()
+            nodeID = item['nodeID']  # starting from 1 representing ACC1                              
+            t = item['t']
+            y = item['acc']
+            if not nodeID in Resamplers.keys():
+                Resamplers[nodeID] = resampling.resampling(62.5)
+            t_rs, y_rs = Resamplers[nodeID].push(t, y)
+            # logger.info("resampled timestamp = {}".format(t_rs[0]))
+            Storage.push(nodeID, t_rs, y_rs)                    
+        else:
+            time.sleep(0.01)
+def begin():
+
+    # start a thread to run update() function
+    thread = threading.Thread(target = update)
+    thread.start()
+    logger.info("sigproc update started.")