Speed handling and stuff

door.py

@@ -1,11 +1,16 @@
 #!/usr/bin/python
-import os, serial, socket, subprocess, select, datetime, sys
+import os, serial, socket, select, datetime
 import paho.mqtt.client as mqcl
 import argparse
+import logging
 
-OPEN_THRESHOLD = 35
+UPDATE_RATE = 2
-CLOSED_THRESHOLD = 60
+MAX_UPDATE_RATE = 20
-CLOSED_WANT = 135
+MIN_SPEED = 5
+ERROR_THRESHOLD = 250
+OPEN_THRESHOLD = 190
+CLOSED_THRESHOLD = 160
+CLOSED_WANT = 40
 
 parser = argparse.ArgumentParser()
 parser.add_argument("--serial_port", default="/dev/serial/by-id/usb-Imaginaerraum.de_DoorControl_43363220195053573A002C0-if01")
@@ -14,33 +19,98 @@ parser.add_argument("--control_socket", default="/tmp/nfc.sock")
 parser.add_argument("--valid_tokens", default="/etc/door_tokens")
 parser.add_argument("--log_file", default="/tmp/nfc.log")
 parser.add_argument("--state_timeout", type=float, default=10)
+parser.add_argument("--state_timeout_speed", type=float, default=3)
 parser.add_argument("--repeat_time", type=float, default=5)
 parser.add_argument("--mqtt_host", default="10.10.21.2")
 
 config = parser.parse_args()
 
-mqttc = mqcl.Client()
-
 def timestamp():
     return datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S")
 
-# Log data to stdout and the log file
+class DoorControl:
-def log(*args):
+    # Actions
-    data = "%s %s" % (timestamp(), " ".join(str(i) for i in args))
+    IDLE, CLOSE, OPEN_THEN_CLOSE, OPEN, CLOSE_THEN_OPEN, ERROR = range(6)
-    lgf.write(data + "\n")
+    state_names = {
-    lgf.flush()
+        OPEN: "open",
-    print(data)
+        CLOSE: "close",
+        ERROR: "error",
+    }
+    action_names = {
+        IDLE: "idling",
+        OPEN: "waiting for open door",
+        CLOSE: "waiting for closed door",
+        OPEN_THEN_CLOSE: "waiting for open door, then closing again",
+        CLOSE_THEN_OPEN: "waiting for closed door, then opening again",
+        ERROR: "error",
+    }
 
-def mqtt(topic, msg, persistent = True):
+    def __init__(self, config):
-    mqttc.publish("door/" + topic, msg, qos=2, retain=persistent)
+        self.config = config
 
-# Opens the socket that can control the daemon
+        self.mqttc = mqcl.Client()
-# Commands are TBD
+        self.logger = self._setup_logging(config)
-def open_control_socket():
+        self._open_serial_port(config)
-    global control_socket
+        self.valid_tokens = self._read_valid_tokens(config)
-    global config
+        self.nfc_fifo = self._open_nfc_fifo(config)
-    global comm_channels
+        self.control_socket, self.comm_channels = self._open_control_socket(config)
 
