def accel_calibrate_reference():
'''run accelcal on reference board'''
logger.info("STARTING REFERENCE ACCEL CALIBRATION")
conn = connection.Connection(ref_only=True)
logger.info("Turning safety off")
rotate.set_rotation(conn, 'level', wait=False)
util.safety_off(conn.refmav)
conn.ref.send("accelcal\n")
for rotation in ['level', 'left', 'right', 'up', 'down', 'back']:
try:
conn.ref.expect("Place vehicle")
conn.ref.expect("and press any key")
except Exception as ex:
util.failure("Failed to get place vehicle message for %s" %
rotation)
logger.debug("Rotating %s" % rotation)
attitude = rotate.set_rotation(conn, rotation, wait=False)
time.sleep(13)
conn.ref.send("\n")
i = conn.ref.expect(["Calibration successful", "Calibration FAILED"])
if i != 0:
util.failure("Accel calibration failed at %s" % time.ctime())
logger.info("Calibration successful")
rotate.set_rotation(conn, 'level', wait=False)
util.param_set(conn.ref, 'AHRS_TRIM_X', 0)
util.param_set(conn.ref, 'AHRS_TRIM_Y', 0)
util.discard_messages(conn.refmav)
util.wait_heartbeat(conn.refmav)
def accel_calibrate_reference():
"""run accelcal on reference board"""
logger.info("STARTING REFERENCE ACCEL CALIBRATION")
conn = connection.Connection(ref_only=True)
logger.info("Turning safety off")
rotate.set_rotation(conn, "level", wait=False)
util.safety_off(conn.refmav)
conn.ref.send("accelcal\n")
for rotation in ["level", "left", "right", "up", "down", "back"]:
try:
conn.ref.expect("Place vehicle")
conn.ref.expect("and press any key")
except Exception as ex:
util.failure("Failed to get place vehicle message for %s" % rotation)
logger.debug("Rotating %s" % rotation)
attitude = rotate.set_rotation(conn, rotation, wait=False)
time.sleep(13)
conn.ref.send("\n")
i = conn.ref.expect(["Calibration successful", "Calibration FAILED"])
if i != 0:
util.failure("Accel calibration failed at %s" % time.ctime())
logger.info("Calibration successful")
rotate.set_rotation(conn, "level", wait=False)
util.param_set(conn.ref, "AHRS_TRIM_X", 0)
util.param_set(conn.ref, "AHRS_TRIM_Y", 0)
util.discard_messages(conn.refmav)
util.wait_heartbeat(conn.refmav)
def wait_gyros_healthy(conn):
'''wait for gyros to be healthy'''
util.wait_heartbeat(conn.testmav)
# we must have AHRS_ORIENTATION 0 for the accelcal
# we will fix for AHRS_ORIENTATION=12 later
util.param_set(conn.test, 'AHRS_ORIENTATION', 0)
util.param_set(conn.ref, 'AHRS_ORIENTATION', 0)
# disable mag to prevent it causing affects on the gyro drift
util.param_set(conn.ref, 'MAG_ENABLE', 0)
# setup right reference board parameters, in case someone has reset them
util.param_set(conn.ref, 'THR_FAILSAFE', 0)
util.param_set(conn.ref, 'RCMAP_ROLL', 5)
util.param_set(conn.ref, 'RCMAP_PITCH', 6)
# give time for 1Hz loop to set orientation
time.sleep(2)
logger.info("Waiting for gyro health")
start_time = time.time()
ref_gyros_healthy = False
test_gyros_healthy = False
conn.discard_messages()
while time.time() < start_time + 20 and (not ref_gyros_healthy
or not test_gyros_healthy):
ref_sys_status = conn.refmav.recv_match(type='SYS_STATUS',
blocking=True,
timeout=1)
if ref_sys_status:
ref_gyros_healthy = (
ref_sys_status.onboard_control_sensors_health
& mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO) != 0
test_sys_status = conn.testmav.recv_match(type='SYS_STATUS',
blocking=True,
timeout=1)
if test_sys_status:
test_gyros_healthy = (
test_sys_status.onboard_control_sensors_health
& mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO) != 0
if not ref_gyros_healthy:
logger.error("Failed to get healthy reference gyros")
return False
if not test_gyros_healthy:
logger.error("Failed to get healthy test gyros")
return False
logger.info("Gyros are healthy")
return True
def wait_gyros_healthy(conn):
"""wait for gyros to be healthy"""
util.wait_heartbeat(conn.testmav)
# we must have AHRS_ORIENTATION 0 for the accelcal
# we will fix for AHRS_ORIENTATION=12 later
util.param_set(conn.test, "AHRS_ORIENTATION", 0)
util.param_set(conn.ref, "AHRS_ORIENTATION", 0)
# disable mag to prevent it causing affects on the gyro drift
util.param_set(conn.ref, "MAG_ENABLE", 0)
# setup right reference board parameters, in case someone has reset them
util.param_set(conn.ref, "THR_FAILSAFE", 0)
util.param_set(conn.ref, "RCMAP_ROLL", 5)
util.param_set(conn.ref, "RCMAP_PITCH", 6)
# give time for 1Hz loop to set orientation
time.sleep(2)
logger.info("Waiting for gyro health")
start_time = time.time()
ref_gyros_healthy = False
test_gyros_healthy = False
conn.discard_messages()
while time.time() < start_time + 20 and (not ref_gyros_healthy or not test_gyros_healthy):
ref_sys_status = conn.refmav.recv_match(type="SYS_STATUS", blocking=True, timeout=1)
if ref_sys_status:
ref_gyros_healthy = (
ref_sys_status.onboard_control_sensors_health & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO
) != 0
test_sys_status = conn.testmav.recv_match(type="SYS_STATUS", blocking=True, timeout=1)
if test_sys_status:
test_gyros_healthy = (
test_sys_status.onboard_control_sensors_health & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO
) != 0
if not ref_gyros_healthy:
logger.error("Failed to get healthy reference gyros")
return False
if not test_gyros_healthy:
logger.error("Failed to get healthy test gyros")
return False
logger.info("Gyros are healthy")
return True
def __init__(self, ref_only=False):
util.kill_processes(['mavproxy.py', GDB])
self.reflog = StringIO()
self.testlog = StringIO()
self.ref = None
self.test = None
self.nsh = None
self.refmav = None
self.testmav = None
try:
if not ref_only:
self.nsh = nsh_console.nsh_console()
except Exception as ex:
self.close()
util.show_error('Connecting to nsh console', ex, self.testlog)
try:
self.ref = mav_reference.mav_reference(self.reflog)
except Exception as ex:
self.close()
util.show_error('Connecting to reference board1', ex, self.reflog)
try:
if not ref_only:
self.test = mav_test.mav_test(self.testlog)
except Exception as ex:
self.close()
util.show_error('Connecting to test board1', ex, self.testlog)
try:
logger.info("CONNECTING MAVLINK TO REFERENCE BOARD")
self.refmav = mavutil.mavlink_connection('127.0.0.1:14550')
util.wait_heartbeat(self.refmav, timeout=30)
util.wait_mode(self.refmav, IDLE_MODES)
except Exception as ex:
self.close()
util.show_error('Connecting to reference board2', ex, self.reflog)
try:
if not ref_only:
logger.info("CONNECTING MAVLINK TO TEST BOARD at %s" % time.ctime())
self.testmav = mavutil.mavlink_connection('127.0.0.1:14551')
util.wait_heartbeat(self.testmav, timeout=30)
logger.info("got heartbeat at %s" % time.ctime())
util.wait_mode(self.testmav, IDLE_MODES)
logger.info("Waiting for 'Ready to FLY'")
self.test.expect('Ready to FLY', timeout=20)
except Exception as ex:
self.close()
util.show_error('Connecting to test board2 at %s' % time.ctime(), ex, self.testlog)
if self.nsh is not None:
# log any extra nsh data
logger.debug("Draining nsh")
try:
self.nsh.read_nonblocking(4096, 1)
except Exception as ex:
pass
try:
if not ref_only and not ref_gyro_offset_ok(self.refmav):
self.close()
util.failure("Bad reference gyro - FAILED")
except Exception as ex:
self.close()
util.show_error('testing reference gyros', ex)
logger.info("Setting rotation level")
try:
rotate.set_rotation(self, 'level', wait=False)
except Exception as ex:
self.close()
util.show_error("unable to set safety off", ex)
def __init__(self, ref_only=False):
util.kill_processes(['mavproxy.py', GDB])
self.reflog = StringIO()
self.testlog = StringIO()
self.ref = None
self.test = None
self.nsh = None
self.refmav = None
self.testmav = None
try:
if not ref_only:
self.nsh = nsh_console.nsh_console()
except Exception as ex:
self.close()
util.show_error('Connecting to nsh console', ex, self.testlog)
try:
self.ref = mav_reference.mav_reference(self.reflog)
except Exception as ex:
self.close()
util.show_error('Connecting to reference board1', ex, self.reflog)
try:
if not ref_only:
self.test = mav_test.mav_test(self.testlog)
except Exception as ex:
self.close()
util.show_error('Connecting to test board1', ex, self.testlog)
try:
logger.info("CONNECTING MAVLINK TO REFERENCE BOARD")
self.refmav = mavutil.mavlink_connection('127.0.0.1:14550')
util.wait_heartbeat(self.refmav, timeout=30)
util.wait_mode(self.refmav, IDLE_MODES)
except Exception as ex:
self.close()
util.show_error('Connecting to reference board2', ex, self.reflog)
try:
if not ref_only:
logger.info("CONNECTING MAVLINK TO TEST BOARD")
self.testmav = mavutil.mavlink_connection('127.0.0.1:14551')
util.wait_heartbeat(self.testmav, timeout=30)
logger.info("got heartbeat")
util.wait_mode(self.testmav, IDLE_MODES)
logger.info("Waiting for 'Ready to FLY'")
self.fw_version = None
self.px4_version = None
self.nuttx_version = None
self.stm32_serial = None
ready = False
self.test.send("param fetch\n")
# log version information for later reference
while (self.fw_version is None or self.px4_version is None
or self.nuttx_version is None
or self.stm32_serial is None or not ready):
i = self.test.expect([
'APM: ([^\r\n]*Copter[^\r\n]*)\r\n',
'APM: PX4: ([0-9a-f]+) NuttX: ([0-9a-f]+)\r\n',
'APM: PX4v2 ([0-9A-F]+ [0-9A-F]+ [0-9A-F]+)\r\n',
'(Ready to FLY)'
],
timeout=20)
if i == 3:
ready = True
elif i == 0:
self.fw_version = self.test.match.group(1)
elif i == 1:
self.px4_version = self.test.match.group(1)
self.nuttx_version = self.test.match.group(2)
elif i == 2:
self.stm32_serial = self.test.match.group(1)
except Exception as ex:
self.close()
util.show_error('Connecting to test board2', ex, self.testlog)
if self.nsh is not None:
# log any extra nsh data
logger.debug("Draining nsh")
try:
self.nsh.read_nonblocking(4096, 1)
except Exception as ex:
pass
try:
if not ref_only and not ref_gyro_offset_ok(self.refmav):
self.close()
util.failure("Bad reference gyro - FAILED")
except Exception as ex:
self.close()
util.show_error('testing reference gyros', ex)
logger.info("Setting rotation level")
try:
rotate.set_rotation(self, 'level', wait=False)
except Exception as ex:
self.close()
util.show_error("unable to set safety off", ex)
def __init__(self, ref_only=False):
util.kill_processes(['mavproxy.py', GDB])
self.reflog = StringIO()
self.testlog = StringIO()
self.ref = None
self.test = None
self.nsh = None
self.refmav = None
self.testmav = None
try:
if not ref_only:
self.nsh = nsh_console.nsh_console()
except Exception as ex:
self.close()
util.show_error('Connecting to nsh console', ex, self.testlog)
try:
self.ref = mav_reference.mav_reference(self.reflog)
except Exception as ex:
self.close()
util.show_error('Connecting to reference board1', ex, self.reflog)
try:
if not ref_only:
self.test = mav_test.mav_test(self.testlog)
except Exception as ex:
self.close()
util.show_error('Connecting to test board1', ex, self.testlog)
try:
logger.info("CONNECTING MAVLINK TO REFERENCE BOARD")
self.refmav = mavutil.mavlink_connection('127.0.0.1:14550')
util.wait_heartbeat(self.refmav, timeout=30)
util.wait_mode(self.refmav, IDLE_MODES)
except Exception as ex:
self.close()
util.show_error('Connecting to reference board2', ex, self.reflog)
try:
if not ref_only:
logger.info("CONNECTING MAVLINK TO TEST BOARD")
self.testmav = mavutil.mavlink_connection('127.0.0.1:14551')
util.wait_heartbeat(self.testmav, timeout=30)
logger.info("got heartbeat")
util.wait_mode(self.testmav, IDLE_MODES)
logger.info("Waiting for 'Ready to FLY'")
self.fw_version = None
self.px4_version = None
self.nuttx_version = None
self.stm32_serial = None
ready = False
self.test.send("param fetch\n")
# log version information for later reference
while (self.fw_version is None or
self.px4_version is None or
self.nuttx_version is None or
self.stm32_serial is None or
not ready):
i = self.test.expect(['APM: ([^\r\n]*Copter[^\r\n]*)\r\n',
'APM: PX4: ([0-9a-f]+) NuttX: ([0-9a-f]+)\r\n',
'APM: PX4v2 ([0-9A-F]+ [0-9A-F]+ [0-9A-F]+)\r\n',
'(Ready to FLY)'],
timeout=20)
if i == 3:
ready = True
elif i == 0:
self.fw_version = self.test.match.group(1)
elif i == 1:
self.px4_version = self.test.match.group(1)
self.nuttx_version = self.test.match.group(2)
elif i == 2:
self.stm32_serial = self.test.match.group(1)
except Exception as ex:
self.close()
util.show_error('Connecting to test board2', ex, self.testlog)
if self.nsh is not None:
# log any extra nsh data
logger.debug("Draining nsh")
try:
self.nsh.read_nonblocking(4096, 1)
except Exception as ex:
pass
try:
if not ref_only and not ref_gyro_offset_ok(self.refmav):
self.close()
util.failure("Bad reference gyro - FAILED")
except Exception as ex:
self.close()
util.show_error('testing reference gyros', ex)
logger.info("Setting rotation level")
try:
rotate.set_rotation(self, 'level', wait=False)
except Exception as ex:
self.close()
util.show_error("unable to set safety off", ex)
