wonderful
/
usvcontrol
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

打印数据，共调试使用

This commit is contained in:
STT 2021-12-01 11:51:04 +08:00
parent 85704aead2
commit 1cbde98046
2 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ground_control_station.py
View File

@ -41,19 +41,16 @@ class GroundControlStation:
packet_data = self.buffer[5:packet_length]
if packet_id == 0 and packet_data_length == 8: # 心跳包
print('心跳包' + str(time.time()))
self.heart_beat['timestamp'] = time.time()
self.heart_beat['buffer_err'] = self.errors
del self.buffer[:packet_length]
continue
elif (packet_data[0] | packet_data[1] << 8) != usv.control.pid['id']: # 数据过滤
print('数据过滤')
del self.buffer[:packet_length]
continue
elif packet_id == 1 and packet_data_length == 40: # Command
print('命令包')
command = struct.unpack('<Bffddhhb', bytes(packet_data[10:packet_data_length]))
self.command['timestamp'] = time.time()
self.command['setting'] = command[0]
@ -68,7 +65,6 @@ class GroundControlStation:
continue
elif packet_id == 2 and packet_data_length == 10: # 参数请求
print('参数请求包')
data_bytes = struct.pack('<Hdfffffffff', usv.control.pid['id'], time.time(),
usv.control.pid['heading_p'], usv.control.pid['heading_i'],
usv.control.pid['heading_d'], usv.control.pid['speed_p'],
@ -83,7 +79,6 @@ class GroundControlStation:
continue
elif packet_id == 3 and packet_data_length == 46: # 设置请求
print('PID设置包')
# 解析PID参数
pid = struct.unpack('<fffffffff', bytes(packet_data[10:packet_data_length]))
# 保存PID
@ -106,7 +101,6 @@ class GroundControlStation:
continue
elif packet_id == 4:
print('路点包')
data_type = packet_data[10]
if packet_data_length == 11 and data_type == 0:
pass
@ -124,7 +118,6 @@ class GroundControlStation:
continue
def send_status(self, usv):
# print('发送状态包')
data_bytes = struct.pack('<HdBdddffffffffffffHHffhhb', usv.control.pid['id'], time.time(), usv.control.status,
usv.navigation.data['location']['latitude'],
usv.navigation.data['location']['longitude'],

17
usv.py
View File

@ -18,16 +18,27 @@ class UsvControl:
self.control = Control()
self.gcs = GroundControlStation(self.settings.gcs_com)
def main_run(self):
def ms10_run(self):
self.futaba.rcu_run(self)
self.control.c_run(self)
self.navigation.n_run()
self.gcs.g_run(self)
timer_10 = threading.Timer(0.01, self.main_run, )
timer_10 = threading.Timer(0.01, self.ms10_run, )
timer_10.start()
def ms1000_run(self):
print(self.futaba.receive_data)
print(self.navigation.data)
print(self.control.data)
print(self.control.pid)
print(self.gcs.heart_beat)
print(self.gcs.command)
timer_1000 = threading.Timer(1, self.ms1000_run, )
timer_1000.start()
# 按间距中的绿色按钮以运行脚本。
if __name__ == '__main__':
usv1 = UsvControl()
usv1.main_run()
usv1.ms10_run()
usv1.ms1000_run()