def accel_calibrate_retries(retries=4): '''run full accel calibration with retries return True on success, False on failure ''' while retries > 0: retries -= 1 if not util.wait_devices([USB_DEV_TEST, USB_DEV_REFERENCE]): logger.error("FAILED to find USB test and reference devices") power_control.power_cycle(down_time=4) continue try: time.sleep(2) accel_calibrate() except Exception as ex: logger.error("accel cal failed: %s" % ex) if retries > 0: logger.info("RETRYING ACCEL CAL") power_control.power_cycle(down_time=4) continue logger.info("PASSED ACCEL CAL") return True logger.error("accelcal: no more retries") return False
def accel_calibrate_retries(retries=4): '''run full accel calibration with retries return True on success, False on failure ''' while retries > 0: retries -= 1 if not util.wait_devices([USB_DEV_TEST, USB_DEV_REFERENCE]): logger.error("FAILED to find USB test and reference devices") power_control.power_cycle(down_time=4) continue try: time.sleep(2) accel_calibrate() except Exception as ex: logger.error("accel cal failed: %s" % ex) if retries > 0: logger.info("RETRYING ACCEL CAL") #sys.exit(1) power_control.power_cycle(down_time=4) continue logger.info("PASSED ACCEL CAL") return True logger.error("accelcal: no more retries") return False
def load_all_firmwares(retries=3): '''load 4 firmwares. Return True on success, False on failure''' while retries > 0: retries -= 1 if not util.wait_devices([IO_JTAG, FMU_JTAG, FMU_DEBUG]): if retries == 1: logger.info("RETRIES=1 - POWER CYCLING") power_control.power_cycle(down_time=4) continue try: load_firmware(IO_JTAG, FW_IO, CPUID_IO) load_firmware(IO_JTAG, BL_IO, CPUID_IO) load_firmware(FMU_JTAG, BL_FMU, CPUID_FMU) load_firmware(FMU_JTAG, FW_FMU, CPUID_FMU) except Exception as ex: logger.error("error loading firmwares %s" % ex) continue # power cycle after loading to ensure the boards can come up cleanly power_retries = 6 while power_retries > 0: power_retries -= 1 power_control.power_cycle(down_time=4) if util.wait_devices([FMU_DEBUG], timeout=5): break logger.info("Retrying power cycle - tries left %u" % power_retries) if not util.wait_devices([FMU_DEBUG], timeout=10): logger.info("Failed to find nsh console device") continue logger.debug("Checking nsh console") nsh = nsh_console.nsh_console() failure = None i = -1 try: i = nsh.expect([ 'No MPU6000 external', 'l3gd20: driver start failed', 'Error in startup', 'ArduPilot started OK', 'format failed', 'Opening USB nsh', 'no RGB led', 'rgbled: init failed' ]) except Exception as ex: failure = "******* Failed to get data from NSH console *******" pass try: nsh.send("\nver all\n") nsh.expect("UID:") nsh.expect("nsh>") nsh.close() except Exception as ex: if i == 3: failure = "******* failed to get version from nsh *******" pass if failure is None: if i == 0: failure = "******* No external mpu6000 found - is IMU board connected? *****" if i == 1: failure = "******* No external l3gd20 found - is IMU board connected? *****" if i == 2: failure = "******* Failed to startup ArduPilot - is IMU board connected? *****" if i == 4: failure = "******* microSD card failure ********" if i == 5: failure = "****** ArduPilot failed to start - general failure ******" if i == 6: failure = "****** RGB LED not found on I2C ******" if i == 7: failure = "****** RGB LED initialisation failed on I2C ******" if failure is not None: logger.info(failure) colour_text.print_fail(failure) continue if util.wait_devices([USB_DEV_TEST, USB_DEV_REFERENCE]): break logger.info("Failed to find USB devices") if retries > 0: logger.info("RETRIES %u - TRYING AGAIN" % retries) if retries == 0: logger.error("FAILED TO LOAD FIRMWARES") return False logger.info("All firmwares loaded OK") return True
import jtag import power_control import time import util import sys, os, fcntl import logger import colour_text import connection import rotate import barcode import savedstate import otp_program_mod from pymavlink import mavutil logger.info("Setting Pixhawk into Factory Mode %s" % time.ctime()) power_control.power_cycle(down_time=4) # disable stdout buffering sys.stdout = os.fdopen(sys.stdout.fileno(), 'w', 0) # wait for the power to come on again while not util.wait_devices([FMU_JTAG, IO_JTAG, FMU_DEBUG]): logger.info("waiting for power up....") util.kill_processes(['mavproxy.py', GDB]) time.sleep(2) while not util.wait_devices([USB_DEV_TEST, USB_DEV_REFERENCE]): logger.error("FAILED to find USB test and reference devices") power_control.power_cycle(down_time=4) conn = connection.Connection()
# load the jig state file savedstate.init() savedstate.reset('current_cycles') while True: logger.get_ftdi() jigstate = savedstate.get() logger.info("jigstate: total_cycles = %i" % jigstate['total_cycles']) logger.info("jigstate: current_cycles = %i" % jigstate['current_cycles']) util.kill_processes(['mavproxy.py', GDB]) if args.test: # power cycle each time, simulating new board put in power_control.power_cycle() else: # wait for the power to be switched off, disable serial logging logger.info("waiting for power off") util.wait_no_device([FMU_JTAG, IO_JTAG], timeout=600) device_barcode = args.barcode if not args.test and device_barcode is None: colour_text.print_blue(''' ========================================== | PLEASE SWIPE DEVICE BARCODE ========================================== ''') device_barcode = barcode.barcode_read() if device_barcode is None: colour_text.print_fail('''
def load_all_firmwares(retries=3): '''load 4 firmwares. Return True on success, False on failure''' while retries > 0: retries -= 1 if not util.wait_devices([IO_JTAG, FMU_JTAG, FMU_DEBUG]): if retries == 1: logger.info("RETRIES=1 - POWER CYCLING") power_control.power_cycle(down_time=4) continue try: load_firmware(IO_JTAG, FW_IO, CPUID_IO) load_firmware(IO_JTAG, BL_IO, CPUID_IO) load_firmware(FMU_JTAG, BL_FMU, CPUID_FMU) load_firmware(FMU_JTAG, FW_FMU, CPUID_FMU) except Exception as ex: logger.error("error loading firmwares %s" % ex) continue # power cycle after loading to ensure the boards can come up cleanly power_retries = 6 while power_retries > 0: power_retries -= 1 power_control.power_cycle(down_time=4) if util.wait_devices([FMU_DEBUG], timeout=5): break logger.info("Retrying power cycle - tries left %u" % power_retries) if not util.wait_devices([FMU_DEBUG], timeout=10): logger.info("Failed to find nsh console device") continue logger.debug("Checking nsh console") nsh = nsh_console.nsh_console() failure = None i = -1 try: i = nsh.expect(['No MPU6000 external', 'l3gd20: driver start failed', 'Error in startup', 'ArduPilot started OK', 'format failed', 'Opening USB nsh', 'no RGB led', 'rgbled: init failed']) except Exception as ex: failure = "******* Failed to get data from NSH console *******" pass try: nsh.send("\nver all\n") nsh.expect("UID:") nsh.expect("nsh>") nsh.close() except Exception as ex: if i == 3: failure = "******* failed to get version from nsh *******" pass if failure is None: if i == 0: failure = "******* No external mpu6000 found - is IMU board connected? *****" if i == 1: failure = "******* No external l3gd20 found - is IMU board connected? *****" if i == 2: failure = "******* Failed to startup ArduPilot - is IMU board connected? *****" if i == 4: failure = "******* microSD card failure ********" if i == 5: failure = "****** ArduPilot failed to start - general failure ******" if i == 6: failure = "****** RGB LED not found on I2C ******" if i == 7: failure = "****** RGB LED initialisation failed on I2C ******" if failure is not None: logger.info(failure) colour_text.print_fail(failure) continue if util.wait_devices([USB_DEV_TEST, USB_DEV_REFERENCE]): break logger.info("Failed to find USB devices") if retries > 0: logger.info("RETRIES %u - TRYING AGAIN" % retries) if retries == 0: logger.error("FAILED TO LOAD FIRMWARES") return False logger.info("All firmwares loaded OK") return True
while True: logger.get_ftdi() jigstate = savedstate.get() logger.info("jigstate: total_cycles = %i" % jigstate['total_cycles']) logger.info("jigstate: current_cycles = %i" % jigstate['current_cycles']) # Set ETE speeds if ETE == 1: ete = PixETE() ete.yawspeed(5000) ete.rollspeed(10000) util.kill_processes(['mavproxy.py', GDB]) if args.test: # power cycle each time, simulating new board put in power_control.power_cycle() else: # wait for the power to be switched off, disable serial logging logger.info("waiting for power off") util.wait_no_device([FMU_JTAG, IO_JTAG], timeout=600) device_barcode = args.barcode if not args.test and device_barcode is None: ete.command_bytes('test_wait') colour_text.print_blue(''' ========================================== | PLEASE SWIPE DEVICE BARCODE ========================================== ''') device_barcode = barcode.barcode_read() if device_barcode is None: