23 changed files with 931 additions and 236 deletions
@ -1,178 +0,0 @@ |
|||||
import atexit |
|
||||
import logging |
|
||||
import os |
|
||||
import time |
|
||||
from dataclasses import dataclass |
|
||||
from enum import Enum |
|
||||
from multiprocessing import Process |
|
||||
from multiprocessing import Queue |
|
||||
from struct import pack |
|
||||
from struct import unpack |
|
||||
from typing import Optional |
|
||||
|
|
||||
from flask import Response |
|
||||
from flask import blueprints |
|
||||
from flask import request |
|
||||
from flask_api import status |
|
||||
from serial import Serial |
|
||||
|
|
||||
from .container import get_container |
|
||||
from .container import initialize_container |
|
||||
|
|
||||
bp = blueprints.Blueprint("command", __name__) |
|
||||
|
|
||||
_process: Optional[Process] = None |
|
||||
_queue: Queue = Queue() |
|
||||
_logger = logging.getLogger(__name__) |
|
||||
|
|
||||
|
|
||||
@bp.route("/command", methods=["POST", "GET"]) |
|
||||
def command(): |
|
||||
logger = logging.getLogger("test") |
|
||||
command_id = request.args.get("command-id") |
|
||||
|
|
||||
if _queue is not None and command_id is not None: |
|
||||
logger.info(f"put in queue: {command_id}") |
|
||||
_queue.put(command_id) |
|
||||
|
|
||||
return Response(status=status.HTTP_200_OK) |
|
||||
|
|
||||
|
|
||||
def _end_running_process(): |
|
||||
if _process is not None: |
|
||||
_process.kill() |
|
||||
|
|
||||
|
|
||||
def worker_process( |
|
||||
queue: Queue, |
|
||||
config, |
|
||||
# container: Container |
|
||||
): |
|
||||
logging.basicConfig( |
|
||||
level=logging.DEBUG, |
|
||||
format="[%(asctime)s] [%(name)-20s] [%(levelname)-8s] --- %(message)s", |
|
||||
) |
|
||||
logger = logging.getLogger("worker_process") |
|
||||
logger.setLevel(logging.INFO) |
|
||||
|
|
||||
initialize_container(config) |
|
||||
container = get_container() |
|
||||
|
|
||||
counter = 1 |
|
||||
with container.serial() as serial: |
|
||||
while True: |
|
||||
logger.debug(f"Ping {counter} process_id={os.getpid()}") |
|
||||
counter += 1 |
|
||||
time.sleep(0.01) |
|
||||
receive_and_log( |
|
||||
serial=serial, |
|
||||
header_size=container.config.header_size(), |
|
||||
) |
|
||||
|
|
||||
if queue is not None: |
|
||||
while not queue.empty(): |
|
||||
command_id = queue.get() |
|
||||
payload = "test value".encode() |
|
||||
length = len(payload) |
|
||||
|
|
||||
data = pack( |
|
||||
">BHB" + "B" * length + "B", |
|
||||
int(command_id), |
|
||||
length, |
|
||||
0, |
|
||||
*list(payload), |
|
||||
0xFF, |
|
||||
) |
|
||||
|
|
||||
serial.write(data) |
|
||||
|
|
||||
|
|
||||
class CommandId(Enum): |
|
||||
command_none = 0 |
|
||||
command_log = 0x1 |
|
||||
|
|
||||
|
|
||||
@dataclass |
|
||||
class LogCommand: |
|
||||
"""Command ID: command_log""" |
|
||||
|
|
||||
level: int |
|
||||
message: str |
|
||||
|
|
||||
HEADER_SIZE = 1 # log level |
|
||||
|
|
||||
def __init__( |
|
||||
self, |
|
||||
data: bytes, |
|
||||
) -> None: |
|
||||
self._logger = logging.getLogger(self.__class__.__name__) |
|
||||
self.received_logger = logging.getLogger("stm32wb55") |
|
||||
self.received_logger.setLevel(logging.DEBUG) |
|
||||
|
|
||||
self._logger.setLevel(logging.INFO) |
|
||||
|
|
||||
level = int(data[0]) |
|
||||
self._logger.debug(f"level: {level}") |
|
||||
|
|
||||
message = data[self.HEADER_SIZE :] |
|
||||
self._logger.debug("Message: " + str(message)) |
|
||||
|
|
||||
self.level = level |
|
||||
self.message = message.decode() |
|
||||
|
|
||||
def execute(self): |
|
||||
self.received_logger.log(level=self.level, msg=self.message) |
|
||||
|
|
||||
|
|
||||
def receive_and_log( |
|
||||
serial: Serial, |
|
||||
header_size: int, |
|
||||
): |
|
||||
logger = logging.getLogger("receive_and_log") |
|
||||
logger.setLevel(logging.INFO) |
|
||||
|
|
||||
bytes_read = serial.read(serial.in_waiting) |
|
||||
|
|
||||
while bytes_read: |
|
||||
|
|
||||
logger.debug(f"bytes: {bytes_read.hex()}") |
|
||||
command_id_int, data_length, _ = unpack(">BHB", bytes_read[:header_size]) |
|
||||
payload = bytes(bytes_read[header_size : header_size + data_length]) |
|
||||
stop_byte = bytes_read[header_size + data_length] |
|
||||
|
|
||||
try: |
|
||||
command_id = CommandId(command_id_int) |
|
||||
except ValueError: |
|
||||
logger.error( |
|
||||
f"invalid command {command_id_int} with payload {str(payload)}", |
|
||||
) |
|
||||
bytes_read = bytes_read[header_size + data_length + 1 :] |
|
||||
continue |
|
||||
|
|
||||
if command_id == CommandId.command_log: |
|
||||
command = LogCommand( |
|
||||
data=payload, |
|
||||
) |
|
||||
command.execute() |
|
||||
else: |
|
||||
return |
|
||||
|
|
||||
logger.debug(f"stop_byte: {stop_byte}") |
|
||||
assert stop_byte == 0xFF |
|
||||
|
|
||||
bytes_read = bytes_read[header_size + data_length + 1 :] |
|
||||
|
|
||||
|
|
||||
def start_backgroup_process(): |
|
||||
_logger.warning("start_backgroup_process called") |
|
||||
global _process |
|
||||
|
|
||||
_process = Process( |
|
||||
target=worker_process, |
|
||||
args=( |
|
||||
_queue, |
|
||||
get_container().config(), |
|
||||
), |
|
||||
) |
|
||||
_process.start() |
|
||||
atexit.register(_end_running_process) |
|
||||
@ -0,0 +1,31 @@ |
|||||
|
from flask import Response |
||||
|
from flask import blueprints |
||||
|
from flask import request |
||||
|
from flask_api import status |
||||
|
|
||||
|
from .command_execution import enqueue_command |
||||
|
from .commands import CommandId |
||||
|
from .commands import get_command_id_from_name |
||||
|
from .commands import get_request_class |
||||
|
|
||||
|
bp = blueprints.Blueprint("command", __name__) |
||||
|
|
||||
|
|
||||
|
@bp.route("/command", methods=["POST", "GET"]) |
||||
|
def command(): |
||||
|
arguments = dict(request.args) |
||||
|
|
||||
|
cmd = arguments.pop("cmd") |
||||
|
try: |
||||
|
command_id = CommandId(int(cmd)) |
||||
|
except ValueError: |
||||
|
command_id = get_command_id_from_name(cmd) |
||||
|
|
||||
|
try: |
||||
|
command = get_request_class(command_id=command_id)(**arguments) |
||||
|
except Exception: |
||||
|
return Response(status=status.HTTP_400_BAD_REQUEST) |
||||
|
|
||||
|
enqueue_command(command) |
||||
|
|
||||
|
return Response(status=status.HTTP_200_OK) |
||||
@ -0,0 +1,243 @@ |
|||||
|
import atexit |
||||
|
import logging |
||||
|
import time |
||||
|
from enum import Enum |
||||
|
from multiprocessing import Process |
||||
|
from multiprocessing import Queue |
||||
|
from struct import unpack |
||||
|
from typing import List |
||||
|
from typing import Optional |
||||
|
from typing import Sequence |
||||
|
from typing import Tuple |
||||
|
|
||||
|
from serial import Serial |
||||
|
|
||||
|
from . import commands |
||||
|
from .commands import Command |
||||
|
from .commands import CommandId |
||||
|
from .commands import Request |
||||
|
from .commands import Response |
||||
|
from .container import get_container |
||||
|
from .container import initialize_container |
||||
|
|
||||
|
_logger = logging.getLogger(__file__) |
||||
|
|
||||
|
_command_queue: Queue = Queue() |
||||
|
|
||||
|
|
||||
|
class State(Enum): |
||||
|
heart_beat = 0x0 |
||||
|
executing_command = 0x1 |
||||
|
executing_command_waiting_for_response = 0x2 |
||||
|
receiving_command = 0x10 |
||||
|
|
||||
|
|
||||
|
def worker_process( |
||||
|
queue: Queue, |
||||
|
config, |
||||
|
): |
||||
|
logging.basicConfig( |
||||
|
level=logging.DEBUG, |
||||
|
format="[%(asctime)s] [%(name)-20s] [%(levelname)-8s] --- %(message)s", |
||||
|
) |
||||
|
logger = logging.getLogger("Command Loop") |
||||
|
logger.setLevel(logging.INFO) |
||||
|
|
||||
|
initialize_container(config) |
||||
|
container = get_container() |
||||
|
heartbeat_interval = container.config.heartbeat_interval() |
||||
|
serial_reconnection_wait_timeout = ( |
||||
|
container.config.serial_reconnection_wait_timeout() |
||||
|
) |
||||
|
connected = False |
||||
|
|
||||
|
logger.info("entering command loop...") |
||||
|
while True: |
||||
|
try: |
||||
|
with container.serial() as serial: |
||||
|
logger.info("connected with serial device") |
||||
|
connected = True |
||||
|
enter_fsm( |
||||
|
serial=serial, |
||||
|
command_queue=queue, |
||||
|
heartbeat_interval=heartbeat_interval, |
||||
|
) |
||||
|
except OSError: |
||||
|
if connected: |
||||
|
logger.warning("connection to serial device lost") |
||||
|
connected = False |
||||
|
time.sleep(serial_reconnection_wait_timeout) |
||||
|
logger.warning("reconnecting...") |
||||
|
|
||||
|
|
||||
|
def enter_fsm( |
||||
|
serial: Serial, |
||||
|
command_queue: Queue, |
||||
|
heartbeat_interval: float, |
||||
|
): |
||||
|
logger = logging.getLogger("FSM") |
||||
|
|
||||
|
state = State.executing_command |
||||
|
current_command: Optional[Command] = None |
||||
|
responses_received: List[Response] = list() |
||||
|
time_at_beginning_waiting_for_response: float = 0.0 |
||||
|
last_heart_beat_time: float = 0.0 |
||||
|
|
||||
|
while True: |
||||
|
if state == State.heart_beat: |
||||
|
if time.time() - heartbeat_interval > last_heart_beat_time: |
||||
|
command_queue.put(commands.HeartbeatRequest()) |
||||
|
last_heart_beat_time = time.time() |
||||
|
|
||||
|
state = State.executing_command |
||||
|
continue |
||||
|
|
||||
|
elif state == State.executing_command: |
||||
|
current_command = dequeue_command(queue=command_queue) |
||||
|
if current_command is None: |
||||
|
state = State.receiving_command |
||||
|
continue |
||||
|
|
||||
|
current_command.execute(serial=serial) |
||||
|
|
||||
|
if isinstance(current_command, Request): |
||||
|
time_at_beginning_waiting_for_response = time.time() |
||||
|
state = State.executing_command_waiting_for_response |
||||
|
continue |
||||
|
|
||||
|
elif state == State.executing_command_waiting_for_response: |
||||
|
if not isinstance(current_command, Request): |
||||
|
raise RuntimeError( |
||||
|
"entered state 'executing_command_waiting_for_response' but " |
||||
|
"current command does not expect a response.", |
||||
|
) |
||||
|
else: |
||||
|
request: Request = current_command |
||||
|
commands_, responses = receive(serial=serial) |
||||
|
responses_received.extend(responses) |
||||
|
for command in commands_: |
||||
|
command_queue.put(command) |
||||
|
|
||||
|
while responses_received: |
||||
|
received_response: Response = responses_received.pop(0) |
||||
|
if request.response_identifier == received_response.identifier: |
||||
|
request.process_response( |
||||
|
response=received_response, |
||||
|
) |
||||
|
state = State.executing_command |
||||
|
break |
||||
|
else: |
||||
|
logger.warning( |
||||
|
f"received response with ID {received_response.identifier} " |
||||
|
"but expected response with ID " |
||||
|
f"{request.response_identifier}", |
||||
|
) |
||||
|
else: |
||||
|
if ( |
||||
|
time.time() - request.timeout |
||||
|
> time_at_beginning_waiting_for_response |
||||
|
): |
||||
|
logger.error( |
||||
|
"Timeout while waiting for response with ID " |
||||
|
f"{request.response_identifier}", |
||||
|
) |
||||
|
current_command = None |
||||
|
state = State.executing_command |
||||
|
continue |
||||
|
|
||||
|
elif state == State.receiving_command: |
||||
|
commands_, responses = receive(serial=serial) |
||||
|
responses_received.extend(responses) |
||||
|
for command in commands_: |
||||
|
command_queue.put(command) |
||||
|
|
||||
|
state = State.heart_beat |
||||
|
continue |
||||
|
|
||||
|
else: |
||||
|
raise RuntimeError(f"Invalid state: {state}") |
||||
|
|
||||
|
|
||||
|
def dequeue_command( |
||||
|
queue: Queue, |
||||
|
) -> Optional[Command]: |
||||
|
while not queue.empty(): |
||||
|
return queue.get() |
||||
|
|
||||
|
return None |
||||
|
|
||||
|
|
||||
|
def enqueue_command(command: Command): |
||||
|
_command_queue.put(command) |
||||
|
|
||||
|
|
||||
|
def receive( |
||||
|
serial: Serial, |
||||
|
) -> Tuple[Sequence[Command], Sequence[Response]]: |
||||
|
logger = logging.getLogger("receive_and_log") |
||||
|
logger.setLevel(logging.INFO) |
||||
|
commands_received: List[Command] = list() |
||||
|
responses_received: List[Response] = list() |
||||
|
header_size = 4 |
||||
|
|
||||
|
bytes_read = serial.read(serial.in_waiting) |
||||
|
|
||||
|
while bytes_read: |
||||
|
|
||||
|
logger.debug(f"bytes: {bytes_read.hex()}") |
||||
|
command_id_int, data_length, _ = unpack(">BHB", bytes_read[:header_size]) |
||||
|
payload = bytes(bytes_read[header_size : header_size + data_length]) |
||||
|
stop_byte = bytes_read[header_size + data_length] |
||||
|
|
||||
|
try: |
||||
|
command_id = CommandId(command_id_int) |
||||
|
except ValueError: |
||||
|
logger.error( |
||||
|
f"invalid command {command_id_int} with payload {str(payload)}", |
||||
|
) |
||||
|
bytes_read = bytes_read[header_size + data_length + 1 :] |
||||
|
continue |
||||
|
|
||||
|
logger.debug(f"stop_byte: {stop_byte}") |
||||
|
if stop_byte != 0xFF: |
||||
|
logger.error("Invalid stop byte") |
||||
|
bytes_read = None |
||||
|
else: |
||||
|
bytes_read = bytes_read[header_size + data_length + 1 :] |
||||
|
|
||||
|
if command_id == CommandId.command_log: |
||||
|
command = commands.LogCommand( |
||||
|
data=payload, |
||||
|
) |
||||
|
commands_received.append(command) |
||||
|
elif command_id == CommandId.command_heartbeat_response: |
||||
|
responses_received.append(commands.HeartbeatResponse(payload)) |
||||
|
elif command_id == CommandId.command_led_response: |
||||
|
responses_received.append(commands.LEDResponse(payload)) |
||||
|
else: |
||||
|
raise RuntimeError |
||||
|
|
||||
|
return commands_received, responses_received |
||||
|
|
||||
|
|
||||
|
_process: Optional[Process] = None |
||||
|
|
||||
|
|
||||
|
def _end_running_process(): |
||||
|
if _process is not None: |
||||
|
_process.kill() |
||||
|
|
||||
|
|
||||
|
def start_backgroup_process(): |
||||
|
_logger.warning("start_backgroup_process called") |
||||
|
global _process |
||||
|
|
||||
|
_process = Process( |
||||
|
target=worker_process, |
||||
|
args=( |
||||
|
_command_queue, |
||||
|
get_container().config(), |
||||
|
), |
||||
|
) |
||||
|
_process.start() |
||||
|
atexit.register(_end_running_process) |
||||
@ -0,0 +1,273 @@ |
|||||
|
import abc |
||||
|
import logging |
||||
|
from dataclasses import dataclass |
||||
|
from enum import Enum |
||||
|
from random import randint |
||||
|
from struct import pack |
||||
|
from struct import unpack |
||||
|
from typing import Union |
||||
|
|
||||
|
from serial import Serial |
||||
|
|
||||
|
|
||||
|
class CommandId(Enum): |
||||
|
command_none = 0 |
||||
|
command_log = 0x1 |
||||
|
|
||||
|
# even numbers are requests with optional response with ID = request ID + 1 |
||||
|
|
||||
|
command_heartbeat_request = 0x2 |
||||
|
command_heartbeat_response = 0x3 |
||||
|
|
||||
|
command_led_request = 0x4 |
||||
|
command_led_response = 0x5 |
||||
|
|
||||
|
|
||||
|
def get_command_id_from_name(name: str) -> CommandId: |
||||
|
return { |
||||
|
"log": CommandId.command_log, |
||||
|
"led": CommandId.command_led_request, |
||||
|
}[name] |
||||
|
|
||||
|
|
||||
|
def get_request_class( |
||||
|
command_id: CommandId, |
||||
|
): |
||||
|
return { |
||||
|
CommandId.command_log: LogCommand, |
||||
|
CommandId.command_heartbeat_request: HeartbeatRequest, |
||||
|
CommandId.command_led_request: LEDRequest, |
||||
|
}[command_id] |
||||
|
|
||||
|
|
||||
|
def get_response_class( |
||||
|
command_id: CommandId, |
||||
|
): |
||||
|
return { |
||||
|
CommandId.command_heartbeat_response: HeartbeatResponse, |
||||
|
CommandId.command_led_response: LEDResponse, |
||||
|
}[command_id] |
||||
|
|
||||
|
|
||||
|
class Response(abc.ABC): |
||||
|
|
||||
|
identifier: int |
||||
|
|
||||
|
def __init__( |
||||
|
self, |
||||
|
data: bytes, |
||||
|
) -> None: |
||||
|
self._logger = logging.getLogger(self.__class__.__name__) |
||||
|
|
||||
|
self.identifier = unpack(">H", data[:2])[0] |
||||
|
self.unpack_payload(data[2:]) |
||||
|
|
||||
|
@abc.abstractmethod |
||||
|
def unpack_payload( |
||||
|
self, |
||||
|
data: bytes, |
||||
|
): |
||||
|
pass |
||||
|
|
||||
|
|
||||
|
class Command(abc.ABC): |
||||
|
def __init__(self) -> None: |
||||
|
self._logger = logging.getLogger(self.__class__.__name__) |
||||
|
|
||||
|
@property |
||||
|
@abc.abstractmethod |
||||
|
def identifier(self) -> CommandId: |
||||
|
pass |
||||
|
|
||||
|
@abc.abstractmethod |
||||
|
def execute( |
||||
|
self, |
||||
|
serial: Serial, |
||||
|
): |
||||
|
pass |
||||
|
|
||||
|
def send_command(self, payload: bytes, serial: Serial): |
||||
|
length = len(payload) |
||||
|
data = pack( |
||||
|
">BHB" + "B" * length + "B", |
||||
|
int(self.identifier.value), |
||||
|
length, |
||||
|
0, |
||||
|
*list(payload), |
||||
|
0xFF, |
||||
|
) |
||||
|
|
||||
|
serial.write(data) |
||||
|
|
||||
|
|
||||
|
class Request(Command): |
||||
|
def __init__(self) -> None: |
||||
|
super().__init__() |
||||
|
|
||||
|
self.response_identifier = randint(0, pow(2, 16) - 1) |
||||
|
|
||||
|
@property |
||||
|
@abc.abstractmethod |
||||
|
def timeout(self) -> float: |
||||
|
pass |
||||
|
|
||||
|
@abc.abstractmethod |
||||
|
def process_response( |
||||
|
self, |
||||
|
response: Response, |
||||
|
): |
||||
|
pass |
||||
|
|
||||
|
def send_command(self, payload: bytes, serial: Serial): |
||||
|
response_identifier_header = pack(">H", self.response_identifier) |
||||
|
|
||||
|
super().send_command( |
||||
|
payload=response_identifier_header + payload, |
||||
|
serial=serial, |
||||
|
) |
||||
|
|
||||
|
|
||||
|
@dataclass |
||||
|
class LogCommand(Command): |
||||
|
"""Command ID: command_log""" |
||||
|
|
||||
|
level: int |
||||
|
message: str |
||||
|
|
||||
|
HEADER_SIZE = 1 # log level |
||||
|
|
||||
|
def __init__( |
||||
|
self, |
||||
|
data: bytes, |
||||
|
) -> None: |
||||
|
super().__init__() |
||||
|
self.received_logger = logging.getLogger("stm32wb55") |
||||
|
self.received_logger.setLevel(logging.DEBUG) |
||||
|
|
||||
|
self._logger.setLevel(logging.INFO) |
||||
|
|
||||
|
level = int(data[0]) |
||||
|
self._logger.debug(f"level: {level}") |
||||
|
|
||||
|
message = data[self.HEADER_SIZE :] |
||||
|
self._logger.debug("Message: " + str(message)) |
||||
|
|
||||
|
self.level = level |
||||
|
self.message = message.decode() |
||||
|
|
||||
|
@property |
||||
|
def identifier(self) -> CommandId: |
||||
|
return CommandId.command_log |
||||
|
|
||||
|
def execute( |
||||
|
self, |
||||
|
serial: Serial, |
||||
|
): |
||||
|
self.received_logger.log(level=self.level, msg=self.message) |
||||
|
|
||||
|
|
||||
|
class HeartbeatResponse(Response): |
||||
|
def unpack_payload( |
||||
|
self, |
||||
|
data: bytes, |
||||
|
): |
||||
|
pass |
||||
|
|
||||
|
|
||||
|
class HeartbeatRequest(Request): |
||||
|
@property |
||||
|
def identifier(self) -> CommandId: |
||||
|
return CommandId.command_heartbeat_request |
||||
|
|
||||
|
@property |
||||
|
def timeout(self) -> float: |
||||
|
return 0.1 |
||||
|
|
||||
|
def process_response(self, response: Response): |
||||
|
if not isinstance(response, HeartbeatResponse): |
||||
|
raise TypeError(f"{response} is not a {HeartbeatResponse}") |
||||
|
|
||||
|
def execute(self, serial: Serial): |
||||
|
self.send_command( |
||||
|
payload=bytes(), |
||||
|
serial=serial, |
||||
|
) |
||||
|
|
||||
|
|
||||
|
class LEDResponse(Response): |
||||
|
was_successful = True |
||||
|
|
||||
|
# def __init__(self) -> None: |
||||
|
# super().__init__() |
||||
|
|
||||
|
def unpack_payload( |
||||
|
self, |
||||
|
data: bytes, |
||||
|
): |
||||
|
self.was_successful = bool(data[0]) |
||||
|
if self.was_successful: |
||||
|
self._logger.debug("LED command was successful") |
||||
|
else: |
||||
|
self._logger.debug("LED command was not successful") |
||||
|
|
||||
|
|
||||
|
class LEDRequest(Request): |
||||
|
def __init__( |
||||
|
self, |
||||
|
id: Union[int, str], |
||||
|
command: Union[int, str], |
||||
|
) -> None: |
||||
|
""" |
||||
|
led_id |
||||
|
------- |
||||
|
0: green |
||||
|
1: red |
||||
|
2: blue |
||||
|
|
||||
|
led_command |
||||
|
-------- |
||||
|
0: off |
||||
|
1: on |
||||
|
2: toggle |
||||
|
""" |
||||
|
super().__init__() |
||||
|
try: |
||||
|
self.led_id = int(id) |
||||
|
except ValueError: |
||||
|
self.led_id = { |
||||
|
"green": 0, |
||||
|
"red": 1, |
||||
|
"blue": 2, |
||||
|
}[str(id)] |
||||
|
|
||||
|
try: |
||||
|
self.led_command = int(command) |
||||
|
except ValueError: |
||||
|
self.led_command = { |
||||
|
"off": 0, |
||||
|
"on": 1, |
||||
|
"toggle": 2, |
||||
|
}[str(command)] |
||||
|
|
||||
|
@property |
||||
|
def identifier(self) -> CommandId: |
||||
|
return CommandId.command_led_request |
||||
|
|
||||
|
@property |
||||
|
def timeout(self) -> float: |
||||
|
return 0.1 |
||||
|
|
||||
|
def process_response(self, response: Response): |
||||
|
if not isinstance(response, LEDResponse): |
||||
|
raise TypeError(f"{response} is not a {LEDResponse}") |
||||
|
|
||||
|
def execute(self, serial: Serial): |
||||
|
payload = pack( |
||||
|
">BB", |
||||
|
self.led_id, |
||||
|
self.led_command, |
||||
|
) |
||||
|
self.send_command( |
||||
|
payload=payload, |
||||
|
serial=serial, |
||||
|
) |
||||
@ -0,0 +1,61 @@ |
|||||
|
/*
|
||||
|
* LedCommand.cpp |
||||
|
* |
||||
|
* Created on: Jul 16, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#include "LedCommand.hpp" |
||||
|
#include "commands.h" |
||||
|
#include "stm32wbxx_hal.h" |
||||
|
#include "main.h" |
||||
|
|
||||
|
LedResponse::LedResponse(uint16_t response_identifier, bool was_successful) |
||||
|
: Response(response_identifier) { |
||||
|
(this->payload_ptr + this->get_payload_size())[0] = (uint8_t)was_successful; |
||||
|
this->add_to_payload_size(1); |
||||
|
} |
||||
|
|
||||
|
LedRequest::LedRequest(uint8_t * payload_ptr, uint16_t size) : Request(payload_ptr, size) { |
||||
|
uint16_t expected_length = this->buffer_offset + 2; |
||||
|
if (expected_length != size) { |
||||
|
//log_error("LedCommand: received request with length %d, expected length %d", 2, size, expected_length);
|
||||
|
this->has_error = true; |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
uint8_t * data_ptr = payload_ptr + this->buffer_offset; |
||||
|
this->led_id = static_cast<LedID>(data_ptr[0]); |
||||
|
this->led_command = static_cast<LedCommand>(data_ptr[1]); |
||||
|
} |
||||
|
|
||||
|
LedResponse * LedRequest::execute_request(uint16_t response_identifier) { |
||||
|
bool was_successful = true; |
||||
|
int led_pin_mapping[3] = {LED_GREEN_Pin, LED_RED_Pin, LED_BLUE_Pin}; |
||||
|
GPIO_TypeDef* led_prio_port_mapping[3] = {LED_GREEN_GPIO_Port, LED_RED_GPIO_Port, LED_BLUE_GPIO_Port}; |
||||
|
|
||||
|
if (led_id >= led_id_max) { |
||||
|
//log_error("LedCommand: invalid LED ID %d", 1, led_id);
|
||||
|
was_successful = false; |
||||
|
} else { |
||||
|
switch (this->led_command) |
||||
|
{ |
||||
|
case led_off: |
||||
|
HAL_GPIO_WritePin(led_prio_port_mapping[led_id], led_pin_mapping[led_id], GPIO_PIN_RESET); |
||||
|
break; |
||||
|
case led_on: |
||||
|
HAL_GPIO_WritePin(led_prio_port_mapping[led_id], led_pin_mapping[led_id], GPIO_PIN_SET); |
||||
|
break; |
||||
|
case led_toggle: |
||||
|
HAL_GPIO_TogglePin(led_prio_port_mapping[led_id], led_pin_mapping[led_id]); |
||||
|
break; |
||||
|
default: |
||||
|
//log_error("LedCommand: invalid LED command %d", 1, this->led_id);
|
||||
|
was_successful = false; |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
return new LedResponse(response_identifier, was_successful); |
||||
|
} |
||||
|
|
||||
@ -0,0 +1,74 @@ |
|||||
|
/*
|
||||
|
* LedCommand.hpp |
||||
|
* |
||||
|
* Created on: Jul 16, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#ifndef LEDCOMMAND_HPP_ |
||||
|
#define LEDCOMMAND_HPP_ |
||||
|
|
||||
|
#include "Request.hpp" |
||||
|
#include "Response.hpp" |
||||
|
|
||||
|
class LedResponse : public Response { |
||||
|
public: |
||||
|
LedResponse(uint16_t response_identifier, bool was_successful); |
||||
|
|
||||
|
CommandId get_command_id() override { return COMMAND_LED_RESPONSE; } |
||||
|
|
||||
|
private: |
||||
|
bool was_successful = false; |
||||
|
}; |
||||
|
|
||||
|
class LedRequest : public Request { |
||||
|
public: |
||||
|
/**
|
||||
|
* @brief Construct a new Led Request object |
||||
|
* |
||||
|
* @param payload_ptr |
||||
|
* HEADER | led_id | led_command | |
||||
|
* | 1 byte | 1 byte | |
||||
|
* |
||||
|
* led_id |
||||
|
* ------- |
||||
|
* 0: green |
||||
|
* 1: red |
||||
|
* 2: blue |
||||
|
* |
||||
|
* led_command |
||||
|
* ----------- |
||||
|
* 0: off |
||||
|
* 1: on |
||||
|
* 2: toggle |
||||
|
* |
||||
|
* @param size |
||||
|
*/ |
||||
|
LedRequest(uint8_t * payload_ptr, uint16_t size); |
||||
|
|
||||
|
enum LedID { |
||||
|
greed_led, |
||||
|
greed_red, |
||||
|
greed_blue, |
||||
|
led_id_max, |
||||
|
}; |
||||
|
|
||||
|
enum LedCommand { |
||||
|
led_off, |
||||
|
led_on, |
||||
|
led_toggle, |
||||
|
led_command_max, |
||||
|
}; |
||||
|
|
||||
|
LedResponse * execute_request(uint16_t response_identifier); |
||||
|
|
||||
|
CommandId get_command_id() override { return COMMAND_LED_REQUEST; } |
||||
|
|
||||
|
private: |
||||
|
bool has_error = false; |
||||
|
LedID led_id = greed_led; |
||||
|
LedCommand led_command = led_off; |
||||
|
|
||||
|
}; |
||||
|
|
||||
|
#endif /* LEDCOMMAND_HPP_ */ |
||||
@ -0,0 +1,34 @@ |
|||||
|
/*
|
||||
|
* Notification.cpp |
||||
|
* |
||||
|
* Created on: Jul 14, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#include "usbd_cdc_if.h" |
||||
|
#include "Notification.hpp" |
||||
|
|
||||
|
Notification::Notification() { |
||||
|
this->payload_ptr = this->data +4; |
||||
|
} |
||||
|
|
||||
|
bool Notification::execute() { |
||||
|
uint16_t size = NOTIFICATION_COMMAND_TOTAL_OVERHEAD + this->payload_size; |
||||
|
|
||||
|
this->data[0] = (uint8_t)this->get_command_id(); |
||||
|
this->data[1] = (this->payload_size & 0xFF00) >> 8; |
||||
|
this->data[2] = this->payload_size & 0x00FF; |
||||
|
this->data[3] = 0x00; // reserved
|
||||
|
this->data[size-1] = 0xff; // set stop byte
|
||||
|
|
||||
|
uint8_t result = CDC_Transmit_FS(this->data, size); |
||||
|
return result == USBD_OK || result == USBD_BUSY; |
||||
|
} |
||||
|
|
||||
|
uint16_t Notification::get_payload_size() { |
||||
|
return this->payload_size; |
||||
|
} |
||||
|
|
||||
|
void Notification::add_to_payload_size(uint16_t size) { |
||||
|
this->payload_size += size; |
||||
|
} |
||||
@ -0,0 +1,38 @@ |
|||||
|
/*
|
||||
|
* Notification.hpp |
||||
|
* |
||||
|
* Created on: Jul 14, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#ifndef NOTIFICATION_HPP_ |
||||
|
#define NOTIFICATION_HPP_ |
||||
|
|
||||
|
#include "Command.hpp" |
||||
|
|
||||
|
#define BUFFER_SIZE (512) |
||||
|
#define NOTIFICATION_COMMAND_HEADER_SIZE (4) |
||||
|
#define NOTIFICATION_COMMAND_STOP_BYTE_SIZE (1) |
||||
|
#define NOTIFICATION_COMMAND_TOTAL_OVERHEAD (NOTIFICATION_COMMAND_HEADER_SIZE + NOTIFICATION_COMMAND_STOP_BYTE_SIZE) |
||||
|
#define NOTIFICATION_COMMAND_FREE_BUFFER (BUFFER_SIZE - NOTIFICATION_COMMAND_TOTAL_OVERHEAD) |
||||
|
|
||||
|
class Notification: public Command { |
||||
|
public: |
||||
|
|
||||
|
Notification(); |
||||
|
virtual ~Notification() {}; |
||||
|
|
||||
|
bool execute() override; |
||||
|
|
||||
|
uint16_t get_payload_size(); |
||||
|
void add_to_payload_size(uint16_t size); |
||||
|
|
||||
|
protected: |
||||
|
uint8_t * payload_ptr; |
||||
|
|
||||
|
private: |
||||
|
uint16_t payload_size = 0; |
||||
|
uint8_t data[BUFFER_SIZE]; |
||||
|
}; |
||||
|
|
||||
|
#endif /* NOTIFICATION_HPP_ */ |
||||
@ -0,0 +1,19 @@ |
|||||
|
/*
|
||||
|
* Request.cpp |
||||
|
* |
||||
|
* Created on: Jul 14, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#include "Request.hpp" |
||||
|
|
||||
|
Request::Request(uint8_t * payload_ptr, uint16_t size) : Command() { |
||||
|
this->response_identifier = payload_ptr[0] << 8 | payload_ptr[1]; |
||||
|
} |
||||
|
|
||||
|
bool Request::execute() { |
||||
|
Response * response = this->execute_request(this->response_identifier); |
||||
|
bool result = response->execute(); |
||||
|
delete response; |
||||
|
return result; |
||||
|
} |
||||
@ -0,0 +1,30 @@ |
|||||
|
/*
|
||||
|
* Request.hpp |
||||
|
* |
||||
|
* Created on: Jul 14, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#ifndef REQUEST_HPP_ |
||||
|
#define REQUEST_HPP_ |
||||
|
|
||||
|
#include "Command.hpp" |
||||
|
#include "Response.hpp" |
||||
|
|
||||
|
class Request : public Command { |
||||
|
public: |
||||
|
Request(uint8_t * payload_ptr, uint16_t size); |
||||
|
|
||||
|
bool execute() override; |
||||
|
|
||||
|
virtual Response * execute_request(uint16_t response_identifier) = 0; |
||||
|
|
||||
|
protected: |
||||
|
int buffer_offset = 2; |
||||
|
|
||||
|
private: |
||||
|
uint16_t response_identifier; |
||||
|
|
||||
|
}; |
||||
|
|
||||
|
#endif /* REQUEST_HPP_ */ |
||||
@ -0,0 +1,15 @@ |
|||||
|
/*
|
||||
|
* Response.cpp |
||||
|
* |
||||
|
* Created on: Jul 14, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#include "Response.hpp" |
||||
|
|
||||
|
Response::Response(uint16_t response_identifier) |
||||
|
: Notification(), response_identifier(response_identifier) { |
||||
|
this->payload_ptr[0] = (response_identifier & 0xff00) >> 8; |
||||
|
this->payload_ptr[1] = response_identifier & 0x00ff; |
||||
|
this->add_to_payload_size(2); |
||||
|
} |
||||
@ -0,0 +1,23 @@ |
|||||
|
/*
|
||||
|
* Response.hpp |
||||
|
* |
||||
|
* Created on: Jul 14, 2021 |
||||
|
* Author: Andreas Berthoud |
||||
|
*/ |
||||
|
|
||||
|
#ifndef RESPONSE_HPP_ |
||||
|
#define RESPONSE_HPP_ |
||||
|
|
||||
|
#include "Notification.hpp" |
||||
|
|
||||
|
class Response : public Notification { |
||||
|
public: |
||||
|
Response(uint16_t response_identifier); |
||||
|
virtual ~Response() {}; |
||||
|
|
||||
|
private: |
||||
|
uint16_t response_identifier; |
||||
|
|
||||
|
}; |
||||
|
|
||||
|
#endif /* RESPONSE_HPP_ */ |
||||
Loading…
Reference in new issue