aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autotest: added some debug to last_letter startup

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-21 21:17:38 +10:00
parent cd66ce1c74
commit a661187ac0
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Tools/autotest/ROS/runsim.py
View File

@ -82,7 +82,7 @@ def process_sitl_input(buf):
if e.errno not in [ errno.ECONNREFUSED ]: if e.errno not in [ errno.ECONNREFUSED ]:
raise raise
def process_ros_input(buf,frame_count): def process_ros_input(buf,frame_count, timestamp):
'''process FG FDM input from ROS Simulator''' '''process FG FDM input from ROS Simulator'''
global fdm, fg_out, sim_out global fdm, fg_out, sim_out
FG_FDM_FPS = 30 FG_FDM_FPS = 30
@ -98,7 +98,9 @@ def process_ros_input(buf,frame_count):
if e.errno not in [ errno.ECONNREFUSED ]: if e.errno not in [ errno.ECONNREFUSED ]:
raise raise
simbuf = struct.pack('<17dI', print('timestamp=', timestamp)
simbuf = struct.pack('<Q17dI',
timestamp,
fdm.get('latitude', units='degrees'), fdm.get('latitude', units='degrees'),
fdm.get('longitude', units='degrees'), fdm.get('longitude', units='degrees'),
fdm.get('altitude', units='meters'), fdm.get('altitude', units='meters'),
@ -208,6 +210,7 @@ def main_loop():
last_wind_update = tnow last_wind_update = tnow
frame_count = 0 frame_count = 0
paused = False paused = False
timestamp = 0
while True: while True:
rin = [ros_in.fileno(), sim_in.fileno()] rin = [ros_in.fileno(), sim_in.fileno()]
@ -222,11 +225,13 @@ def main_loop():
if ros_in.fileno() in rin: if ros_in.fileno() in rin:
buf = ros_in.recv(fdm.packet_size()) buf = ros_in.recv(fdm.packet_size())
process_ros_input(buf,frame_count) process_ros_input(buf,frame_count, timestamp)
frame_count += 1 frame_count += 1
timestamp += 1000
if sim_in.fileno() in rin: if sim_in.fileno() in rin:
simbuf = sim_in.recv(28) simbuf = sim_in.recv(28)
print(len(simbuf))
process_sitl_input(simbuf) process_sitl_input(simbuf)
last_sim_input = tnow last_sim_input = tnow