+        self.mqttc.on_connect = lambda client, userdata, flags, rc: self.logger.info(f"Connected to mqtt host with result {rc}")
+        self.mqttc.connect_async(config.mqtt_host, keepalive=60)
+        self.mqttc.loop_start()
+
+        # Current door state
+        self.state = None
+        self.state_pos = 0
+        # Current target action
+        self.action = DoorControl.CLOSE
+        # Start time of the current action
+        self.start_time = None
+        # How often the action was repeated
+        self.repeats = 0
+
+        self.last_handled_state = self.OPEN
+        self.last_door_pos_time = datetime.datetime.now() - datetime.timedelta(minutes = 10)
+        self.last_position = 0
+        self.last_handle_state_timestamp = datetime.datetime.now()
+
+    def _send_door_cmd(self, cmd: bytes):
+        """Send a command to the door."""
+        if cmd != b'R': self.logger.debug(f"Sending {cmd}")
+        return self.serial_port.write(cmd)
+
+    def _read_door_line(self):
+        """Read a single line from the serial port"""
+        return self.serial_port.readline().decode('ascii')
+
+    def _setup_logging(self, config):
+        """Set up logging"""
+        log_format = '%(asctime)s - %(name)s - %(levelname)s - %(message)s'
+        logging.basicConfig(force = True,
+                            level = logging.DEBUG,
+                            format = log_format)
+
+        # create logger
+        logger = logging.getLogger('nfc_log')
+
+        # create console handler and set level to debug
+        fh = logging.FileHandler(config.log_file)
+        fh.setLevel(logging.DEBUG)
+
+        # create formatter
+        formatter = logging.Formatter(log_format)
+        fh.setFormatter(formatter)
+        logger.addHandler(fh)
+
+        return logger
+
+    def mqtt(self, topic: str, msg: str, persistent: bool = True):
+        """Publishes data to a topic"""
+        self.mqttc.publish("door/" + topic, msg, qos=2, retain=persistent)
+
+    def _open_control_socket(self, config):
+        """(Re-)creates and opens the control socket. Config must have a control_socket member."""
+        self.logger.debug("Opening control socket")
         if os.path.exists(config.control_socket):
             os.unlink(config.control_socket)
 
@@ -49,33 +119,27 @@ def open_control_socket():
         control_socket.listen(5)
         comm_channels = []
 
-# Opens the serial port to talk to the lock actuator
+        return control_socket, comm_channels
-# Might return a failure. In that case, it will be tried again later
-def open_serial_port():
-    global serial_port
-    global config
 
+    def _open_serial_port(self, config):
+        """Opens the serial port controlling the door opener. config must have a serial_port member."""
         try:
-        serial_port = serial.Serial(config.serial_port, timeout=2)
+            self.serial_port = serial.Serial(config.serial_port, timeout=2)
+            self._send_door_cmd(b'r')
         except:
             serial_port = None
-        pass
+        return self.serial_port
-
-# Detected tokens are passed in through a FIFO
-def open_nfc_fifo():
-    global nfc_fifo
-    global config
 
+    def _open_nfc_fifo(self, config):
+        """Opens config.nfc_fifo as the FIFO through which detected tokens are passed in."""
         nfc_fifo = open(config.nfc_fifo, "r")
+        return nfc_fifo
 
-# Refresh valid tokens from the configured file
+    def _read_valid_tokens(self, config):
-def read_valid_tokens():
+        """Refreshes all tokens from config.valid_tokens"""
-    global valid
-    global config
-
-    try:
-        log("Loading tokens")
         valid = {}
+        try:
+            self.logger.info("Loading tokens")
             lines =[ s.strip() for s in open(config.valid_tokens, "r").readlines() ]
             for i, line in enumerate(lines):
                 l = line.split('|')
@@ -88,181 +152,190 @@ def read_valid_tokens():
                             else:
                                 valid_thru = None
                         except Exception:
-                        log(f"Could not parse valid thru date for token {token} in line {i}")
+                            logging.error(f"Could not parse valid thru date for token {token} in line {i}")
                             valid_thru = None
-                    log(f"Got token {token} associated with {name} <{email}> of {organization}, valid thru {valid_thru}")
+                        logging.debug(f"Got token {token} associated with {name} <{email}> of {organization}, valid thru {valid_thru}")
                         if token in valid:
-                        log(f"Warning: Overwriting token {token}")
+                            logging.warning(f"Overwriting token {token}")
                         valid[token] = {'name': name, 'organization': organization, 'email': email,
                                         'valid_thru': valid_thru}
                 else:
-                log(f"Skipping line {i} ({line}) since it does not contain exactly 5 data field")
+                    logging.warning(f"Skipping line {i} ({line}) since it does not contain exactly 5 data field")
-    except:
+        except Exception as e:
             valid = {}
