shepherd-agent/shepherd/plugins/scout.py

#!/usr/bin/env python3

import shepherd.config as shconf
import shepherd.plugin

import queue
import threading
import re
import serial
import time

from enum import Enum, auto

from collections import namedtuple


class Command(Enum):
    BATV = "batv"
    BATI = "bati"
    TIME = "time"
    ALARM = "alarm"
    AUX5V = "aux5v"
    PWM1 = "pwm1"
    PWM2 = "pwm2"
    OUT1 = "out1"
    OUT2 = "out2"




class CommandRequest():
    def __init__(self, command, arglist):
        self.command = command
        self.arglist = arglist
        self.response_args = []
        # event that is triggered when a matching command response is
        # recieved and the data parsed into ScoutState. ScoutState should
        # only be then read via the lock on it.
        self.responded = threading.Event()


ScoutState = namedtuple(
    'ScoutState', ['batv',
                   'bati',
                   'out1',
                   'out2',
                   'pwm1',
                   'pwm2', ])

# Plugin to interface with the shepherd scout pcb modules.

# A seperate thread is started to handle the comms with the
# supervising microcontroller. Interface functions add a request
# to the queue, and some may wait for a state to be updated before
# returning (with a timeout).

# Command Requests (things to be added to the queue) should only be generated
# internally from this plugin - interface functions are provided to wrap
# them and optionally return some changed state
# The request object (namedTuple?) contains the request string,


class CommandHandler():
    def __init__(self, config):
        self._command_queue = queue.Queue()
        self.port = serial.Serial()
        self.current_request = None
        self.curr_request_sent_time = None
        self.request_timeout = 2

        self.config = config

        self._rx_string = ""

        self.port.baudrate = 57600
        self.port.timeout = 0
        self.port.port = self.config["serialport"]
        # pull in serial config
        # start thread
        self.thread = threading.Thread(target=self._serial_comm_thread, daemon=True)
        self.thread.start()

    def request(self, newRequest):
        self._command_queue.put(newRequest)

    def _send_request(self):
        argstr = ""
        if len(self.current_request.arglist) > 0:
            argstr = ':'+','.join(self.current_request.arglist)
        send_str = '?'+self.current_request.command.value+argstr+'\n'
        self.port.write(send_str.encode('utf-8'))
        self.curr_request_sent_time = time.time()

    def _process_bytes(self, new_bytes):
        self._rx_string = self._rx_string + new_bytes.decode('utf-8')
        #print(self._rx_string)
        if ('!' in self._rx_string) or ('?' in self._rx_string):
            match = re.search(r"([!\?])(.+?)[\r\n]", self._rx_string)
            while match is not None:
                self._parse_command(match[2])
                # only keep part of string after our match and look for another
                # command
                self._rx_string = self._rx_string[match.end():]
                match = re.search(r"[!\?].+?\n", self._rx_string)
        else:
            # No command start characters anywhere in the string, so ditch it
            self._rx_string = ""

    def _parse_command(self, command_str):
        command_name, _, command_args = command_str.partition(':')
        if self.current_request is not None:
            if command_name == self.current_request.command.value:
                self.current_request.response_args = command_args.split(',')
                self.current_request.responded.set()
                self.current_request = None

    def _handle_serial_port(self):
        while True:
            if self.port.in_waiting > 0:
                self._process_bytes(self.port.read(self.port.in_waiting))
            elif self.current_request is None:
                try:
                    self.current_request = self._command_queue.get(
                        block=True, timeout=0.01)
                    self._send_request()
                except queue.Empty:
                    pass
            else:
                if (time.time()-self.curr_request_sent_time) > self.request_timeout:
                    #Timeout the request
                    self.current_request = None
                else:
                    pass
                    #Ideally have a short spin loop delay here, but could cause serial buffer to back up
                    #pyserial buffer is apparently 1024 or 4096 bytes, and at 57600 baud 10ms delay would only
                    # be 72 bytes.
                    time.sleep(0.01)

    def _serial_comm_thread(self):
        while True:
            # Producers-consumer pattern
            # If there's a CommandRequest to on the queue, pop it and set as current
            # Send the command, and wait for a matching response
            # timeout the response wait if necessary
            # Parse any response and update the state
            # notify the queue responded event

            # Actual wait is on either the non-empty queue or a serial character to parse
            # Serial comms is not synchronous, so need to be available to recieve characters
            # at any point
            try:
                self.port.open()
                self._handle_serial_port()
            except serial.SerialException:
                time.sleep(1)

                # If there's bytes, read them and deal with them. If there's
                # a serialexception, try to reopen the port
                # If no bytes,


