def set_rotation(conn, rotation, wait=True, timeout=25):
'''set servo rotation'''
if not rotation in ROTATIONS:
util.failure("No rotation %s" % rotation)
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
logger.info("set_rotation: call set_servo -- YAW rotation[%s].chan1=%s PITCH rotation[%s].chan2=%s" % (rotation, ROTATIONS[rotation].chan1, rotation, ROTATIONS[rotation].chan2) )
# start with initial settings from the table
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS[rotation].chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS[rotation].chan2)
if not wait:
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
time.sleep(1)
util.discard_messages(conn.refmav)
if expected_roll == 0 and expected_pitch == 0:
tolerance = ROTATION_LEVEL_TOLERANCE
else:
tolerance = ROTATION_TOLERANCE
# now optimise it
if not optimise_attitude(conn, rotation, tolerance, timeout=timeout):
util.failure("Failed to reach target attitude")
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
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_quiescent(mav, type='RAW_IMU', quick=False):
'''wait for movement to stop'''
t1 = time.time()
util.discard_messages(mav)
raw_imu = None
if quick:
timeout = 7
else:
timeout = 15
while time.time() < t1+timeout:
raw_imu = mav.recv_match(type=type, blocking=True, timeout=4) # JQM
# logger.debug("mav.recv_match: type=%s x=%s y=%s z=%s" % (type, raw_imu.xgyro, raw_imu.ygyro, raw_imu.zgyro))
if raw_imu is None:
util.failure("communication with board lost for %s" % type)
if time.time() > t1+10:
logger.debug("Time > 10 -- Not quiescent: x=%.2f y=%.2f z=%.2f" % (abs(degrees(raw_imu.xgyro*0.001)), abs(degrees(raw_imu.ygyro*0.001)), abs(degrees(raw_imu.zgyro*0.001))) )
if (abs(degrees(raw_imu.xgyro*0.001)) < GYRO_TOLERANCE and
abs(degrees(raw_imu.ygyro*0.001)) < GYRO_TOLERANCE and
abs(degrees(raw_imu.zgyro*0.001)) < GYRO_TOLERANCE):
break
if raw_imu is None and not quick:
util.failure("Failed to reach quiescent state")
attitude = mav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
if attitude is None:
util.failure("Failed to receive ATTITUDE message")
return attitude
def set_rotation(conn, rotation, wait=True, timeout=25, quick=False):
'''set servo rotation'''
if not rotation in ROTATIONS:
util.failure("No rotation %s" % rotation)
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
logger.info("set_rotation: %s chan1=%u chan2=%u" % (rotation, ROTATIONS[rotation].chan1, ROTATIONS[rotation].chan2) )
# start with initial settings from the table
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS[rotation].chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS[rotation].chan2)
if not wait:
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
time.sleep(1)
util.discard_messages(conn.refmav)
if expected_roll == 0 and expected_pitch == 0:
tolerance = ROTATION_LEVEL_TOLERANCE
else:
tolerance = ROTATION_TOLERANCE
# now optimise it
if not optimise_attitude(conn, rotation, tolerance, timeout=timeout, quick=quick):
util.failure("Failed to reach target attitude")
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
def check_serial_pair(testmav, port1, port2):
'''check a pair of loopback serial ports'''
# lock both ports and flush data
flags = mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE | mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI
# drain the serial ports
testmav.mav.serial_control_send(port1, flags, 100, 57600, 0, util.serial_control_buf(""))
testmav.mav.serial_control_send(port2, flags, 100, 57600, 0, util.serial_control_buf(""))
util.discard_messages(testmav)
testmav.mav.serial_control_send(port1, flags, 10, 0, 5, util.serial_control_buf("TEST1"))
testmav.mav.serial_control_send(port2, flags, 10, 0, 5, util.serial_control_buf("TEST2"))
testmav.mav.serial_control_send(port1, flags, 10, 0, 5, util.serial_control_buf("TEST1"))
testmav.mav.serial_control_send(port2, flags, 10, 0, 5, util.serial_control_buf("TEST2"))
start_time = time.time()
port1_ok = False
port2_ok = False
while time.time() < start_time+5 and (not port1_ok or not port2_ok):
reply = testmav.recv_match(type='SERIAL_CONTROL', blocking=True, timeout=2)
if reply is None or reply.count == 0:
continue
str = serial_control_str(reply)
#logger.error("reply: %u %s" % (reply.device, str))
if reply.device == port1 and str == "TEST2":
port1_ok = True
if reply.device == port2 and str == "TEST1":
port2_ok = True
if not port1_ok:
util.failure("No reply on serial port %u" % port1)
if not port2_ok:
util.failure("No reply on serial port %u" % port2)
util.discard_messages(testmav)
def set_rotation(conn, rotation, wait=True, timeout=25):
'''set servo rotation'''
if not rotation in ROTATIONS:
util.failure("No rotation %s" % rotation)
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
logger.debug("set_rotation: call set_servo -- YAW rotation[%s].chan1=%s PITCH rotation[%s].chan2=%s" % (rotation, ROTATIONS[rotation].chan1, rotation, ROTATIONS[rotation].chan2) )
if ETE == 0:
# start with initial settings from the table
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS[rotation].chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS[rotation].chan2)
elif ETE == 1:
ete = PixETE()
ete.position(ROTATIONS_ETE[rotation].chan2, ROTATIONS_ETE[rotation].chan1)
if not wait:
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
time.sleep(2)
util.discard_messages(conn.refmav)
if expected_roll == 0 and expected_pitch == 0:
tolerance = ROTATION_LEVEL_TOLERANCE
else:
tolerance = ROTATION_TOLERANCE
# now optimise it
if not optimise_attitude(conn, rotation, tolerance, timeout=timeout):
util.failure("Failed to reach target attitude")
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
def erase_parameters():
'''erase parameters on test board'''
conn = Connection()
logger.info("Setting SYSID_SW_MREV to 0")
conn.test.send('param set SYSID_SW_MREV 0\n')
time.sleep(1)
logger.info("rebooting")
conn.test.send('reboot\n')
util.discard_messages(conn.testmav)
util.wait_mode(conn.testmav, IDLE_MODES, timeout=30)
logger.info("Parameters erased")
return True
def erase_parameters():
'''erase parameters on test board'''
conn = Connection()
logger.info("Setting SYSID_SW_MREV to 0")
conn.test.send('param set SYSID_SW_MREV 0\n')
time.sleep(1)
logger.info("rebooting")
conn.test.send('reboot\n')
util.discard_messages(conn.testmav)
util.wait_mode(conn.testmav, IDLE_MODES, timeout=30)
logger.info("Parameters erased")
return True
def discard_messages(self):
'''discard pending mavlink messages'''
if self.refmav:
util.discard_messages(self.refmav)
if self.testmav:
util.discard_messages(self.testmav)
if self.test:
try:
self.test.read_nonblocking(4096, 0)
except Exception:
pass
if self.ref:
try:
self.ref.read_nonblocking(4096, 0)
except Exception:
pass
def discard_messages(self):
'''discard pending mavlink messages'''
if self.refmav:
util.discard_messages(self.refmav)
if self.testmav:
util.discard_messages(self.testmav)
if self.test:
try:
self.test.read_nonblocking(4096, 0)
except Exception:
pass
if self.ref:
try:
self.ref.read_nonblocking(4096, 0)
except Exception:
pass
def check_pwm(conn):
'''check PWM output and RC input'''
logger.info("Checking PWM out and RC in")
# disable safety on test board
conn.test.send('arm safetyoff\n')
test_values = [1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800]
# quad remaps the first 4 to a different order
map_values = [1100, 1300, 1400, 1200, 1500, 1600, 1700, 1800]
# we need to send the MOTOR_TEST command multuple times to avoid a race
# condition in ArduCopter motor_test.pde
for repeat in range(10):
for i in range(1, 5):
conn.testmav.mav.command_long_send(
0, 0, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, 0, i, 1,
test_values[i - 1], 300, 0, 0, 0)
time.sleep(0.05)
for i in range(5, 9):
conn.test.send('servo set %u %u\n' % (i, test_values[i - 1]))
time.sleep(0.5)
util.discard_messages(conn.testmav)
rcin = conn.testmav.recv_match(type='RC_CHANNELS_RAW',
blocking=True,
timeout=3)
if rcin is None:
util.failure("Failed to get RC input")
servo = conn.testmav.recv_match(type='SERVO_OUTPUT_RAW',
blocking=True,
timeout=3)
if servo is None:
util.failure("Failed to get servo output")
pwm_ok = True
for i in range(8):
rc = getattr(rcin, 'chan{0}_raw'.format(i + 1))
servoval = getattr(servo, 'servo{0}_raw'.format(i + 1))
if abs(rc - map_values[i]) > 3:
pwm_ok = False
logger.debug("pwm_check[%u] rc=%u test=%u servo=%u" %
(i, rc, map_values[i], servoval))
if not pwm_ok:
util.failure("Incorrect PWM passthrough")
def check_serial_pair(testmav, port1, port2):
'''check a pair of loopback serial ports'''
# lock both ports and flush data
flags = mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE | mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI
# drain the serial ports
testmav.mav.serial_control_send(port1, flags, 100, 57600, 0,
util.serial_control_buf(""))
testmav.mav.serial_control_send(port2, flags, 100, 57600, 0,
util.serial_control_buf(""))
util.discard_messages(testmav)
testmav.mav.serial_control_send(port1, flags, 10, 0, 5,
util.serial_control_buf("TEST1"))
testmav.mav.serial_control_send(port2, flags, 10, 0, 5,
util.serial_control_buf("TEST2"))
testmav.mav.serial_control_send(port1, flags, 10, 0, 5,
util.serial_control_buf("TEST1"))
testmav.mav.serial_control_send(port2, flags, 10, 0, 5,
util.serial_control_buf("TEST2"))
start_time = time.time()
port1_ok = False
port2_ok = False
while time.time() < start_time + 5 and (not port1_ok or not port2_ok):
reply = testmav.recv_match(type='SERIAL_CONTROL',
blocking=True,
timeout=2)
if reply is None or reply.count == 0:
continue
str = serial_control_str(reply)
#logger.error("reply: %u %s" % (reply.device, str))
if reply.device == port1 and str == "TEST2":
port1_ok = True
if reply.device == port2 and str == "TEST1":
port2_ok = True
if not port1_ok:
util.failure("No reply on serial port %u" % port1)
if not port2_ok:
util.failure("No reply on serial port %u" % port2)
util.discard_messages(testmav)
def check_pwm(conn):
'''check PWM output and RC input'''
logger.info("Checking PWM out and RC in")
# disable safety on test board
conn.test.send('arm safetyoff\n')
test_values = [ 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800 ]
# quad remaps the first 4 to a different order
map_values = [ 1100, 1300, 1400, 1200, 1500, 1600, 1700, 1800 ]
# we need to send the MOTOR_TEST command multuple times to avoid a race
# condition in ArduCopter motor_test.pde
for repeat in range(10):
for i in range(1,5):
conn.testmav.mav.command_long_send(0, 0,
mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, 0,
i, 1, test_values[i-1], 300,
0, 0, 0)
time.sleep(0.05)
for i in range(5,9):
conn.test.send('servo set %u %u\n' % (i, test_values[i-1]))
time.sleep(0.5)
util.discard_messages(conn.testmav)
rcin = conn.testmav.recv_match(type='RC_CHANNELS_RAW', blocking=True, timeout=3)
if rcin is None:
util.failure("Failed to get RC input")
servo = conn.testmav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True, timeout=3)
if servo is None:
util.failure("Failed to get servo output")
pwm_ok = True
for i in range(8):
rc = getattr(rcin, 'chan{0}_raw'.format(i+1))
servoval = getattr(servo, 'servo{0}_raw'.format(i+1))
if abs(rc - map_values[i]) > 3:
pwm_ok = False
logger.debug("pwm_check[%u] rc=%u test=%u servo=%u" % (i, rc, map_values[i], servoval))
if not pwm_ok:
util.failure("Incorrect PWM passthrough")
def discard_messages(self):
'''discard pending mavlink messages'''
if self.refmav:
util.discard_messages(self.refmav)
if self.testmav:
util.discard_messages(self.testmav)