-        log("Error reading token file")
+            logging.error(f"Error reading token file. Exception: {e}")
-        raise
 
-# Opens the log file for writing
+        return valid
-def open_logfile():
-    global lgf
-    global config
 
-    lgf = open(config.log_file, "a+")
+    def _set_position(self, data: int):
+        """Set a new door position"""
+        if self.state_pos != data:
+            self.mqtt("position/value", data, True)
+            self.state_pos = data
+            if data > ERROR_THRESHOLD and self.state != DoorControl.ERROR:
+                self.logger.error("Invalid position:", state)
+                self.state = DoorControl.ERROR
+            elif data > OPEN_THRESHOLD and self.state != DoorControl.OPEN:
+                self.mqtt("state/value", "open", True)
+                self.state = DoorControl.OPEN
+            elif data < CLOSED_THRESHOLD and self.state != DoorControl.CLOSE:
+                self.mqtt("state/value", "closed", True)
+                self.state = DoorControl.CLOSE
 
-# Actions
+    def _check_reporting(self, current: int):
-IDLE, CLOSE, OPEN_THEN_CLOSE, OPEN, CLOSE_THEN_OPEN = range(5)
+        if current == 0:
-state_names = {
+            self.logger.info("Turning position reporting on")
-    OPEN: "open",
+            self._send_door_cmd(b'r')
-    CLOSE: "close"
+        else:
-}
+            self.logger.info("Position reporting is on")
-action_names = {
-    IDLE: "idling",
-    OPEN: "waiting for open door",
-    CLOSE: "waiting for closed door",
-    OPEN_THEN_CLOSE: "waiting for open door, then closing again",
-    CLOSE_THEN_OPEN: "waiting for closed door, then opening again"
-}
-# Current door state
-state = None
-state_pos = 0
-# Current target action
-action = CLOSE
-# Start time of the current action
-start_time = None
-# How often the action was repeated
-repeats = 0
 
-# Read the state of the door from the serial port
+    def handle_door_line(self):
-def poll_door_state():
+        """Reads a single line from the serial port and handles it"""
-    global state
+        data = self._read_door_line().strip()
-    global state_pos
-    global serial_port
-    serial_port.reset_input_buffer()
-    serial_port.write(b"R")
-    data = serial_port.readline().strip().split()
-    if data[0] == b'pos:':
-        data = int(data[1])
-        if state_pos != data:
-            mqtt("position/value", data, True)
-        state_pos = data
-        changed = False
-        if data < OPEN_THRESHOLD and state != OPEN:
-            state = OPEN
-            changed = True
-            mqtt("state/value", "open", True)
-        elif data > CLOSED_THRESHOLD and state != CLOSE:
-            state = CLOSE
-            changed = True
-            mqtt("state/value", "closed", True)
-    return state
 
