init

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('<ffffffffffchhhhhhfc', data)
+#         # print(esc_voltage)
+#         # print(esc_current)
+#         # print(esc_power)
+#         # print(rot_e)
+
+#         print(load_thrust)
+#         print(load_left)
+#         print(load_right)
+
+#         # print(acc_x)
+#         # print(acc_y)
+#         # print(acc_z)
+
+# t = threading.Thread(target=xxx)
+# t.start()
+
+# while True:
+#     s.write(make_poll(pwm))
+#     time.sleep(0.2)
+
+async def main():
+    r, w = async_serial.wrap_serial(s)
+    m = msp.MSPSlave(r, w)
+    await m.ensure_reader()
+    print('aaaaa')
+    thr = ThrustStand(m)
+    await thr.ensure_running()
+    await asyncio.sleep(10)
+
+    sample = thr.samples[-1]
+    thrust_tare = raw_thrust(sample.load_thrust)
+    torque_tare = raw_torque(sample.load_left, sample.load_right)
+
+    while True:
+        # thr.mot_pwm = 2000
+        # await asyncio.sleep(1000)
+        thr.mot_pwm += 10
+        # print(thr.mot_pwm)
+        # await asyncio.sleep(1)
+        await thr.stabilize_rpm(4, 1)
+        sample = thr.samples[-1]
+        thrust = raw_thrust(sample.load_thrust) - thrust_tare
+        torque = raw_torque(sample.load_left, sample.load_right) - torque_tare
+        print(f'{thr.mot_pwm} {sample.rot_e} {thrust} {torque}')
+
+        if thr.mot_pwm >= 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())

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

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)
+    # 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('<ffffffffffchhhhhhfc', data)
+
+@dataclass
+class PollResponse:
+    esc_voltage: float
+    esc_current: float
+    esc_power: float
+    load_thrust: float
+    load_left: float
+    rot_e: float
+    rot_o: float
+    temp0: float
+    temp1: float
+    temp2: float
+    basic_data_flag: int
+    acc_x: int
+    acc_y: int
+    acc_z: int
+    vibration: int
+    raw_pressure_p: int
+    raw_pressure_t: int
+    load_right: float
+    pro_data_flag: int
+
+    @classmethod
+    def from_bytes(cls, data: bytes) -> Self:
+        unpacked = struct.unpack_from('<ffffffffffchhhhhhfc', data)
+        # print(unpacked)
+        return cls(*unpacked)
+
+
+
+
+class MSPSlave:
+    w: StreamWriter
+    r: StreamReader
+
+    pending_requests: Dict[int, Future[bytes]]
+    _reader_task: Optional[Task]
+
+
+    def __init__(self, reader: StreamReader, writer: StreamWriter):
+        self.r = reader
+        self.w = writer
+        self._reader_task = None
+        self.pending_requests = {}
+
+    async def ensure_reader(self):
+        if self._reader_task is None:
+            self._reader_task = asyncio.create_task(self._reader())
+
+    async def _reader(self):
+        try:
+            while True:
+                code, data = await read_msp(self.r)
+                self.handle_packet(code, data)
+        except Exception as e:
+            for req in self.pending_requests.values():
+                req.set_exception(e)
+            
+
+    def handle_packet(self, code: int, data: bytes):
+        if code in self.pending_requests:
+            self.pending_requests[code].set_result(data)
+        else:
+            raise RuntimeError(f'Unexpected packet: {code}')
+
+    def do_request(self, code: int, data: bytes) -> 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)

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)
+