class ScoutPlugin(shepherd.plugin.Plugin):
    @staticmethod
    def define_config(confdef):
        confdef.add_def('boardver', shconf.StringDef())
        confdef.add_def('serialport', shconf.StringDef())

    def __init__(self, pluginInterface, config):
        super().__init__(pluginInterface, config)
        self.config = config
        self.interface = pluginInterface
        self.plugins = pluginInterface.other_plugins
        self.hooks = pluginInterface.hooks

        self.cmd_handler = CommandHandler(config)

        self.interface.register_function(self.get_batv)
        self.interface.register_function(self.get_bati)
        self.interface.register_function(self.get_time)
        self.interface.register_function(self.set_alarm)
        self.interface.register_function(self.set_aux5v)
        self.interface.register_function(self.set_pwm1)
        self.interface.register_function(self.set_pwm2)
        self.interface.register_function(self.set_out1)
        self.interface.register_function(self.set_out2)

        self.interface.register_function(self.test)

    def get_batv(self):
        req = CommandRequest(Command.BATV, [])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def get_bati(self):
        req = CommandRequest(Command.BATI, [])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def set_aux5v(self, enabled):
        req = CommandRequest(Command.AUX5V, [str(enabled).lower()])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def set_pwm1(self, enabled, pulse_length):
        req = CommandRequest(Command.PWM1, [str(enabled).lower(), str(pulse_length)])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def set_pwm2(self, enabled, pulse_length):
        req = CommandRequest(Command.PWM2, [str(enabled).lower(), str(pulse_length)])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def set_out1(self, enabled):
        req = CommandRequest(Command.OUT1, [str(enabled).lower()])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def set_out2(self, enabled):
        req = CommandRequest(Command.OUT2, [str(enabled).lower()])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def get_time(self):
        req = CommandRequest(Command.TIME, [])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def set_alarm(self, unix_time):
        req = CommandRequest(Command.ALARM, [unix_time])
        self.cmd_handler.request(req)
        if req.responded.wait(2):
            return req.response_args[0]
        return None

    def test(self):
        print("Testing companion board...")
        print(F"Current RTC time is {self.get_time()}")
        print(F"Current BatV is {self.get_batv()}")
        print(F"Current BatI is {self.get_bati()}")
        print("Turning on Out1 for 1 second")
        self.set_out1(True)
        time.sleep(1)
        self.set_out1(False)
        print("Turning on Out2 for 1 second")
        self.set_out2(True)
        time.sleep(1)
        self.set_out2(False)

        print("Enabling auxilliary 5V")
        self.set_aux5v(True)

        print("Sweeping PWM1 from 1000us to 2000us")
        self.set_pwm1(True, 1000)
        time.sleep(1)
        self.set_pwm1(True, 2000)
        time.sleep(1)
        self.set_pwm1(False, 1000)

        print("Sweeping PWM2 from 1000us to 2000us")
        self.set_pwm2(True, 1000)
        time.sleep(1)
        self.set_pwm2(True, 2000)
        time.sleep(1)
        self.set_pwm2(False, 1000)
        self.set_aux5v(False)
        print("Test finished")

        return None