-# Check the door state and send commands through a state machine
+        handling = {
-def handle_door_state():
+            "pos": (int, self._set_position),
-    global action, target, serial_port, start_time, repeats
+            "pos reporting": (int, self._check_reporting),
-    
-    # Commands associated with each target state
-    target_state_cmd = {
-        OPEN:  b'O',
-        CLOSE: b'C'
         }
 
-    old_state = state
+        try:
+            prefix, data = data.split(':')
+            if prefix in handling:
+                data_type, fun = handling[prefix]
+                fun(data_type(data))
+        except:
+            pass
+
+    def poll_door_state(self):
+        """Checks the door state if the last polling was at least 5 seconds ago, and returns the current state"""
+        t = (datetime.datetime.now() - self.last_door_pos_time).total_seconds()
+        if t >= 5:
+            self.last_door_pos_time = datetime.datetime.now()
+            self._send_door_cmd(b"R")
+            self.handle_door_line()
+        return self.state
+
+    def handle_door_state(self):
+        """Checks the door state and executes any actions necessary to reach the target state"""
+        # Commands associated with each target state
+        target_state_cmd = {
+            DoorControl.OPEN:  b'O',
+            DoorControl.CLOSE: b'C'
+        }
+
+        now = datetime.datetime.now()
+
+        old_state = self.last_handled_state
+        delta_t = (now - self.last_handle_state_timestamp).total_seconds()
+
+        if 1. / delta_t > MAX_UPDATE_RATE:
+            return
+
+        speed = abs(self.state_pos - self.last_position) / delta_t
+        self.last_position = self.state_pos
+        self.last_handle_state_timestamp = now
+        
+        if speed >= 0 or delta_t >= 1. / UPDATE_RATE:
+            self.mqtt("position/speed", f"{speed}")
 
         # If no serial port, try to open it or return
-    if not serial_port:
+        if not self.serial_port:
             try:
-            open_serial_port()
+                self._open_serial_port(self.config)
             except:
                 return
 
         # Get new state
-    poll_door_state()
+        self.poll_door_state()
+        self.last_handled_state = self.state
+
+        if self.state == DoorControl.ERROR:
+            self.logger.error("Restarting the MCU and exiting.")
+            self.send_door_cmd(b'S')
+            return
 
         # Idle + change = key?
-    if action == IDLE:
+        if self.action == DoorControl.IDLE:
-        if state != old_state:
+            if self.state != old_state:
-            log("Door changed unexpectedly:", state_names[state])
+                self.logger.info(f"Door changed unexpectedly: {DoorControl.state_names[self.state]}")
-            start_time = datetime.datetime.now()
+                self.start_time = now
-        if start_time and (datetime.datetime.now() - start_time).total_seconds() >= config.state_timeout:
+            if self.start_time and (now - self.start_time).total_seconds() >= self.config.state_timeout:
-            start_time = None
+                self.start_time = None
-            if state_pos >= CLOSED_THRESHOLD and state_pos < CLOSED_WANT:
+                if self.state_pos <= CLOSED_THRESHOLD and self.state_pos > CLOSED_WANT:
-                log("Closing door a bit more")
+                    self.logger.info("Closing door a bit more")
-                serial_port.write(target_state_cmd[CLOSE])
+                    self._send_door_cmd(target_state_cmd[DoorControl.CLOSE])
             return
 
         # Target state, next action, timeout action
         actions = {
-        OPEN:  (OPEN,  IDLE, CLOSE_THEN_OPEN ),
+            DoorControl.OPEN:  (DoorControl.OPEN,  DoorControl.IDLE, DoorControl.CLOSE_THEN_OPEN ),
-        CLOSE: (CLOSE, IDLE, OPEN_THEN_CLOSE ),
+            DoorControl.CLOSE: (DoorControl.CLOSE, DoorControl.IDLE, DoorControl.OPEN_THEN_CLOSE ),
-        OPEN_THEN_CLOSE: (OPEN, CLOSE, CLOSE ),
+            DoorControl.OPEN_THEN_CLOSE: (DoorControl.OPEN, DoorControl.CLOSE, DoorControl.CLOSE ),
-        CLOSE_THEN_OPEN: (CLOSE, OPEN, OPEN  )
+            DoorControl.CLOSE_THEN_OPEN: (DoorControl.CLOSE, DoorControl.OPEN, DoorControl.OPEN  )
         }
 
-    if start_time == None:
+        if self.start_time is None:
-        start_time = datetime.datetime.now()
+            self.start_time = now
-        serial_port.write(target_state_cmd[actions[action][0]])
+            self._send_door_cmd(target_state_cmd[actions[self.action][0]])
 
         # Target state reached
-    if state == actions[action][0]:
+        if self.state == actions[self.action][0]:
             # Select next action, reset start time and repetitions
-        action = actions[action][1]
+            self.action = actions[self.action][1]
-        start_time = datetime.datetime.now()
+            self.start_time = now
-        repeats = 0
+            self.repeats = 0
             # On idle, we're done
-        if action == IDLE:
+            if self.action == DoorControl.IDLE:
-            log("Reached target position:", state_names[state])
+                self.logger.debug(f"Reached target position: {DoorControl.state_names[self.state]}")
                 return
 
         # Execution time
-    t = (datetime.datetime.now() - start_time).total_seconds()
+        t = (now - self.start_time).total_seconds()
-    if t >= config.state_timeout:
+        if t >= self.config.state_timeout or (t >= self.config.state_timeout_speed and speed < MIN_SPEED and delta_t >= 1. / UPDATE_RATE):
             # Timeout -> switch to timeout action
-        action = actions[action][2]
+            self.state = actions[self.action][0]
-        start_time = None
+            self.action = actions[self.action][2]
-        repeats = 0
+            self.start_time = None
-        log("Timeout. Switching to", action_names[action])
+            self.repeats = 0
-    elif t >= (1 + repeats) * config.repeat_time:
+            self.logger.debug(f"Timeout. Switching to {DoorControl.action_names[self.action]}")
+        elif t >= (1 + self.repeats) * self.config.repeat_time:
             # Repeat every couple of seconds
-        repeats += 1
+            self.repeats += 1
-        serial_port.write(target_state_cmd[actions[action][0]])
+            self.logger.debug(f"Repeating command: {target_state_cmd[actions[self.action][0]]}")
-        log("Repeating command:", target_state_cmd[actions[action][0]])
+            self._send_door_cmd(target_state_cmd[actions[self.action][0]])
 
-def open_door():
+    def open_door(self):
-    global action
+        self.logger.info("Opening the door")
-    global start_time
+        self.mqtt("state/target", "open", True)
-    log("Opening the door")
+        self.action = DoorControl.OPEN
-    action = OPEN
+        self.start_time = None
-    start_time = None
+        self.handle_door_state()
-    mqtt("state/target", "open", True)
-    handle_door_state()
 
-def close_door():
+    def close_door(self):
-    global action
+        self.logger.info("Closing the door")
-    global start_time
+        self.mqtt("state/target", "closed", True)
-    log("Closing the door")
+        self.action = DoorControl.CLOSE
-    action = CLOSE
+        self.start_time = None
-    start_time = None
+        self.handle_door_state()
-    mqtt("state/target", "closed", True)
-    handle_door_state()
 
-def toggle_door_state():
+    def toggle_door_state(self):
-    global state, action
+        if self.state == DoorControl.CLOSE:
-    if state == CLOSE:
+            self.open_door()
-        open_door()
         else:
-        close_door()
+            self.close_door()
-
-def handle_nfc_token(token = None):
-    global valid
 
+    def handle_nfc_token(self, token = None):
         if not token:
-        token = nfc_fifo.readline()
+            token = self.nfc_fifo.readline().strip()
+            self.logger.debug(f"Token from nfc_fifo: {token}")
             if token == "":
-            open_nfc_fifo()
+                self.logger.debug("Opening nfc_fifo")
+                self.nfc_fifo = self._open_nfc_fifo(self.config)
+                return
 
         token = token.strip()
-    if token in valid:
+        if token in self.valid_tokens:
-        data = valid[token]
+            data = self.valid_tokens[token]
 
             if data['valid_thru'] is not None:
                 # if a valid thru date has been set we check if the token is still valid
@@ -272,15 +345,15 @@ def handle_nfc_token(token = None):
                 authorized = True
 
             if authorized:
-            log(f"Valid token {token} of {data['name']}")
+                self.logger.info(f"Valid token {token} of {data['name']}")
-            toggle_door_state()
+                self.toggle_door_state()
             else:
-            log(f"Token {token} of {data['name']} expired on {data['valid_thru']}")
+                self.logger.warning(f"Token {token} of {data['name']} expired on {data['valid_thru']}")
         else:
-        log("Invalid token:", token)
+            self.logger.warning(f"Invalid token: {token}")
-        mqtt("token/last_invalid", "%s;%s" % (timestamp(), token))
+            self.mqtt("token/last_invalid", "%s;%s" % (timestamp(), token))
 
-class LineBuffer(object):
+    class LineBuffer(object):
         def __init__(self, f, handler):
             self.data = b''
             self.f = f
@@ -297,61 +370,56 @@ class LineBuffer(object):
                 self.handler(self.f, i)
             return True
 
-def handle_cmd(comm, data):
+    def handle_cmd(self, comm, data):
         cmd = data.decode('utf8').split()
         cmd, args = cmd[0], cmd[1:]
-    log("Got command:", data)
+        self.logger.debug(f"Got command: {data}")
 
         send = lambda x: comm.send(x.encode('utf8'))
 
         if cmd == 'fake':
-        log("Faking token", args[0])
+            self.logger.debug(f"Faking token {args[0]}")
             send("Handling token\n")
-        handle_nfc_token(args[0])
+            self.handle_nfc_token(args[0])
         elif cmd == 'open':
-        log("Control socket opening door")
+            self.logger.debug("Control socket opening door")
             send("Opening door")
-        open_door()
+            self.open_door()
         elif cmd == 'close':
-        log("Control socket closing door")
+            self.logger.debug("Control socket closing door")
             send("Closing door")
-        close_door()
+            self.close_door()
         elif cmd == 'rld':
-        log("Reloading tokens")
+            self.logger.debug("Reloading tokens")
             send("Reloading tokens")
-        read_valid_tokens()
+            self.valid_tokens = self._read_valid_tokens(self.config)
         elif cmd == 'stat':
             send("Door status is %s, position is %d. Current action: %s (%g seconds ago)\n" % (
-            state_names.get(state, "None"),
+                DoorControl.state_names.get(self.state, "None"),
-            state_pos,
+                self.state_pos,
-            action_names[action],
+                DoorControl.action_names[self.action],
-            (datetime.datetime.now() - start_time).total_seconds()))
+                (datetime.datetime.now() - self.start_time).total_seconds()))
 
