117 lines
2.5 KiB
Python
117 lines
2.5 KiB
Python
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())
|