From d33eeb0b051363a3db3a37db51fa170a597cbe4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Sadowski?= Date: Fri, 6 Dec 2024 10:04:33 +0000 Subject: [PATCH] init --- a.py | 116 +++++++++++++++++++++++++++++++++++++++++++ async_serial.py | 70 ++++++++++++++++++++++++++ msp.py | 128 ++++++++++++++++++++++++++++++++++++++++++++++++ thrust_stand.py | 95 +++++++++++++++++++++++++++++++++++ 4 files changed, 409 insertions(+) create mode 100644 a.py create mode 100644 async_serial.py create mode 100644 msp.py create mode 100644 thrust_stand.py diff --git a/a.py b/a.py new file mode 100644 index 0000000..da82895 --- /dev/null +++ b/a.py @@ -0,0 +1,116 @@ +import struct +import serial +import asyncio +import time +import operator +import functools +import threading + +import msp +import async_serial +from thrust_stand import ThrustStand, raw_thrust, raw_torque + +# baud 250000 +#version 1585 +print(time.time()) +s = serial.Serial(port='/dev/ttyUSB0', baudrate=250000) +print(time.time()) + +# ccode = bytes((1,)) + +# s.write(ccode) + + + +while True: + l = s.readline() + print(l) + if l == b'Ready\r\n': + break + +print('dupa') + +# # s.write(make_msp(2)) + +# # while True: +# # i = s.read() +# # print(f'{i[0]}: ({i})') + +pwm = 1000 + +# def xxx(): +# while True: +# code, data = read_msp(s) +# print('aaa') +# print(code) +# print(data.hex()) +# esc_voltage, esc_current, esc_power, load_thrust, load_left, rot_e, rot_o, temp0, temp1, temp2, basic_data_flag, acc_x, acc_y, acc_z, vibration, raw_pressure_p, raw_pressure_t, load_right, pro_data_flag = struct.unpack_from('= 1900: + break + + + + + + + # while True: + # global pwm + # print(pwm) + # res = await m.do_poll(pwm) + # print(res.load_thrust*1000) + # await asyncio.sleep(0.2) + # print(res) + + +# def worker(): +# asyncio.run(main()) + +# t = threading.Thread(target=worker) +# t.start() + +asyncio.run(main()) diff --git a/async_serial.py b/async_serial.py new file mode 100644 index 0000000..88735e9 --- /dev/null +++ b/async_serial.py @@ -0,0 +1,70 @@ +import asyncio +from typing import cast +from serial.serialutil import SerialException +from typing_extensions import Buffer +import serial + +class X(asyncio.StreamReaderProtocol): + ... + +class SerialTransport(asyncio.Transport): + ser: serial.Serial + loop: asyncio.BaseEventLoop + + def __init__(self, loop, protocol, serial: serial.Serial): + serial.timeout = 0 + + self.loop = loop + self.protocol = protocol + self.ser = serial + + self.paused = False + self._read_ready() + + def write(self, data: Buffer): + self.ser.write(data) + + def _read_ready(self): + READ_BUFF_SIZE = 2048 + + while True: + try: + data = self.ser.read(READ_BUFF_SIZE) + except SerialException as e: + self.protocol.connection_lost(e) + return + if len(data) == 0: + break + self.protocol.data_received(data) + + if self.ser.fd is None: + raise RuntimeError('Invalid serial fd') + self.loop.add_reader(cast(int, self.ser.fd), self._read_ready) + + def is_closing(self) -> bool: + print('asked if closing') + return True + + +# def serial_create_connection(protocol_factory, serial): +# loop = asyncio.get_event_loop() +# protocol = protocol_factory() +# transport = SerialTransport(loop, protocol, serial) +# return transport, protocol + + +# def serial_open_connection(serial): +# reader = streams.StreamReader() +# protocol = streams.StreamReaderProtocol(reader) +# factory = lambda: protocol +# transport, _ = serial_create_connection(factory, serial) +# writer = streams.StreamWriter(transport, protocol, reader, asyncio.get_running_loop()) +# return reader, writer + +def wrap_serial(serial: serial.Serial): + loop = asyncio.get_running_loop() + reader = asyncio.StreamReader() + protocol = asyncio.StreamReaderProtocol(reader) + transport = SerialTransport(loop, protocol, serial) + writer = asyncio.StreamWriter(transport, protocol, reader, loop) + return reader, writer diff --git a/msp.py b/msp.py new file mode 100644 index 0000000..d8aa425 --- /dev/null +++ b/msp.py @@ -0,0 +1,128 @@ +from asyncio import Future, StreamReader, StreamWriter, Task, shield +import asyncio +import functools +import operator +import struct +from typing import Awaitable, Dict, Optional, Self +from dataclasses import dataclass + +MSP_MAGIC_OUT = b"$R<" +MSP_MAGIC_IN = b"$R>" +MSP_CODE_POLL = 3 + +def make_msp(code: int, data: bytes = b'') -> bytes: + ret = bytearray(MSP_MAGIC_OUT) + if len(data) > 255: + raise ValueError() + ret.append(len(data)) + ret.append(code) + ret += data + checksum = functools.reduce(operator.xor, map(int, ret[len(MSP_MAGIC_OUT):])) + ret.append(checksum) + return bytes(ret) + +async def read_msp(r: StreamReader) -> tuple[int, bytes]: + magic_pos = 0 + while magic_pos < len(MSP_MAGIC_IN): + b, = await r.readexactly(1) + if b == MSP_MAGIC_IN[magic_pos]: + magic_pos += 1 + else: + magic_pos = 0 + print('Unexpected byte from RCBenchmark serial') + size, = await r.readexactly(1) + code, = await r.readexactly(1) + data = await r.readexactly(size) + checksum, = await r.readexactly(1) + _ = checksum + return code, data + + +def make_poll(esc_pwm: int) -> bytes: + data = struct.pack('HHHH', esc_pwm, 0, 0, 0) + # return make_msp(MSP_CODE_POLL, data) + return data + + +# esc_voltage, esc_current, esc_power, load_thrust, load_left, rot_e, rot_o, temp0, temp1, temp2, basic_data_flag, acc_x, acc_y, acc_z, vibration, raw_pressure_p, raw_pressure_t, load_right, pro_data_flag = struct.unpack_from(' Self: + unpacked = struct.unpack_from(' Awaitable[bytes]: + if code in self.pending_requests: + raise RuntimeError('Duplicate request') + f = Future() + def future_done(_): + del self.pending_requests[code] + self.pending_requests[code] = f + f.add_done_callback(future_done) + self.w.write(make_msp(code, data)) + return shield(f) + + async def do_poll(self, esc_pwm: int) -> PollResponse: + resp = await self.do_request(MSP_CODE_POLL, make_poll(esc_pwm)) + return PollResponse.from_bytes(resp) diff --git a/thrust_stand.py b/thrust_stand.py new file mode 100644 index 0000000..3c90a42 --- /dev/null +++ b/thrust_stand.py @@ -0,0 +1,95 @@ +import asyncio +from asyncio.futures import Future +from asyncio.tasks import Task +from time import time +from typing import Optional, cast +from msp import MSPSlave, PollResponse + + +HIST_SIZE = 100 +GRAVITY_CONST = 9.80665 +HINGE_DISTANCE = 0.07492 +THRUST_CONST = 1000 * 5 / 5 * GRAVITY_CONST +TORQUE_CONST = 1000 * 2 / 5 * GRAVITY_CONST * HINGE_DISTANCE / 2 + +CAL_HINGE_LEFT = 1.2100092475098374 +CAL_HINGE_RIGHT = 1.2590952216896254 +CAL_LEFT = 0.9663293361785854 +CAL_RIGHT = -0.9575068323376389 +CAL_THRUST = 0.9516456828857573 +CAL_TORQUE_LEFT = CAL_LEFT * CAL_HINGE_LEFT +CAL_TORQUE_RIGHT = CAL_RIGHT * CAL_HINGE_RIGHT + +def raw_torque(val_left: float, val_right: float) -> float: + return ((val_right * CAL_TORQUE_RIGHT) - (val_left * CAL_TORQUE_LEFT)) * TORQUE_CONST + +def raw_thrust(val: float) -> float: + return val * CAL_THRUST * THRUST_CONST + +class ThrustStandMeasurement: + pass + + +class ThrustStand: + mot_pwm: int + msp: MSPSlave + sample_number: int + samples: list[PollResponse] + + tare_thrust: float + tare_torque: float + + + _next_sample_future: Future + _poller_task: Optional[Task] + _closed: bool + + def __init__(self, msp: MSPSlave): + self.msp = msp + self.mot_pwm = 1000 + self._poller_task = None + self.samples = [] + self._next_sample_future = Future() + + + async def _poller(self): + while True: + + await asyncio.gather( + asyncio.sleep(0.03), + self._do_poll() + ) + + async def _do_poll(self): + + res = await self.msp.do_poll(self.mot_pwm) + self.samples.append(res) + # print(res.rot_e) + self._next_sample_future.set_result(None) + self._next_sample_future = Future() + + async def ensure_running(self): + if self._poller_task is None: + self._poller_task = asyncio.create_task(self._poller()) + + async def wait_samples(self, n): + for _ in range(n): + await self.next_sample() + + async def stabilize_rpm(self, window: int, tolerance: float): + await self.wait_samples(window) + + while True: + samples = self.samples[-window:] + rpms = list(map(lambda s: s.rot_e, samples)) + + if max(rpms) - min(rpms) < tolerance: + return + await self.next_sample() + + + + + def next_sample(self) -> Future: + return asyncio.shield(self._next_sample_future) +