mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
px4_uploader: Prevent spin-lock from hogging CPU
This commit is contained in:
@@ -501,75 +501,84 @@ print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..."
|
|||||||
print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.")
|
print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.")
|
||||||
|
|
||||||
# Spin waiting for a device to show up
|
# Spin waiting for a device to show up
|
||||||
while True:
|
try:
|
||||||
portlist = []
|
while True:
|
||||||
patterns = args.port.split(",")
|
portlist = []
|
||||||
# on unix-like platforms use glob to support wildcard ports. This allows
|
patterns = args.port.split(",")
|
||||||
# the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from
|
# on unix-like platforms use glob to support wildcard ports. This allows
|
||||||
# causing modem hangups etc
|
# the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from
|
||||||
if "linux" in _platform or "darwin" in _platform:
|
# causing modem hangups etc
|
||||||
import glob
|
if "linux" in _platform or "darwin" in _platform:
|
||||||
for pattern in patterns:
|
import glob
|
||||||
portlist += glob.glob(pattern)
|
for pattern in patterns:
|
||||||
else:
|
portlist += glob.glob(pattern)
|
||||||
portlist = patterns
|
else:
|
||||||
|
portlist = patterns
|
||||||
|
|
||||||
for port in portlist:
|
for port in portlist:
|
||||||
|
|
||||||
#print("Trying %s" % port)
|
#print("Trying %s" % port)
|
||||||
|
|
||||||
# create an uploader attached to the port
|
# create an uploader attached to the port
|
||||||
try:
|
try:
|
||||||
if "linux" in _platform:
|
if "linux" in _platform:
|
||||||
# Linux, don't open Mac OS and Win ports
|
# Linux, don't open Mac OS and Win ports
|
||||||
if not "COM" in port and not "tty.usb" in port:
|
if not "COM" in port and not "tty.usb" in port:
|
||||||
up = uploader(port, args.baud)
|
up = uploader(port, args.baud)
|
||||||
elif "darwin" in _platform:
|
elif "darwin" in _platform:
|
||||||
# OS X, don't open Windows and Linux ports
|
# OS X, don't open Windows and Linux ports
|
||||||
if not "COM" in port and not "ACM" in port:
|
if not "COM" in port and not "ACM" in port:
|
||||||
up = uploader(port, args.baud)
|
up = uploader(port, args.baud)
|
||||||
elif "win" in _platform:
|
elif "win" in _platform:
|
||||||
# Windows, don't open POSIX ports
|
# Windows, don't open POSIX ports
|
||||||
if not "/" in port:
|
if not "/" in port:
|
||||||
up = uploader(port, args.baud)
|
up = uploader(port, args.baud)
|
||||||
except Exception:
|
except Exception:
|
||||||
# open failed, rate-limit our attempts
|
# open failed, rate-limit our attempts
|
||||||
time.sleep(0.05)
|
time.sleep(0.05)
|
||||||
|
|
||||||
# and loop to the next port
|
# and loop to the next port
|
||||||
continue
|
continue
|
||||||
|
|
||||||
# port is open, try talking to it
|
# port is open, try talking to it
|
||||||
try:
|
try:
|
||||||
# identify the bootloader
|
# identify the bootloader
|
||||||
up.identify()
|
up.identify()
|
||||||
print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
|
print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
|
||||||
|
|
||||||
except Exception:
|
except Exception:
|
||||||
# most probably a timeout talking to the port, no bootloader, try to reboot the board
|
# most probably a timeout talking to the port, no bootloader, try to reboot the board
|
||||||
print("attempting reboot on %s..." % port)
|
print("attempting reboot on %s..." % port)
|
||||||
print("if the board does not respond, unplug and re-plug the USB connector.")
|
print("if the board does not respond, unplug and re-plug the USB connector.")
|
||||||
up.send_reboot()
|
up.send_reboot()
|
||||||
|
|
||||||
# wait for the reboot, without we might run into Serial I/O Error 5
|
# wait for the reboot, without we might run into Serial I/O Error 5
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
# always close the port
|
# always close the port
|
||||||
up.close()
|
up.close()
|
||||||
continue
|
continue
|
||||||
|
|
||||||
try:
|
try:
|
||||||
# ok, we have a bootloader, try flashing it
|
# ok, we have a bootloader, try flashing it
|
||||||
up.upload(fw)
|
up.upload(fw)
|
||||||
|
|
||||||
except RuntimeError as ex:
|
except RuntimeError as ex:
|
||||||
|
|
||||||
# print the error
|
# print the error
|
||||||
print("\nERROR: %s" % ex.args)
|
print("\nERROR: %s" % ex.args)
|
||||||
|
|
||||||
finally:
|
finally:
|
||||||
# always close the port
|
# always close the port
|
||||||
up.close()
|
up.close()
|
||||||
|
|
||||||
# we could loop here if we wanted to wait for more boards...
|
# we could loop here if we wanted to wait for more boards...
|
||||||
sys.exit(0)
|
sys.exit(0)
|
||||||
|
|
||||||
|
#Rate-limit retries to prevent spin-lock from hogging the CPU
|
||||||
|
time.sleep(0.5)
|
||||||
|
|
||||||
|
#CTRL+C aborts the upload/spin-lock by interrupt mechanics
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\n Upload aborted by user.")
|
||||||
|
sys.exit(0)
|
||||||
Reference in New Issue
Block a user