Everything integrated, simple graphing
This commit is contained in:
parent
e10661be5e
commit
e30c647f76
6
Pipfile
6
Pipfile
|
|
@ -8,6 +8,12 @@ pyserial = "*"
|
|||
aiohttp = "*"
|
||||
typing-extensions = "*"
|
||||
websockets = "*"
|
||||
aioftp = "*"
|
||||
colorist = "*"
|
||||
matplotlib = "*"
|
||||
bokeh = "*"
|
||||
bokeh-sampledata = "*"
|
||||
scipy = "*"
|
||||
|
||||
[dev-packages]
|
||||
|
||||
|
|
|
|||
76
a.py
76
a.py
|
|
@ -1,10 +1,6 @@
|
|||
import struct
|
||||
import serial
|
||||
import asyncio
|
||||
import time
|
||||
import operator
|
||||
import functools
|
||||
import threading
|
||||
|
||||
import msp
|
||||
import async_serial
|
||||
|
|
@ -16,12 +12,6 @@ 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)
|
||||
|
|
@ -30,40 +20,6 @@ while True:
|
|||
|
||||
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)
|
||||
|
|
@ -74,18 +30,16 @@ async def main():
|
|||
await thr.ensure_running()
|
||||
await asyncio.sleep(10)
|
||||
|
||||
sample = thr.samples[-1]
|
||||
sample = thr.samples_raw[-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)
|
||||
if thr.mot_pwm == 1000:
|
||||
thr.mot_pwm = 1100
|
||||
thr.mot_pwm += 10
|
||||
# print(thr.mot_pwm)
|
||||
# await asyncio.sleep(1)
|
||||
await thr.stabilize_rpm(4, 1)
|
||||
sample = thr.samples[-1]
|
||||
await thr.stabilize_rpm(5, 2)
|
||||
sample = thr.samples_raw[-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}')
|
||||
|
|
@ -93,24 +47,4 @@ async def main():
|
|||
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,51 @@
|
|||
import pickle
|
||||
import math
|
||||
|
||||
from measurement_station import OpPointData
|
||||
|
||||
class MyCustomUnpickler(pickle.Unpickler):
|
||||
def find_class(self, module, name):
|
||||
if module == '__main__':
|
||||
module = 'measurement_station'
|
||||
return super().find_class(module, name)
|
||||
|
||||
f = open('m3.pickle', 'rb')
|
||||
data = MyCustomUnpickler(f).load()
|
||||
d1 = data[1100]
|
||||
|
||||
def prrow(r: OpPointData):
|
||||
print(r.data_thrust_stand_avg.thrust, end=',')
|
||||
print(r.data_thrust_stand_avg.torque, end=',')
|
||||
print(r.data_thrust_stand_avg.rot_speed, end=',')
|
||||
for v in r.data_accustic_avg.values():
|
||||
print(v, end=',')
|
||||
print()
|
||||
|
||||
print(list(list(data.values())[0].data_accustic_avg.keys()))
|
||||
|
||||
for pwm, d in data.items():
|
||||
prrow(d)
|
||||
|
||||
# thrusts = [d.data_thrust_stand_avg.thrust for d in data.values()]
|
||||
# powers = [d.data_thrust_stand_avg.torque * d.data_thrust_stand_avg.rot_speed * -2*math.pi / 7 for d in data.values()]
|
||||
# laeqs = [d.data_accustic_avg['S1 LAeq'] for d in data.values()]
|
||||
# lzeqs = [d.data_accustic_avg['S1 LZeq'] for d in data.values()]
|
||||
|
||||
# import matplotlib.pyplot as plt
|
||||
|
||||
# _, ax1 = plt.subplots()
|
||||
|
||||
# ax1.plot(thrusts, laeqs, label="LAeq [dB]", color='blue')
|
||||
# ax1.plot(thrusts, lzeqs, label="LZeq [dB]", color='red')
|
||||
|
||||
# ax1.set_xlabel("Thrust [N]")
|
||||
|
||||
# ax2 = ax1.twinx()
|
||||
# ax2.plot(thrusts, powers, label="Power [W]", color='green')
|
||||
|
||||
# ax1.legend()
|
||||
# ax2.legend(loc=1)
|
||||
|
||||
# plt.title("Powertrain noise measurement")
|
||||
|
||||
# plt.savefig('fig.svg')
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
from dataclasses import fields
|
||||
from typing import Iterable, Sequence
|
||||
from bokeh.core.enums import SizingMode, SizingModeType
|
||||
from bokeh.io import curdoc
|
||||
from bokeh.layouts import column
|
||||
from bokeh.models import ColumnDataSource, MultiChoice, Select
|
||||
from bokeh.plotting import figure
|
||||
import pickle
|
||||
|
||||
from measurement_station import OpPointData
|
||||
|
||||
|
||||
def oppoint_data_src(p: OpPointData) -> dict[str, float]:
|
||||
ret = {}
|
||||
|
||||
# print(fields(p.data_thrust_stand_avg)[0])
|
||||
|
||||
ret.update({k: v for k, v in field_entries(p.data_thrust_stand_avg).items()})
|
||||
|
||||
return ret
|
||||
|
||||
def field_entries(datacls) -> dict[str, float]:
|
||||
fnames = [f.name for f in fields(datacls)]
|
||||
return {n: getattr(datacls, n) for n in fnames}
|
||||
|
||||
|
||||
def read_series(fname: str) -> dict[int, OpPointData]:
|
||||
with open(fname, 'rb') as f:
|
||||
return pickle.load(f)
|
||||
|
||||
def series_dataclosrc(series: dict[int, OpPointData]) -> ColumnDataSource:
|
||||
# entries = [field_entries(v) for v in series.values()]
|
||||
entries = [oppoint_data_src(p) for p in series.values()]
|
||||
keys = entries[0].keys()
|
||||
data = { key: [e[key] for e in entries] for key in keys }
|
||||
return ColumnDataSource(data)
|
||||
|
||||
|
||||
filespeed = 15
|
||||
|
||||
files = [ f'./dmuch/gf7035_v{filespeed}.pickle', f'./dmuch/gf7042_v{filespeed}.pickle', f'./dmuch/apc838_v{filespeed}.pickle', f'./dmuch/apc938_v{filespeed}.pickle', f'./dmuch/ep_v{filespeed}.pickle' ]
|
||||
|
||||
series = [ read_series(f) for f in files ]
|
||||
|
||||
print(oppoint_data_src(series[0][1500]))
|
||||
|
||||
print(series_dataclosrc(series[0]))
|
||||
|
||||
sources = [series_dataclosrc(s) for s in series]
|
||||
|
||||
keys = list(sources[0].data.keys())
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
p = figure(title="Dynamic Column Plot", x_axis_label='Index', y_axis_label='Value', tools=['hover', 'pan', 'xwheel_zoom'])
|
||||
p.sizing_mode = 'scale_both' # type: ignore
|
||||
|
||||
multi_choice = MultiChoice(title="Choose Columns to Plot", options=keys)
|
||||
xsel = Select(title="Y axis:", value="cos", options=keys)
|
||||
|
||||
colors = ['blue', 'green', 'red', 'yellow', 'orange']
|
||||
|
||||
def update_plot(attr, _, new_values):
|
||||
p.renderers = [] # type: ignore
|
||||
|
||||
for source, fname, color in zip(sources, files, colors):
|
||||
for column in multi_choice.value: # type: ignore
|
||||
p.line(x=xsel.value, y=column, source=source, legend_label=f'{fname} {column}', line_width=2, color=color)
|
||||
|
||||
multi_choice.on_change('value', update_plot)
|
||||
xsel.on_change('value', update_plot)
|
||||
|
||||
layout = column(multi_choice, xsel, p)
|
||||
layout.sizing_mode = 'scale_both' # type: ignore
|
||||
|
||||
curdoc().add_root(layout)
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
import asyncio
|
||||
from measurement_station import ConnectionParams, meas_series
|
||||
|
||||
|
||||
CONN_PARAMS = ConnectionParams(
|
||||
stand_tty='/dev/ttyUSB0',
|
||||
nor_addr='10.145.1.1',
|
||||
nor_ftp_user='AAAA',
|
||||
nor_ftp_pass='1234',
|
||||
nor_recordings_dir='/SD Card/NorMeas/Nor14530408/TEST'
|
||||
)
|
||||
|
||||
|
||||
|
||||
range(1200, 1500, 50)
|
||||
|
||||
|
||||
async def main():
|
||||
global x
|
||||
x = await meas_series(CONN_PARAMS, range(1200, 1800, 100))
|
||||
|
||||
|
||||
|
||||
# logging.basicConfig(level=logging.INFO)
|
||||
asyncio.run(main(), debug=True)
|
||||
|
||||
import pickle
|
||||
import sys
|
||||
|
||||
with open(sys.argv[1], 'wb') as f:
|
||||
pickle.dump(x, f)
|
||||
|
|
@ -3,18 +3,26 @@ from collections.abc import Iterable, Sequence
|
|||
from dataclasses import dataclass
|
||||
from logging import debug
|
||||
import logging
|
||||
from typing import Optional
|
||||
|
||||
from logger import spr
|
||||
from norsonic_parser import parse_report
|
||||
import thrust_stand
|
||||
import norsonic
|
||||
|
||||
from norsonic_fetcher import nor_get_reports, recording_path
|
||||
from norsonic_fetcher import nor_get_reports
|
||||
from thrust_stand import ThrustStand, ThrustStandMeasurement
|
||||
|
||||
@dataclass
|
||||
@dataclass(frozen=True)
|
||||
class OpPointData:
|
||||
data_thrust_stand: Sequence[ThrustStandMeasurement]
|
||||
data_accustic: dict
|
||||
raw_nor_report: Optional[bytes]
|
||||
|
||||
@property
|
||||
def data_accustic(self):
|
||||
if self.raw_nor_report is None:
|
||||
raise ValueError()
|
||||
return parse_report(self.raw_nor_report)
|
||||
|
||||
@property
|
||||
def data_thrust_stand_avg(self):
|
||||
|
|
@ -24,6 +32,12 @@ class OpPointData:
|
|||
def data_accustic_avg(self):
|
||||
return {k: sum(map(float, (row[k] for row in self.data_accustic)))/len(self.data_accustic) for k in self.data_accustic[0].keys() if k != 'Date'}
|
||||
|
||||
import sys
|
||||
async def ainput(string: str) -> str:
|
||||
await asyncio.get_event_loop().run_in_executor(
|
||||
None, lambda s=string: sys.stdout.write(s+' '))
|
||||
return await asyncio.get_event_loop().run_in_executor(
|
||||
None, sys.stdin.readline)
|
||||
|
||||
|
||||
@dataclass
|
||||
|
|
@ -34,14 +48,6 @@ class ConnectionParams:
|
|||
nor_ftp_pass: str
|
||||
nor_recordings_dir: str
|
||||
|
||||
CONN_PARAMS = ConnectionParams(
|
||||
stand_tty='/dev/ttyUSB0',
|
||||
nor_addr='10.145.1.1',
|
||||
nor_ftp_user='AAAA',
|
||||
nor_ftp_pass='1234',
|
||||
nor_recordings_dir='/SD Card/NorMeas/Nor14530408/TEST'
|
||||
)
|
||||
|
||||
async def meas_series(params: ConnectionParams, pwms: Iterable[int]):
|
||||
stand = await ThrustStand.open_connection(params.stand_tty)
|
||||
nor = await norsonic.open_connection(params.nor_addr)
|
||||
|
|
@ -53,16 +59,31 @@ async def meas_series(params: ConnectionParams, pwms: Iterable[int]):
|
|||
sample, = stand.get_samples_raw(1)
|
||||
stand.tare_thrust = thrust_stand.raw_thrust(sample.load_thrust)
|
||||
stand.tare_torque = thrust_stand.raw_torque(sample.load_left, sample.load_right)
|
||||
stand.tare_current = sample.esc_current
|
||||
|
||||
stand.mot_pwm = 1000
|
||||
|
||||
####
|
||||
# stand.mot_pwm = 1200
|
||||
# await ainput("Press Enter to continue...")
|
||||
|
||||
|
||||
for pwm in pwms:
|
||||
stand.mot_pwm = pwm
|
||||
spr(f'Output: {pwm}PWM')
|
||||
await stand.stabilize_rpm(5, 1)
|
||||
await stand.stabilize_rpm(10, 4)
|
||||
spr(f'Starting measurement')
|
||||
stand_series_pending = stand.start_meas_series()
|
||||
#
|
||||
files_nor[pwm] = await norsonic.record(nor)
|
||||
await asyncio.sleep(1)
|
||||
spr(f'Done')
|
||||
results_stand[pwm] = stand.finish_meas_series(stand_series_pending)
|
||||
print(results_stand[pwm][0])
|
||||
|
||||
while stand.mot_pwm > 1000:
|
||||
stand.mot_pwm -= 10
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
stand.mot_pwm = 1000
|
||||
|
||||
|
|
@ -77,15 +98,9 @@ async def meas_series(params: ConnectionParams, pwms: Iterable[int]):
|
|||
ret = {
|
||||
pwm: OpPointData(
|
||||
data_thrust_stand=results_stand[pwm],
|
||||
data_accustic=nor_reports[i]
|
||||
# raw_nor_report=None
|
||||
raw_nor_report=nor_reports[i]
|
||||
) for i, pwm in enumerate(pwms)
|
||||
}
|
||||
return ret
|
||||
|
||||
|
||||
async def main():
|
||||
global x
|
||||
x = await meas_series(CONN_PARAMS, range(1100, 1950, 30))
|
||||
|
||||
# logging.basicConfig(level=logging.INFO)
|
||||
# asyncio.run(main(), debug=True)
|
||||
|
|
|
|||
27
msp.py
27
msp.py
|
|
@ -6,10 +6,17 @@ import struct
|
|||
from typing import Awaitable, Dict, Optional, Self
|
||||
from dataclasses import dataclass
|
||||
|
||||
import serial
|
||||
|
||||
import async_serial
|
||||
from logger import spr
|
||||
|
||||
MSP_MAGIC_OUT = b"$R<"
|
||||
MSP_MAGIC_IN = b"$R>"
|
||||
MSP_CODE_POLL = 3
|
||||
|
||||
MSP_TTY_DEF_BAUDRATE = 250000
|
||||
|
||||
def make_msp(code: int, data: bytes = b'') -> bytes:
|
||||
ret = bytearray(MSP_MAGIC_OUT)
|
||||
if len(data) > 255:
|
||||
|
|
@ -126,3 +133,23 @@ class MSPSlave:
|
|||
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)
|
||||
|
||||
# TODO
|
||||
# ASYNC rozjebion
|
||||
# TODO
|
||||
@staticmethod
|
||||
async def open_connection(tty: str, baudrate = MSP_TTY_DEF_BAUDRATE) -> 'MSPSlave':
|
||||
s = serial.Serial(port=tty, baudrate = baudrate)
|
||||
|
||||
while True:
|
||||
l = s.readline()
|
||||
print(l)
|
||||
if l == b'Ready\r\n':
|
||||
break
|
||||
|
||||
spr('MSP: Ready')
|
||||
|
||||
r, w = async_serial.wrap_serial(s)
|
||||
m = MSPSlave(r, w)
|
||||
await m.ensure_reader()
|
||||
return m
|
||||
|
|
|
|||
|
|
@ -10,7 +10,6 @@ FNAME = '/SD Card/NorMeas/Nor14530408/TEST/VIP 124 2024-12-10 15-31-19/VIP 124 2
|
|||
DNAME = '/SD Card/NorMeas/Nor14530408/TEST/VIP 124 2024-12-10 15-31-19/'
|
||||
RECORDINGS_PATH = '/SD Card/NorMeas/Nor14530408/TEST'
|
||||
CRLF = '\r\n'
|
||||
COL_SEPARATOR = '\t'
|
||||
STR_DIR = '<DIR>'
|
||||
|
||||
def recording_path(rec_name: str) -> str:
|
||||
|
|
@ -40,32 +39,10 @@ async def ftp_fetch(client: aioftp.Client, path: str) -> bytes:
|
|||
stream.close()
|
||||
return file
|
||||
|
||||
def parse_report_table(table: str):
|
||||
_, head_cols, *rows = table.split(CRLF)
|
||||
cols = head_cols.split(COL_SEPARATOR)
|
||||
|
||||
def parse_row(row: str):
|
||||
vals = row.split(COL_SEPARATOR)
|
||||
if len(vals) != len(cols):
|
||||
print(vals)
|
||||
print(cols)
|
||||
print(len(cols))
|
||||
print(len(vals))
|
||||
raise ValueError('NorPaeser: Invalid row read')
|
||||
return {cols[i]: vals[i] for i in range(len(cols))}
|
||||
|
||||
return [parse_row(r) for r in rows if len(r) > 0]
|
||||
|
||||
def parse_report(report: bytes):
|
||||
*header, glob, prof = report.decode().split(2*CRLF)
|
||||
t_prof = parse_report_table(prof)
|
||||
# t_glob = parse_report_table(glob)
|
||||
# return t_glob, t_prof
|
||||
return t_prof
|
||||
|
||||
async def nor_get_reports(addr: str, user: str, password: str, recs: Iterable[str]) -> Sequence:
|
||||
async def nor_get_reports(addr: str, user: str, password: str, recs: Iterable[str]) -> Sequence[bytes]:
|
||||
async with aioftp.Client.context(addr, user=user, password=password, parse_list_line_custom=ftp_parse_line) as ftp:
|
||||
return [parse_report(await ftp_fetch(ftp, recording_path(p))) for p in recs]
|
||||
return [await ftp_fetch(ftp, recording_path(p)) for p in recs]
|
||||
|
||||
|
||||
|
||||
async def main():
|
||||
|
|
|
|||
|
|
@ -0,0 +1,26 @@
|
|||
CRLF = '\r\n'
|
||||
COL_SEPARATOR = '\t'
|
||||
|
||||
def parse_report_table(table: str):
|
||||
_, head_cols, *rows = table.split(CRLF)
|
||||
cols = head_cols.split(COL_SEPARATOR)
|
||||
|
||||
def parse_row(row: str):
|
||||
vals = row.split(COL_SEPARATOR)
|
||||
if len(vals) != len(cols):
|
||||
print(vals)
|
||||
print(cols)
|
||||
print(len(cols))
|
||||
print(len(vals))
|
||||
raise ValueError('NorPaeser: Invalid row read')
|
||||
return {cols[i]: vals[i] for i in range(len(cols))}
|
||||
|
||||
return [parse_row(r) for r in rows if len(r) > 0]
|
||||
|
||||
def parse_report(report: bytes):
|
||||
*header, glob, prof = report.decode().split(2*CRLF)
|
||||
t_prof = parse_report_table(prof)
|
||||
# t_glob = parse_report_table(glob)
|
||||
# return t_glob, t_prof
|
||||
return t_prof
|
||||
|
||||
|
|
@ -1,8 +1,13 @@
|
|||
import asyncio
|
||||
import numbers
|
||||
import statistics
|
||||
from asyncio.futures import Future
|
||||
from asyncio.tasks import Task
|
||||
from collections.abc import Sequence
|
||||
from dataclasses import dataclass, fields
|
||||
import operator
|
||||
from time import time
|
||||
from typing import Optional, cast
|
||||
from typing import Any, Callable, Optional, Self, cast
|
||||
from msp import MSPSlave, PollResponse
|
||||
|
||||
|
||||
|
|
@ -12,6 +17,7 @@ HINGE_DISTANCE = 0.07492
|
|||
THRUST_CONST = 1000 * 5 / 5 * GRAVITY_CONST
|
||||
TORQUE_CONST = 1000 * 2 / 5 * GRAVITY_CONST * HINGE_DISTANCE / 2
|
||||
|
||||
CAL_POLES = 14
|
||||
CAL_HINGE_LEFT = 1.2100092475098374
|
||||
CAL_HINGE_RIGHT = 1.2590952216896254
|
||||
CAL_LEFT = 0.9663293361785854
|
||||
|
|
@ -19,6 +25,7 @@ CAL_RIGHT = -0.9575068323376389
|
|||
CAL_THRUST = 0.9516456828857573
|
||||
CAL_TORQUE_LEFT = CAL_LEFT * CAL_HINGE_LEFT
|
||||
CAL_TORQUE_RIGHT = CAL_RIGHT * CAL_HINGE_RIGHT
|
||||
CAL_SYNCSPEED = 2/CAL_POLES * 2* 3.1415
|
||||
|
||||
def raw_torque(val_left: float, val_right: float) -> float:
|
||||
return ((val_right * CAL_TORQUE_RIGHT) - (val_left * CAL_TORQUE_LEFT)) * TORQUE_CONST
|
||||
|
|
@ -26,18 +33,58 @@ def raw_torque(val_left: float, val_right: float) -> float:
|
|||
def raw_thrust(val: float) -> float:
|
||||
return val * CAL_THRUST * THRUST_CONST
|
||||
|
||||
def raw_torque_resp(raw: PollResponse) -> float:
|
||||
return raw_torque(raw.load_left, raw.load_right)
|
||||
|
||||
def raw_thrust_resp(raw: PollResponse) -> float:
|
||||
return raw_thrust(raw.load_thrust)
|
||||
|
||||
@dataclass
|
||||
class PendingMeasurementSeries:
|
||||
start_sample_num: int
|
||||
|
||||
|
||||
@dataclass
|
||||
class ThrustStandMeasurement:
|
||||
pass
|
||||
thrust: float
|
||||
torque: float
|
||||
rot_speed: float
|
||||
volt: float
|
||||
current: float
|
||||
|
||||
@classmethod
|
||||
def num_operation(cls, op: Callable[[numbers.Real, numbers.Real], numbers.Real], a: Self, b: Self | numbers.Real):
|
||||
results = {}
|
||||
for f in fields(cls):
|
||||
val_a: numbers.Real = a if isinstance(a, numbers.Real) else getattr(a, f.name)
|
||||
val_b: numbers.Real = b if isinstance(b, numbers.Real) else getattr(b, f.name)
|
||||
results[f.name] = op(val_a, val_b)
|
||||
return cls(**results)
|
||||
|
||||
def __add__(self, other: Self):
|
||||
return self.num_operation(operator.add, self, other)
|
||||
|
||||
def __truediv__(self, divisor):
|
||||
# def __truediv__(self, divisor):
|
||||
return self.num_operation(operator.truediv, self, divisor)
|
||||
|
||||
def __pow__(self, power):
|
||||
return self.num_operation(operator.pow, self, power)
|
||||
|
||||
@classmethod
|
||||
def zero(cls) -> Self:
|
||||
return cls(0, 0, 0, 0, 0)
|
||||
|
||||
|
||||
class ThrustStand:
|
||||
mot_pwm: int
|
||||
msp: MSPSlave
|
||||
sample_number: int
|
||||
samples: list[PollResponse]
|
||||
samples_raw: list[PollResponse]
|
||||
|
||||
tare_thrust: float
|
||||
tare_torque: float
|
||||
tare_current: float
|
||||
|
||||
|
||||
_next_sample_future: Future
|
||||
|
|
@ -48,7 +95,8 @@ class ThrustStand:
|
|||
self.msp = msp
|
||||
self.mot_pwm = 1000
|
||||
self._poller_task = None
|
||||
self.samples = []
|
||||
self.sample_number = 0
|
||||
self.samples_raw = []
|
||||
self._next_sample_future = Future()
|
||||
|
||||
|
||||
|
|
@ -63,7 +111,8 @@ class ThrustStand:
|
|||
async def _do_poll(self):
|
||||
|
||||
res = await self.msp.do_poll(self.mot_pwm)
|
||||
self.samples.append(res)
|
||||
self.samples_raw.append(res)
|
||||
self.sample_number += 1
|
||||
# print(res.rot_e)
|
||||
self._next_sample_future.set_result(None)
|
||||
self._next_sample_future = Future()
|
||||
|
|
@ -80,16 +129,42 @@ class ThrustStand:
|
|||
await self.wait_samples(window)
|
||||
|
||||
while True:
|
||||
samples = self.samples[-window:]
|
||||
samples = self.samples_raw[-window:]
|
||||
rpms = list(map(lambda s: s.rot_e, samples))
|
||||
|
||||
if max(rpms) - min(rpms) < tolerance:
|
||||
return
|
||||
# print(rpms)
|
||||
await self.next_sample()
|
||||
|
||||
|
||||
|
||||
|
||||
def next_sample(self) -> Future:
|
||||
return asyncio.shield(self._next_sample_future)
|
||||
|
||||
def sample_from_raw(self, raw: PollResponse) -> ThrustStandMeasurement:
|
||||
return ThrustStandMeasurement(
|
||||
raw_thrust_resp(raw) - self.tare_thrust,
|
||||
raw_torque_resp(raw) - self.tare_torque, raw.rot_e,
|
||||
raw.esc_voltage,
|
||||
raw.esc_current - self.tare_current,
|
||||
)
|
||||
|
||||
def get_samples_raw(self, n: int) -> Sequence[PollResponse]:
|
||||
return self.samples_raw[-n:]
|
||||
|
||||
def get_samples(self, n: int) -> Sequence[ThrustStandMeasurement]:
|
||||
return list(map(self.sample_from_raw, self.get_samples_raw(n)))
|
||||
|
||||
def start_meas_series(self) -> PendingMeasurementSeries:
|
||||
return PendingMeasurementSeries(self.sample_number)
|
||||
|
||||
def finish_meas_series(self, meas: PendingMeasurementSeries) -> Sequence[ThrustStandMeasurement]:
|
||||
return self.get_samples(self.sample_number - meas.start_sample_num)
|
||||
|
||||
@staticmethod
|
||||
async def open_connection(tty: str):
|
||||
thr = ThrustStand(await MSPSlave.open_connection(tty))
|
||||
await thr.ensure_running()
|
||||
await asyncio.sleep(5)
|
||||
return thr
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue