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 accel_calibrate_run(conn):
'''run accelcal'''
logger.info("STARTING ACCEL CALIBRATION")
wait_gyros(conn)
time.sleep(2)
if ETE == 0:
logger.debug("re-running gyro cal")
conn.discard_messages()
conn.ref.send('gyrocal\n')
conn.test.send('gyrocal\n')
conn.ref.expect('Calibrated')
conn.test.expect('Calibrated')
wait_gyros(conn)
rotate.set_rotation(conn, 'level', wait=False)
logger.info("Turning safety off")
util.safety_off(conn.refmav)
logger.info("Turning on testjig mode")
conn.test.send("factory_test start\n")
# use zero trims on reference board
util.param_set(conn.ref, 'AHRS_TRIM_X', 0)
util.param_set(conn.ref, 'AHRS_TRIM_Y', 0)
level_attitude = None
conn.test.send("accelcal\n")
for rotation in ['level', 'left', 'right', 'up', 'down', 'back']:
try:
conn.test.expect("Place vehicle")
conn.test.expect("and press any key")
except Exception as ex:
util.show_tail(conn.testlog)
util.failure("Failed to get place vehicle message for %s" %
rotation)
attitude = rotate.set_rotation(conn, rotation)
if rotation == 'level':
level_attitude = attitude
conn.test.send("\n")
i = conn.test.expect(["Calibration successful", "Calibration FAILED"])
if i != 0:
logger.error(conn.test.before)
logger.error("Calibration FAILED")
util.show_tail(conn.testlog)
util.failure("Accel calibration failed")
#logger.info(conn.test.before)
logger.info("Calibration successful")
rotate.write_calibration()
if ETE == 0:
rotate.set_rotation(conn, 'level', wait=False)
# get AHRS_TRIM_{X,Y} right
adjust_ahrs_trim(conn, level_attitude)
# rotate while integrating gyros to test all gyros are working
rotate.gyro_integrate(conn)
# finish in level position
rotate.set_rotation(conn, 'level')
def accel_calibrate_run(conn):
'''run accelcal'''
logger.info("STARTING ACCEL CALIBRATION")
wait_gyros(conn)
time.sleep(2)
if ETE == 0:
logger.debug("re-running gyro cal")
conn.discard_messages()
conn.ref.send('gyrocal\n')
conn.test.send('gyrocal\n')
conn.ref.expect('Calibrated')
conn.test.expect('Calibrated')
wait_gyros(conn)
rotate.set_rotation(conn, 'level', wait=False)
logger.info("Turning safety off")
util.safety_off(conn.refmav)
logger.info("Turning on testjig mode")
conn.test.send("factory_test start\n")
# use zero trims on reference board
util.param_set(conn.ref, 'AHRS_TRIM_X', 0)
util.param_set(conn.ref, 'AHRS_TRIM_Y', 0)
level_attitude = None
conn.test.send("accelcal\n")
for rotation in ['level', 'left', 'right', 'up', 'down', 'back']:
try:
conn.test.expect("Place vehicle")
conn.test.expect("and press any key")
except Exception as ex:
util.show_tail(conn.testlog)
util.failure("Failed to get place vehicle message for %s" % rotation)
attitude = rotate.set_rotation(conn, rotation)
if rotation == 'level':
level_attitude = attitude
conn.test.send("\n")
i = conn.test.expect(["Calibration successful","Calibration FAILED"])
if i != 0:
logger.error(conn.test.before)
logger.error("Calibration FAILED")
util.show_tail(conn.testlog)
util.failure("Accel calibration failed")
#logger.info(conn.test.before)
logger.info("Calibration successful")
rotate.write_calibration()
if ETE == 0:
rotate.set_rotation(conn, 'level', wait=False)
# get AHRS_TRIM_{X,Y} right
adjust_ahrs_trim(conn, level_attitude)
# rotate while integrating gyros to test all gyros are working
rotate.gyro_integrate(conn)
# finish in level position
rotate.set_rotation(conn, 'level')
def accel_calibrate_run(conn):
"""run accelcal"""
logger.info("STARTING ACCEL CALIBRATION")
wait_gyros(conn)
time.sleep(2)
logger.debug("re-running gyro cal")
conn.discard_messages()
conn.ref.send("gyrocal\n")
conn.test.send("gyrocal\n")
conn.ref.expect("Calibrated")
conn.test.expect("Calibrated")
wait_gyros(conn)
logger.info("Turning safety off")
rotate.set_rotation(conn, "level", wait=False)
util.safety_off(conn.refmav)
# use zero trims on reference board
util.param_set(conn.ref, "AHRS_TRIM_X", 0)
util.param_set(conn.ref, "AHRS_TRIM_Y", 0)
level_attitude = None
conn.test.send("accelcal\n")
for rotation in ["level", "left", "right", "up", "down", "back"]:
try:
conn.test.expect("Place vehicle")
conn.test.expect("and press any key")
except Exception as ex:
util.show_tail(conn.testlog)
util.failure("Failed to get place vehicle message for %s" % rotation)
attitude = rotate.set_rotation(conn, rotation)
if rotation == "level":
level_attitude = attitude
conn.test.send("\n")
i = conn.test.expect(["Calibration successful", "Calibration FAILED"])
if i != 0:
logger.error(conn.test.before)
logger.error("Calibration FAILED")
util.show_tail(conn.testlog)
util.failure("Accel calibration failed")
# logger.info(conn.test.before)
logger.info("Calibration successful")
rotate.write_calibration()
rotate.set_rotation(conn, "level", wait=False)
# rotate while integrating gyros to test all gyros are working
rotate.gyro_integrate(conn)
# get AHRS_TRIM_{X,Y} right
adjust_ahrs_trim(conn, level_attitude)
# finish in level position
rotate.set_rotation(conn, "level", quick=True)
def accel_calibrate_run(conn):
'''run accelcal'''
logger.info("STARTING ACCEL CALIBRATION")
wait_gyros(conn)
logger.debug("running ref gyro cal")
conn.ref.send('gyrocal\n')
conn.ref.expect('Calibrated')
wait_gyros(conn)
logger.info("Turning safety off")
rotate.set_rotation(conn, 'level', wait=False)
util.safety_off(conn.refmav)
# use zero trims on reference board
util.param_set(conn.ref, 'AHRS_TRIM_X', 0)
util.param_set(conn.ref, 'AHRS_TRIM_Y', 0)
level_attitude = None
conn.test.send("accelcal\n")
for rotation in ['level', 'left', 'right', 'up', 'down', 'back']:
try:
conn.test.expect("Place vehicle")
conn.test.expect("and press any key")
except Exception as ex:
util.show_tail(conn.testlog)
util.failure("Failed to get place vehicle message for %s" % rotation)
attitude = rotate.set_rotation(conn, rotation)
if rotation == 'level':
level_attitude = attitude
conn.test.send("\n")
i = conn.test.expect(["Calibration successful","Calibration FAILED"])
if i != 0:
logger.error(conn.test.before)
logger.error("Calibration FAILED")
util.show_tail(conn.testlog)
util.failure("Accel calibration failed at %s" % time.ctime())
#logger.info(conn.test.before)
logger.info("Calibration successful")
rotate.write_calibration()
rotate.set_rotation(conn, 'level', wait=False)
adjust_ahrs_trim(conn, level_attitude)
parser.add_argument("--tolerance", type=float, dest="tolerance", default=ROTATION_LEVEL_TOLERANCE,
help="rotation tolerance")
parser.add_argument("--wait", dest="wait", action='store_true', default=False,
help="wait for completion")
parser.add_argument("--yaw-zero", type=int, default=ROTATIONS['level'].chan1,
help="zero on yaw channel")
parser.add_argument("--unjam", action='store_true', help="unjam servos")
parser.add_argument("--calibrate", action='store_true', help="calibrate servos")
parser.add_argument("rotation", default="level", help="target rotation")
args = parser.parse_args()
ROTATION_LEVEL_TOLERANCE = args.tolerance
ROTATION_TOLERANCE = args.tolerance
ROTATIONS['level'].chan1 = args.yaw_zero
# power_control.power_cycle()
conn = connection.Connection(ref_only=True)
print("Turning safety off")
util.safety_off(conn.refmav)
if args.unjam:
unjam_servos(conn)
if args.calibrate:
calibrate_servos(conn)
print("Rotating to %s" % args.rotation)
set_rotation(conn, args.rotation, wait=args.wait)
parser.add_argument("--save", action='store_true', help="save on success")
parser.add_argument("--timeout", type=int, default=25, help="timeout in seconds")
parser.add_argument("--output", default=None, help="output mavlink to given address")
parser.add_argument("rotation", default="level", nargs='?', help="target rotation")
args = parser.parse_args()
ROTATION_LEVEL_TOLERANCE = args.tolerance
ROTATION_TOLERANCE = args.tolerance
ROTATIONS['level'].chan1 = args.yaw_zero
# power_control.power_cycle()
conn = connection.Connection(ref_only=True)
print("Turning safety off")
util.safety_off(conn.refmav)
if args.output:
conn.ref.send('output add %s\n' % args.output)
if args.unjam:
unjam_servos(conn)
if args.calibrate:
calibrate_servos(conn)
center_servos(conn)
sys.exit(0)
if args.calscale:
calscale_servos(conn)
center_servos(conn)