-open_logfile()
-read_valid_tokens()
-open_nfc_fifo()
-open_serial_port()
-open_control_socket()
 
-mqttc.on_connect = lambda client, userdata, flags, rc: log("Connected to mqtt host with result %s" % (str(rc), ))
+dc = DoorControl(config)
-mqttc.connect_async(config.mqtt_host, keepalive = 60)
-mqttc.loop_start()
 
 buffers = {}
 while True:
-    readable = select.select([ nfc_fifo, control_socket ] + comm_channels, [], [], 1)[0]
+    readable = select.select([ dc.serial_port.fileno(), dc.nfc_fifo, dc.control_socket ] + dc.comm_channels, [], [], 1 / UPDATE_RATE)[0]
 
     for c in readable:
-        if c == nfc_fifo:
+        if c == dc.serial_port.fileno():
-            handle_nfc_token()
+            dc.handle_door_line()
-        elif c == control_socket:
+        elif c == dc.nfc_fifo:
-            log("Got connection")
+            dc.handle_nfc_token()
-            sock = control_socket.accept()[0]
+        elif c == dc.control_socket:
-            buffers[sock] = LineBuffer(sock, handle_cmd)
+            dc.logger.info("Got connection")
-            comm_channels += [sock]
+            sock = dc.control_socket.accept()[0]
+            buffers[sock] = dc.LineBuffer(sock, dc.handle_cmd)
+            dc.comm_channels += [sock]
         else:
             if not buffers[c].update():
-                log("Lost connection")
+                dc.logger.info("Lost connection")
                 del buffers[c]
-                comm_channels.remove(c)
+                dc.comm_channels.remove(c)
-    handle_door_state()
+    dc.handle_door_state()