init
This commit is contained in:
commit
d33eeb0b05
|
|
@ -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())
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue