From e30c647f76d43040e278a02c2a10e2f08423d810 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Sadowski?= Date: Wed, 15 Jan 2025 11:41:19 +0000 Subject: [PATCH] Everything integrated, simple graphing --- Pipfile | 6 +++ a.py | 76 +++------------------------------- analysis_test.py | 51 +++++++++++++++++++++++ bokehtest.py | 79 +++++++++++++++++++++++++++++++++++ main_dm.py | 31 ++++++++++++++ measurement_station.py | 55 ++++++++++++++++--------- msp.py | 27 ++++++++++++ norsonic_fetcher.py | 29 ++----------- norsonic_parser.py | 26 ++++++++++++ thrust_stand.py | 93 ++++++++++++++++++++++++++++++++++++++---- 10 files changed, 347 insertions(+), 126 deletions(-) create mode 100644 analysis_test.py create mode 100644 bokehtest.py create mode 100644 main_dm.py create mode 100644 norsonic_parser.py diff --git a/Pipfile b/Pipfile index 4cc6a3f..fdb6bea 100644 --- a/Pipfile +++ b/Pipfile @@ -8,6 +8,12 @@ pyserial = "*" aiohttp = "*" typing-extensions = "*" websockets = "*" +aioftp = "*" +colorist = "*" +matplotlib = "*" +bokeh = "*" +bokeh-sampledata = "*" +scipy = "*" [dev-packages] diff --git a/a.py b/a.py index da82895..6874a0e 100644 --- a/a.py +++ b/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('= 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/analysis_test.py b/analysis_test.py new file mode 100644 index 0000000..cf42409 --- /dev/null +++ b/analysis_test.py @@ -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') diff --git a/bokehtest.py b/bokehtest.py new file mode 100644 index 0000000..5bcb56a --- /dev/null +++ b/bokehtest.py @@ -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) diff --git a/main_dm.py b/main_dm.py new file mode 100644 index 0000000..dfa720f --- /dev/null +++ b/main_dm.py @@ -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) diff --git a/measurement_station.py b/measurement_station.py index beab1bb..8c18b75 100644 --- a/measurement_station.py +++ b/measurement_station.py @@ -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) diff --git a/msp.py b/msp.py index d8aa425..03992e8 100644 --- a/msp.py +++ b/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 diff --git a/norsonic_fetcher.py b/norsonic_fetcher.py index 09b356a..152664b 100644 --- a/norsonic_fetcher.py +++ b/norsonic_fetcher.py @@ -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 = '' 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(): diff --git a/norsonic_parser.py b/norsonic_parser.py new file mode 100644 index 0000000..36d8e60 --- /dev/null +++ b/norsonic_parser.py @@ -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 + diff --git a/thrust_stand.py b/thrust_stand.py index 3c90a42..81d2b3b 100644 --- a/thrust_stand.py +++ b/thrust_stand.py @@ -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 +