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 load_firmware(device, firmware, mcu_id, run=False):
'''load a given firmware'''
cmd = "%s %s" % (GDB, firmware)
logger.info("Loading firmware %s" % firmware)
log = StringIO()
try:
gdb = pexpect.spawn(cmd, logfile=log, timeout=10)
gdb.expect("Reading symbols from")
gdb.expect("done")
gdb.expect("(gdb)")
gdb.send("target extended %s\n" % device)
gdb.expect("Remote debugging using")
gdb.expect("(gdb)")
gdb.send("monitor swdp_scan\n")
ids = CPUID_IO + CPUID_FMU
cpu = gdb.expect(ids)
if ids[cpu] not in mcu_id:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (ids[cpu], mcu_id))
gdb.expect("(gdb)")
gdb.send("attach 1\n")
gdb.expect("(gdb)")
gdb.send("set mem inaccessible-by-default off\n")
gdb.expect("(gdb)")
gdb.send("load\n")
gdb.expect("Loading section .text", timeout=20)
gdb.expect("Loading section .data", timeout=30)
gdb.expect("Start address", timeout=10)
gdb.expect("Transfer rate", timeout=10)
gdb.expect("(gdb)")
if run:
gdb.send("run\n")
gdb.expect("Start it from the beginning?")
gdb.send("y\n")
except Exception as ex:
util.show_error('Loading firmware %s' % firmware, ex, log)
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 attach_gdb(device, mcu_id, firmware=None):
'''attach to gdb'''
cmd = GDB
if firmware is not None:
cmd += " " + firmware
gdb = pexpect.spawn(cmd, logfile=None, timeout=10)
gdb.expect("(gdb)")
gdb.send("target extended %s\n" % device)
gdb.expect("Remote debugging using")
gdb.expect("(gdb)")
gdb.send("monitor swdp_scan\n")
cpu = gdb.expect([CPUID_IO, CPUID_FMU])
if cpu == 0:
if mcu_id != CPUID_IO:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (CPUID_IO, mcu_id))
else:
if mcu_id != CPUID_FMU:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (CPUID_FMU, mcu_id))
gdb.expect("(gdb)")
gdb.send("attach 1\n")
gdb.expect("(gdb)")
gdb.send("set mem inaccessible-by-default off\n")
gdb.expect("(gdb)")
if firmware is not None:
logger.info("Use 'load' to load the firmware")
logger.info("Use 'quit' to quit")
gdb.send('\n')
gdb.interact()
def erase_firmware(device, mcu_id):
'''erase a firmware'''
cmd = GDB
logger.info("Erasing firmware for '%s'" % mcu_id)
try:
util.kill_processes([GDB])
gdb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
gdb.expect("(gdb)")
gdb.send("target extended %s\n" % device)
gdb.expect("Remote debugging using")
gdb.expect("(gdb)")
gdb.send("monitor swdp_scan\n")
ids = CPUID_IO + CPUID_FMU
cpu = gdb.expect(ids)
if ids[cpu] not in mcu_id:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (ids[cpu], mcu_id))
gdb.expect("(gdb)")
gdb.send("attach 1\n")
gdb.expect("(gdb)")
gdb.send("set mem inaccessible-by-default off\n")
gdb.expect("(gdb)")
gdb.send("monitor erase\n")
gdb.expect("(gdb)", timeout=20)
gdb.send('quit\n')
gdb.expect('Quit anyway')
gdb.send('y\n')
gdb.expect("Detached from remote")
gdb.close()
logger.info("closed")
except Exception as ex:
util.show_error('Erasing firmware', ex)
logger.info("Erase done")
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.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 check_accel_cal(conn):
'''check accel cal'''
global offset
offset = []
global scale_factor
scale_factor = []
for idx in range(NUM_ACCELS):
offset.append([])
scale_factor.append([])
if idx == 0:
n = ''
else:
n = '%u' % (idx + 1)
for axis in ['X', 'Y', 'Z']:
pname = 'INS_ACC%sOFFS_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
offset[idx].append(ofs)
if abs(ofs) < 1.0e-10:
util.failure(
"%s is zero - accel %u not calibrated (offset) %f" %
(pname, idx, ofs))
pname = 'INS_ACC%sSCAL_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
scale_factor[idx].append(ofs)
if abs(ofs - 1.0) < 1.0e-10:
util.failure("%s is zero - accel %u not calibrated (scale)" %
(pname, idx))
logger.info("Accel cal %u OK" % (idx + 1))
print "Accel Offsets:", offset
print "Accel Scale Factors:", scale_factor
def check_accel_cal(conn):
'''check accel cal'''
global offset
offset = []
global scale_factor
scale_factor = []
for idx in range(NUM_ACCELS):
offset.append([])
scale_factor.append([])
if idx == 0:
n = ''
else:
n = '%u' % (idx+1)
for axis in ['X', 'Y', 'Z']:
pname = 'INS_ACC%sOFFS_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
offset[idx].append(ofs)
if abs(ofs) < 1.0e-10:
util.failure("%s is zero - accel %u not calibrated (offset) %f" % (pname, idx, ofs))
pname = 'INS_ACC%sSCAL_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
scale_factor[idx].append(ofs)
if abs(ofs-1.0) < 1.0e-10:
util.failure("%s is zero - accel %u not calibrated (scale)" % (pname, idx))
logger.info("Accel cal %u OK" % (idx+1))
print "Accel Offsets:", offset
print "Accel Scale Factors:", scale_factor
def attach_gdb(device, mcu_id, firmware=None):
'''attach to gdb'''
cmd = GDB
if firmware is not None:
cmd += " " + firmware
gdb = pexpect.spawn(cmd, logfile=None, timeout=10)
gdb.expect("(gdb)")
gdb.send("target extended %s\n" % device)
gdb.expect("Remote debugging using")
gdb.expect("(gdb)")
gdb.send("monitor swdp_scan\n")
cpu = gdb.expect([CPUID_IO, CPUID_FMU])
if cpu == 0:
if mcu_id != CPUID_IO:
util.failure("Incorrect CPU ID '%s' - expected '%s'" %
(CPUID_IO, mcu_id))
else:
if mcu_id != CPUID_FMU:
util.failure("Incorrect CPU ID '%s' - expected '%s'" %
(CPUID_FMU, mcu_id))
gdb.expect("(gdb)")
gdb.send("attach 1\n")
gdb.expect("(gdb)")
gdb.send("set mem inaccessible-by-default off\n")
gdb.expect("(gdb)")
if firmware is not None:
logger.info("Use 'load' to load the firmware")
logger.info("Use 'quit' to quit")
gdb.send('\n')
gdb.interact()
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 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 erase_firmware(device, mcu_id):
'''erase a firmware'''
cmd = GDB
logger.info("Erasing firmware for '%s'" % mcu_id)
try:
util.kill_processes([GDB])
gdb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
gdb.expect("(gdb)")
gdb.send("target extended %s\n" % device)
gdb.expect("Remote debugging using")
gdb.expect("(gdb)")
gdb.send("monitor swdp_scan\n")
cpu = gdb.expect([CPUID_IO, CPUID_FMU])
if cpu == 0:
if mcu_id != CPUID_IO:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (CPUID_IO, mcu_id))
else:
if mcu_id != CPUID_FMU:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (CPUID_FMU, mcu_id))
gdb.expect("(gdb)")
gdb.send("attach 1\n")
gdb.expect("(gdb)")
gdb.send("set mem inaccessible-by-default off\n")
gdb.expect("(gdb)")
gdb.send("monitor erase\n")
gdb.expect("(gdb)", timeout=20)
gdb.send('quit\n')
gdb.expect('Quit anyway')
gdb.send('y\n')
gdb.expect("Detached from remote")
gdb.close()
logger.info("closed")
except Exception as ex:
util.show_error('Erasing firmware', ex)
logger.info("Erase done")
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 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 check_mag(conn):
'''check mags'''
magx = util.wait_field(conn.testmav, 'RAW_IMU', 'xmag')
magy = util.wait_field(conn.testmav, 'RAW_IMU', 'ymag')
magz = util.wait_field(conn.testmav, 'RAW_IMU', 'zmag')
field = sqrt(magx**2 + magy**2 + magz**2)
if field < 100 or field > 2000:
util.failure("Bad magnetic field (%u, %u, %u, %.1f)" % (magx, magy, magz, field))
logger.info("Magnetometer OK")
def get_ftdi():
global ftdi_device
if not ftdi_device:
ftdi_devices = glob.glob(FTDI_POWER)
if len(ftdi_devices) != 1:
util.failure("Must be exactly 1 FTDI device - %u found" % len(ftdi_devices))
ftdi_device = serial.Serial(ftdi_devices[0], rtscts=False)
power_control.on()
return ftdi_device
def check_mag(conn):
'''check mags'''
magx = util.wait_field(conn.testmav, 'RAW_IMU', 'xmag')
magy = util.wait_field(conn.testmav, 'RAW_IMU', 'ymag')
magz = util.wait_field(conn.testmav, 'RAW_IMU', 'zmag')
field = sqrt(magx**2 + magy**2 + magz**2)
if field < 100 or field > 2000:
util.failure("Bad magnetic field (%u, %u, %u, %.1f)" %
(magx, magy, magz, field))
logger.info("Magnetometer OK")
def get_ftdi():
global ftdi_device
if not ftdi_device:
ftdi_devices = glob.glob(FTDI_POWER)
if len(ftdi_devices) != 1:
util.failure("Must be exactly 1 FTDI device - %u found" %
len(ftdi_devices))
ftdi_device = serial.Serial(ftdi_devices[0], rtscts=False)
power_control.on()
return ftdi_device
def new_tlog(basename, extension="tlog"):
"""get a new tlog name in log directory"""
global current_logdir
if current_logdir is None:
return basename + "." + extension
for i in range(1, 1000000):
tlog = os.path.join(current_logdir, "%s-%u." % (basename, i)) + extension
if not os.path.exists(tlog):
return tlog
util.failure("Unable to create new tlog for %s" % basename)
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 new_tlog(basename, extension='tlog'):
'''get a new tlog name in log directory'''
global current_logdir
if current_logdir is None:
return basename + "." + extension
for i in range(1, 1000000):
tlog = os.path.join(current_logdir, '%s-%u.' %
(basename, i)) + extension
if not os.path.exists(tlog):
return tlog
util.failure("Unable to create new tlog for %s" % basename)
def check_status(conn):
'''check SYS_STATUS flags'''
sensor_bits = {
#'MAG' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_MAG,
'ACCEL' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_ACCEL,
'GYRO' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO
}
teststatus = conn.testmav.recv_match(type='SYS_STATUS', blocking=True, timeout=5)
if teststatus is None:
util.failure("Failed to get SYS_STATUS from test board")
#print(teststatus)
refstatus = conn.refmav.recv_match(type='SYS_STATUS', blocking=True, timeout=2)
if refstatus is None:
util.failure("Failed to get SYS_STATUS from reference board")
#print(refstatus)
for bit in sensor_bits:
present = refstatus.onboard_control_sensors_present & sensor_bits[bit]
enabled = refstatus.onboard_control_sensors_enabled & sensor_bits[bit]
health = refstatus.onboard_control_sensors_health & sensor_bits[bit]
logger.info("%s present" % present)
logger.info("%s enabled" % enabled)
logger.info("%s health" % health)
if present == 0 or present != enabled or present != health:
util.failure("Reference board %s failure in SYS_STATUS" % bit)
present = teststatus.onboard_control_sensors_present & sensor_bits[bit]
enabled = teststatus.onboard_control_sensors_enabled & sensor_bits[bit]
health = teststatus.onboard_control_sensors_health & sensor_bits[bit]
logger.info("%s present" % present)
logger.info("%s enabled" % enabled)
logger.info("%s health" % health)
if present == 0 or present != enabled or present != health:
util.failure("Test board %s failure in SYS_STATUS" % bit)
logger.info("%s status OK" % bit)
def new_log_dir():
'''create a new log directory for a run'''
global current_logdir
current_logdir = None
dirname = "logs/%s" % time.strftime("%Y-%m-%d")
mkdir_p(dirname)
highest = None
for i in range(1, 1000000):
dir = os.path.join(dirname, 'run%u' % i)
if not os.path.exists(dir):
current_logdir = dir
mkdir_p(dir)
return dir
util.failure("Failed to find log directory")
def new_log_dir():
"""create a new log directory for a run"""
global current_logdir
current_logdir = None
dirname = "logs/%s" % time.strftime("%Y-%m-%d")
mkdir_p(dirname)
highest = None
for i in range(1, 1000000):
dir = os.path.join(dirname, "run%u" % i)
if not os.path.exists(dir):
current_logdir = dir
mkdir_p(dir)
return dir
util.failure("Failed to find log directory")
def check_status(conn):
'''check SYS_STATUS flags'''
sensor_bits = {
'MAG': mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_MAG,
'ACCEL': mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_ACCEL,
'GYRO': mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO
}
teststatus = conn.testmav.recv_match(type='SYS_STATUS',
blocking=True,
timeout=5)
if teststatus is None:
util.failure("Failed to get SYS_STATUS from test board")
refstatus = conn.testmav.recv_match(type='SYS_STATUS',
blocking=True,
timeout=2)
if refstatus is None:
util.failure("Failed to get SYS_STATUS from reference board")
for bit in sensor_bits:
present = refstatus.onboard_control_sensors_present & sensor_bits[bit]
enabled = refstatus.onboard_control_sensors_enabled & sensor_bits[bit]
health = refstatus.onboard_control_sensors_health & sensor_bits[bit]
if present == 0 or present != enabled or present != health:
util.failure("Reference board %s failure in SYS_STATUS" % bit)
present = teststatus.onboard_control_sensors_present & sensor_bits[bit]
enabled = teststatus.onboard_control_sensors_enabled & sensor_bits[bit]
health = teststatus.onboard_control_sensors_health & sensor_bits[bit]
if present == 0 or present != enabled or present != health:
util.failure("Test board %s failure in SYS_STATUS" % bit)
logger.info("%s status OK" % bit)
def check_gyro_cal(conn):
'''check gyro cal'''
for idx in range(NUM_GYROS):
if idx == 0:
n = ''
else:
n = '%u' % (idx+1)
for axis in ['X', 'Y', 'Z']:
pname = 'INS_GYR%sOFFS_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
ofs = float(conn.test.match.group(1))
if abs(ofs) < 0.000001:
util.failure("%s is zero - gyro %u not calibrated (offset)" % (pname, idx))
logger.info("Gyro cal %u OK" % (idx+1))
def check_gyro_cal(conn):
'''check gyro cal'''
for idx in range(NUM_GYROS):
if idx == 0:
n = ''
else:
n = '%u' % (idx + 1)
for axis in ['X', 'Y', 'Z']:
pname = 'INS_GYR%sOFFS_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
ofs = float(conn.test.match.group(1))
if abs(ofs) < 0.000001:
util.failure("%s is zero - gyro %u not calibrated (offset)" %
(pname, idx))
logger.info("Gyro cal %u OK" % (idx + 1))
def notify_bot_spam(bot, update, args=None):
tg_user = update.message.from_user
user = User.from_telegram_object(tg_user)
if util.stop_banned(update, user):
return
reply_to = util.original_reply_id(update)
if args:
text = ' '.join(args)
else:
text = update.message.text
command_no_args = len(
re.findall(r'^/spam\s*$',
text)) > 0 or text.lower().strip() == '/spam@botlistbot'
if command_no_args:
update.message.reply_text(util.action_hint(
"Please use this command with an argument. For example:\n/spam @mybot"
),
reply_to_message_id=reply_to)
return
# `#spam` is already checked by handler
try:
username = re.match(settings.REGEX_BOT_IN_TEXT, text).groups()[0]
if username == '@' + settings.SELF_BOT_NAME:
log.info("Ignoring {}".format(text))
return
except AttributeError:
if args:
update.message.reply_text(util.failure(
"Sorry, but you didn't send me a bot `@username`."),
quote=True,
parse_mode=ParseMode.MARKDOWN,
reply_to_message_id=reply_to)
else:
log.info("Ignoring {}".format(text))
# no bot username, ignore update
pass
return
try:
spam_bot = Bot.get(
fn.lower(Bot.username)**username.lower(), Bot.approved == True)
try:
Suggestion.get(action="spam", subject=spam_bot)
except Suggestion.DoesNotExist:
suggestion = Suggestion(user=user,
action="spam",
date=datetime.date.today(),
subject=spam_bot)
suggestion.save()
update.message.reply_text(util.success(
"Thank you! We will review your suggestion and mark the bot as spammy."
),
reply_to_message_id=reply_to)
except Bot.DoesNotExist:
update.message.reply_text(
util.action_hint("The bot you sent me is not in the @BotList."),
reply_to_message_id=reply_to)
return ConversationHandler.END
def check_accel_cal(conn):
'''check accel cal'''
for idx in range(NUM_ACCELS):
if idx == 0:
n = ''
else:
n = '%u' % (idx+1)
for axis in ['X', 'Y', 'Z']:
pname = 'INS_ACC%sOFFS_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
if abs(ofs) < 1.0e-10:
util.failure("%s is zero - accel %u not calibrated (offset) %f" % (pname, idx, ofs))
pname = 'INS_ACC%sSCAL_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
if abs(ofs-1.0) < 1.0e-10:
util.failure("%s is zero - accel %u not calibrated (scale)" % (pname, idx))
logger.info("Accel cal %u OK" % (idx+1))
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)
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 edit_bot(bot, update, chat_data, to_edit=None):
uid = util.uid_from_update(update)
message_id = util.mid_from_update(update)
user = User.from_update(update)
if not to_edit:
if update.message:
command = update.message.text
if "edit" in command:
b_id = re.match(r"^/edit(\d+)$", command).groups()[0]
elif "approve" in command:
b_id = re.match(r"^/approve(\d+)$", command).groups()[0]
else:
raise ValueError("No 'edit' or 'approve' in command.")
try:
to_edit = Bot.get(id=b_id)
except Bot.DoesNotExist:
update.message.reply_text(
util.failure("No bot exists with this id."))
return
else:
bot.formatter.send_failure(uid, "An unexpected error occured.")
return
# if not to_edit.approved:
# return approve_bots(bot, update, override_list=[to_edit])
pending_suggestions = Suggestion.pending_for_bot(to_edit, user)
reply_markup = InlineKeyboardMarkup(
_edit_bot_buttons(to_edit, pending_suggestions, uid
in settings.MODERATORS))
pending_text = ("\n\n{} Some changes are pending approval{}.".format(
captions.SUGGESTION_PENDING_EMOJI,
"" if user.chat_id in settings.MODERATORS else " by a moderator",
) if pending_suggestions else "")
meta_text = ("\n\nDate added: {}\nMember since revision {}\n"
"Submitted by {}\nApproved by {}".format(
to_edit.date_added,
to_edit.revision,
to_edit.submitted_by,
to_edit.approved_by,
))
bot.formatter.send_or_edit(
uid,
"🛃 Edit {}{}{}".format(
to_edit.detail_text,
meta_text if user.id in settings.MODERATORS else "",
pending_text,
),
to_edit=message_id,
reply_markup=reply_markup,
)
def notify_submittant_rejected(bot, admin_user, notify_submittant, reason,
to_reject):
notification_successful = False
msg = "{} rejected by {}.".format(to_reject.username, admin_user)
if notify_submittant or reason:
try:
if reason:
bot.send_message(
to_reject.submitted_by.chat_id,
util.failure(
messages.REJECTION_WITH_REASON.format(
to_reject.username, reason=reason)),
)
else:
bot.sendMessage(
to_reject.submitted_by.chat_id,
util.failure(
messages.REJECTION_PRIVATE_MESSAGE.format(
to_reject.username)),
)
msg += "\nUser {} was notified.".format(str(
to_reject.submitted_by))
notification_successful = True
except TelegramError:
msg += "\nUser {} could NOT be contacted/notified in private.".format(
str(to_reject.submitted_by))
notification_successful = False
text = util.success("{} rejected.".format(to_reject.username))
if notification_successful is True:
text += " User {} was notified.".format(
to_reject.submitted_by.plaintext)
elif notification_successful is False:
try:
text += " " + mdformat.failure("Could not contact {}.".format(
to_reject.submitted_by.plaintext))
except:
pass
else:
text += " No notification sent."
return msg
def check_accel_cal(conn):
'''check accel cal'''
for idx in range(NUM_ACCELS):
if idx == 0:
n = ''
else:
n = '%u' % (idx + 1)
for axis in ['X', 'Y', 'Z']:
pname = 'INS_ACC%sOFFS_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
if abs(ofs) < 1.0e-10:
util.failure(
"%s is zero - accel %u not calibrated (offset) %f" %
(pname, idx, ofs))
pname = 'INS_ACC%sSCAL_%s' % (n, axis)
ofs = util.param_value(conn.test, pname)
if abs(ofs - 1.0) < 1.0e-10:
util.failure("%s is zero - accel %u not calibrated (scale)" %
(pname, idx))
logger.info("Accel cal %u OK" % (idx + 1))
def wait_gyros(conn):
"""wait for gyros to be ready"""
tries = 3
while tries > 0:
tries -= 1
if not wait_gyros_healthy(conn):
util.failure("Failed to get healthy gyros")
rotate.wait_quiescent(conn.refmav)
logger.info("Reference is quiescent")
try:
rotate.wait_quiescent_list(conn.testmav, ["RAW_IMU", "SCALED_IMU2", "SCALED_IMU3"])
logger.info("Test is quiescent")
except Exception as ex:
logger.debug("Recalibrating gyros : %s" % ex)
conn.test.send("gyrocal\n")
conn.test.expect("Calibrated")
continue
break
if tries == 0:
util.failure("Failed waiting for gyros")
logger.info("Gyros ready")
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 wait_gyros(conn):
'''wait for gyros to be ready'''
tries = 3
while tries > 0:
tries -= 1
if not wait_gyros_healthy(conn):
util.failure("Failed to get healthy gyros")
rotate.wait_quiescent(conn.refmav)
logger.info("Reference is quiescent")
try:
rotate.wait_quiescent_list(
conn.testmav, ['RAW_IMU', 'SCALED_IMU2', 'SCALED_IMU3'])
logger.info("Test is quiescent")
except Exception as ex:
logger.debug("Recalibrating gyros : %s" % ex)
conn.test.send('gyrocal\n')
conn.test.expect('Calibrated')
continue
break
if tries == 0:
util.failure("Failed waiting for gyros")
logger.info("Gyros ready")
def load_firmware(device, firmware, mcu_id, run=False):
'''load a given firmware'''
cmd = "%s %s" % (GDB, firmware)
logger.info("Loading firmware %s" % firmware)
log = StringIO()
try:
gdb = pexpect.spawn(cmd, logfile=log, timeout=10)
gdb.expect("Reading symbols from")
gdb.expect("done")
gdb.expect("(gdb)")
gdb.send("target extended %s\n" % device)
gdb.expect("Remote debugging using")
gdb.expect("(gdb)")
gdb.send("monitor swdp_scan\n")
cpu = gdb.expect([CPUID_IO, CPUID_FMU])
if cpu == 0:
if mcu_id != CPUID_IO:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (CPUID_IO, mcu_id))
else:
if mcu_id != CPUID_FMU:
util.failure("Incorrect CPU ID '%s' - expected '%s'" % (CPUID_FMU, mcu_id))
gdb.expect("(gdb)")
gdb.send("attach 1\n")
gdb.expect("(gdb)")
gdb.send("set mem inaccessible-by-default off\n")
gdb.expect("(gdb)")
gdb.send("load\n")
gdb.expect("Loading section .text", timeout=20)
gdb.expect("Loading section .data", timeout=30)
gdb.expect("Start address", timeout=10)
gdb.expect("Transfer rate", timeout=10)
gdb.expect("(gdb)")
if run:
gdb.send("run\n")
gdb.expect("Start it from the beginning?")
gdb.send("y\n")
except Exception as ex:
util.show_error('Loading firmware %s' % firmware, ex, log)
def gyro_integrate(conn):
'''test gyros by integrating while rotating to the given rotations'''
conn.ref.send('set streamrate -1\n')
conn.test.send('set streamrate -1\n')
util.param_set(conn.ref, 'SR0_RAW_SENS', 20)
util.param_set(conn.test, 'SR0_RAW_SENS', 20)
logger.info("Starting gyro integration at %s" % time.ctime())
wait_quiescent(conn.refmav)
conn.discard_messages()
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1+200)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS['level'].chan2+200)
start_time = time.time()
ref_tstart = None
test_tstart = [None]*3
ref_sum = Vector3()
test_sum = [Vector3(), Vector3(), Vector3()]
msgs = { 'RAW_IMU' : 0, 'SCALED_IMU2' : 1, 'SCALED_IMU3' : 2 }
while time.time() < start_time+20:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
gyro = util.gyro_vector(imu)
tnow = imu.time_usec*1.0e-6
if ref_tstart is not None:
deltat = tnow - ref_tstart
ref_sum += gyro * deltat
ref_tstart = tnow
if time.time() - start_time > 2 and gyro.length() < GYRO_TOLERANCE:
break
imu = conn.testmav.recv_match(type=msgs.keys(), blocking=False)
if imu is not None:
idx = msgs[imu.get_type()]
gyro = util.gyro_vector(imu)
if imu.get_type().startswith("SCALED_IMU"):
tnow = imu.time_boot_ms*1.0e-3
else:
tnow = imu.time_usec*1.0e-6
if test_tstart[idx] is not None:
deltat = tnow - test_tstart[idx]
test_sum[idx] += gyro * deltat
test_tstart[idx] = tnow
logger.debug("Gyro ref sums: %s" % ref_sum)
logger.debug("Gyro test sum1: %s" % test_sum[0])
logger.debug("Gyro test sum2: %s" % test_sum[1])
logger.debug("Gyro test sum3: %s" % test_sum[2])
for idx in range(3):
err = test_sum[idx] - ref_sum
if abs(err.x) > GYRO_SUM_TOLERANCE:
util.failure("X gyro %u error: %.1f" % (idx, err.x))
if abs(err.y) > GYRO_SUM_TOLERANCE:
util.failure("Y gyro %u error: %.1f" % (idx, err.y))
if abs(err.z) > GYRO_SUM_TOLERANCE:
util.failure("Z gyro %u error: %.1f" % (idx, err.z))
def gyro_integrate(conn):
'''test gyros by integrating while rotating to the given rotations'''
conn.ref.send('set streamrate -1\n')
conn.test.send('set streamrate -1\n')
util.param_set(conn.ref, 'SR0_RAW_SENS', 20)
util.param_set(conn.test, 'SR0_RAW_SENS', 20)
logger.info("Starting gyro integration")
wait_quiescent(conn.refmav)
conn.discard_messages()
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1+200)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS['level'].chan2+200)
start_time = time.time()
ref_tstart = None
test_tstart = [None]*3
ref_sum = Vector3()
test_sum = [Vector3(), Vector3(), Vector3()]
msgs = { 'RAW_IMU' : 0, 'SCALED_IMU2' : 1, 'SCALED_IMU3' : 2 }
while time.time() < start_time+20:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
gyro = util.gyro_vector(imu)
tnow = imu.time_usec*1.0e-6
if ref_tstart is not None:
deltat = tnow - ref_tstart
ref_sum += gyro * deltat
ref_tstart = tnow
if time.time() - start_time > 2 and gyro.length() < GYRO_TOLERANCE:
break
imu = conn.testmav.recv_match(type=msgs.keys(), blocking=False)
if imu is not None:
idx = msgs[imu.get_type()]
gyro = util.gyro_vector(imu)
if imu.get_type().startswith("SCALED_IMU"):
tnow = imu.time_boot_ms*1.0e-3
else:
tnow = imu.time_usec*1.0e-6
if test_tstart[idx] is not None:
deltat = tnow - test_tstart[idx]
test_sum[idx] += gyro * deltat
test_tstart[idx] = tnow
logger.debug("Gyro ref sums: %s" % ref_sum)
logger.debug("Gyro test sum1: %s" % test_sum[0])
logger.debug("Gyro test sum2: %s" % test_sum[1])
logger.debug("Gyro test sum3: %s" % test_sum[2])
for idx in range(3):
err = test_sum[idx] - ref_sum
if abs(err.x) > GYRO_SUM_TOLERANCE:
util.failure("X gyro %u error: %.1f" % (idx, err.x))
if abs(err.y) > GYRO_SUM_TOLERANCE:
util.failure("Y gyro %u error: %.1f" % (idx, err.y))
if abs(err.z) > GYRO_SUM_TOLERANCE:
util.failure("Z gyro %u error: %.1f" % (idx, err.z))
def search_query(bot, update, chat_data, query, send_errors=True):
cid = update.effective_chat.id
results = search.search_bots(query)
is_admin = cid in settings.MODERATORS
reply_markup = ReplyKeyboardMarkup(
basic.main_menu_buttons(is_admin),
resize_keyboard=True) if util.is_private_message(update) else None
if results:
if len(results) == 1:
return send_bot_details(bot, update, chat_data, results[0])
too_many_results = len(results) > settings.MAX_SEARCH_RESULTS
bots_list = ''
if cid in settings.MODERATORS:
# append edit buttons
bots_list += '\n'.join([
"{} — /edit{} 🛃".format(b, b.id)
for b in list(results)[:100]
])
else:
bots_list += '\n'.join(
[str(b) for b in list(results)[:settings.MAX_SEARCH_RESULTS]])
bots_list += '\n…' if too_many_results else ''
bots_list = messages.SEARCH_RESULTS.format(
bots=bots_list,
num_results=len(results),
plural='s' if len(results) > 1 else '',
query=query)
msg = update.message.reply_text(bots_list,
parse_mode=ParseMode.MARKDOWN,
reply_markup=reply_markup)
else:
if send_errors:
update.message.reply_text(util.failure(
"Sorry, I couldn't find anything related "
"to *{}* in the @BotList. /search".format(
util.escape_markdown(query))),
parse_mode=ParseMode.MARKDOWN,
reply_markup=reply_markup)
return ConversationHandler.END
def add_favorite_handler(bot, update, args=None):
uid = util.uid_from_update(update)
from components.basic import main_menu_buttons
main_menu_markup = ReplyKeyboardMarkup(main_menu_buttons(uid in settings.MODERATORS))
if args:
query = ' '.join(args) if isinstance(args, list) else args
try:
# TODO: add multiple
username = re.match(settings.REGEX_BOT_IN_TEXT, query).groups()[0]
try:
# TODO: get exact database matches for input without `@`
item = Bot.by_username(username, include_disabled=True)
return add_favorite(bot, update, item)
except Bot.DoesNotExist:
buttons = [
InlineKeyboardButton(
"Yai!", callback_data=util.callback_for_action(CallbackActions.ADD_ANYWAY, {'u': username})),
InlineKeyboardButton("Nay...", callback_data=util.callback_for_action(CallbackActions.ADD_FAVORITE))
]
reply_markup = InlineKeyboardMarkup([buttons])
util.send_md_message(bot, uid,
"{} is not in the @BotList. Do you want to add it to your {} anyway?".format(
username, captions.FAVORITES),
reply_markup=reply_markup)
except AttributeError:
# invalid bot username
# TODO when does this happen?
update.message.reply_text(
util.failure("Sorry, but that is not a valid username. Please try again. /addfav"))
else:
buttons = [
InlineKeyboardButton("Search inline", switch_inline_query_current_chat='')
]
reply_markup = InlineKeyboardMarkup([buttons])
bot.sendMessage(uid, messages.ADD_FAVORITE, reply_markup=ForceReply(selective=True))
return ConversationHandler.END
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_power(conn):
'''check power'''
ref_vcc = util.wait_field(conn.refmav, 'POWER_STATUS', 'Vcc')*0.001
test_vcc = util.wait_field(conn.testmav, 'POWER_STATUS', 'Vcc')*0.001
if abs(ref_vcc - test_vcc) > VOLTAGE_TOLERANCE:
util.failure("Vcc error %.2f should be %.2f" % (test_vcc, ref_vcc))
ref_vservo = util.wait_field(conn.refmav, 'POWER_STATUS', 'Vservo')*0.001
test_vservo = util.wait_field(conn.testmav, 'POWER_STATUS', 'Vservo')*0.001
if abs(ref_vservo - test_vservo) > VOLTAGE_TOLERANCE:
util.failure("Vservo error %.2f should be %.2f" % (test_vservo, ref_vservo))
test_flags = util.wait_field(conn.testmav, 'POWER_STATUS', 'flags')
pflags = mavutil.mavlink.MAV_POWER_STATUS_BRICK_VALID
pflags |= mavutil.mavlink.MAV_POWER_STATUS_SERVO_VALID
pflags |= mavutil.mavlink.MAV_POWER_STATUS_USB_CONNECTED
if test_flags != pflags:
util.failure("power flags error %u should be %u" % (test_flags, pflags))
logger.info("Voltages OK")
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_power(conn):
'''check power'''
ref_vcc = util.wait_field(conn.refmav, 'POWER_STATUS', 'Vcc') * 0.001
test_vcc = util.wait_field(conn.testmav, 'POWER_STATUS', 'Vcc') * 0.001
if abs(ref_vcc - test_vcc) > VOLTAGE_TOLERANCE:
util.failure("Vcc error %.2f should be %.2f" % (test_vcc, ref_vcc))
ref_vservo = util.wait_field(conn.refmav, 'POWER_STATUS', 'Vservo') * 0.001
test_vservo = util.wait_field(conn.testmav, 'POWER_STATUS',
'Vservo') * 0.001
if abs(ref_vservo - test_vservo) > VOLTAGE_TOLERANCE:
util.failure("Vservo error %.2f should be %.2f" %
(test_vservo, ref_vservo))
test_flags = util.wait_field(conn.testmav, 'POWER_STATUS', 'flags')
pflags = mavutil.mavlink.MAV_POWER_STATUS_BRICK_VALID
pflags |= mavutil.mavlink.MAV_POWER_STATUS_SERVO_VALID
pflags |= mavutil.mavlink.MAV_POWER_STATUS_USB_CONNECTED
if test_flags != pflags:
util.failure("power flags error %u should be %u" %
(test_flags, pflags))
logger.info("Voltages OK")
def send_bot_details(bot, update, chat_data, item=None):
is_group = util.is_group_message(update)
cid = update.effective_chat.id
user = User.from_update(update)
first_row = list()
if item is None:
if is_group:
return
try:
text = update.message.text
bot_in_text = re.findall(settings.REGEX_BOT_IN_TEXT, text)[0]
item = Bot.by_username(bot_in_text)
except Bot.DoesNotExist:
update.message.reply_text(
util.failure(
"This bot is not in the @BotList. If you think this is a mistake, see the /examples for /contributing."
))
return
if item.approved:
# bot is already in the botlist => show information
txt = item.detail_text
if item.description is None and not Keyword.select().where(
Keyword.entity == item).exists():
txt += ' is in the @BotList.'
btn = InlineCallbackButton(captions.BACK_TO_CATEGORY,
CallbackActions.SELECT_BOT_FROM_CATEGORY,
{'id': item.category.id})
first_row.insert(0, btn)
first_row.append(
InlineKeyboardButton(captions.SHARE,
switch_inline_query=item.username))
# if cid in settings.MODERATORS:
first_row.append(
InlineKeyboardButton("📝 Edit",
callback_data=util.callback_for_action(
CallbackActions.EDIT_BOT,
{'id': item.id})))
else:
txt = '{} is currently pending to be accepted for the @BotList.'.format(
item)
if cid in settings.MODERATORS:
first_row.append(
InlineKeyboardButton("🛃 Accept / Reject",
callback_data=util.callback_for_action(
CallbackActions.APPROVE_REJECT_BOTS,
{'id': item.id})))
if is_group:
reply_markup = InlineKeyboardMarkup([])
else:
buttons = [first_row]
favorite_found = Favorite.search_by_bot(user, item)
if favorite_found:
buttons.append([
InlineKeyboardButton(captions.REMOVE_FAVORITE_VERBOSE,
callback_data=util.callback_for_action(
CallbackActions.REMOVE_FAVORITE, {
'id': favorite_found.id,
'details': True
}))
])
else:
buttons.append([
InlineKeyboardButton(captions.ADD_TO_FAVORITES,
callback_data=util.callback_for_action(
CallbackActions.ADD_TO_FAVORITES, {
'id': item.id,
'details': True
}))
])
reply_markup = InlineKeyboardMarkup(buttons)
reply_markup, callback = botlistchat.append_delete_button(
update, chat_data, reply_markup)
# Should we ever decide to show thumbnails *shrug*
# if os.path.exists(item.thumbnail_file):
# preview = True
# photo = '[\xad]({})'.format('{}/thumbnail/{}.jpeg'.format(
# settings.API_URL,
# item.username[1:]
# ))
# log.info(photo)
# txt = photo + txt
# else:
# preview = False
msg = bot.formatter.send_or_edit(cid,
txt,
to_edit=util.mid_from_update(update),
reply_markup=reply_markup)
callback(msg)
Statistic.of(update, 'view-details', item.username, Statistic.ANALYSIS)
return CallbackStates.SHOWING_BOT_DETAILS
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 adjust_ahrs_trim(conn, level_attitude):
"""
force the AHRS trim to zero, which removes the effect of the jig not being quite
level. This is only incorrect if the accels are not aligned on the board, which
we check in check_accel_cal later
As a side effect this function set AHRS_ORIENTATION for the test board to 12
"""
# start with board right way up
rotate.set_rotation(conn, "level")
# we need to work out what the error in attitude of the 3 IMUs on the test jig is
# to do that we start with it level, and measure the roll/pitch as compared to the reference
conn.discard_messages()
ref_imu = conn.refmav.recv_match(type="RAW_IMU", blocking=True, timeout=3)
test_imu1 = conn.testmav.recv_match(type="RAW_IMU", blocking=True, timeout=3)
test_imu2 = conn.testmav.recv_match(type="SCALED_IMU2", blocking=True, timeout=3)
test_imu3 = conn.testmav.recv_match(type="SCALED_IMU3", blocking=True, timeout=3)
if ref_imu is None:
util.failure("Lost comms to reference board in ahrs trim")
if test_imu1 is None or test_imu2 is None or test_imu3 is None:
util.failure("Lost comms to test board in ahrs trim")
(ref_roll, ref_pitch) = util.attitude_estimate(ref_imu)
(test_roll1, test_pitch1) = util.attitude_estimate(test_imu1)
(test_roll2, test_pitch2) = util.attitude_estimate(test_imu2)
(test_roll3, test_pitch3) = util.attitude_estimate(test_imu3)
# get the roll and pitch errors
roll_error1 = test_roll1 - ref_roll
roll_error2 = test_roll2 - ref_roll
roll_error3 = test_roll3 - ref_roll
pitch_error1 = test_pitch1 - ref_pitch
pitch_error2 = test_pitch2 - ref_pitch
pitch_error3 = test_pitch3 - ref_pitch
conn.discard_messages()
ref_imu = conn.refmav.recv_match(type="RAW_IMU", blocking=True, timeout=3)
test_imu1 = conn.testmav.recv_match(type="RAW_IMU", blocking=True, timeout=3)
test_imu2 = conn.testmav.recv_match(type="SCALED_IMU2", blocking=True, timeout=3)
test_imu3 = conn.testmav.recv_match(type="SCALED_IMU3", blocking=True, timeout=3)
if ref_imu is None:
util.failure("Lost comms to reference board in ahrs trim")
if test_imu1 is None or test_imu2 is None or test_imu3 is None:
util.failure("Lost comms to test board in ahrs trim")
logger.debug(
"Tilt Ref=(%.1f %.1f) Test1=(%.1f %.1f) Test2=(%.1f %.1f) Test3=(%.1f %.1f)"
% (ref_roll, ref_pitch, test_roll1, test_pitch1, test_roll2, test_pitch2, test_roll3, test_pitch3)
)
if abs(ref_roll) > ROTATION_TOLERANCE or abs(ref_pitch) > ROTATION_TOLERANCE:
util.failure("Reference board rotation error")
logger.debug(
"Tilt offsets: Roll(%.1f %.1f %.1f) Pitch(%.1f %.1f %.1f) "
% (roll_error1, roll_error2, roll_error3, pitch_error1, pitch_error2, pitch_error3)
)
if abs(roll_error1) > TILT_TOLERANCE1 or abs(roll_error2) > TILT_TOLERANCE1 or abs(roll_error3) > TILT_TOLERANCE3:
util.failure("Test board roll error")
if (
abs(pitch_error1) > TILT_TOLERANCE1
or abs(pitch_error2) > TILT_TOLERANCE1
or abs(pitch_error3) > TILT_TOLERANCE3
):
util.failure("Test board pitch error")
# flip upside down for the trim calculation
rotate.set_rotation(conn, "back")
# set orientation upside down for trim measurement
util.param_set(conn.test, "AHRS_ORIENTATION", 12)
# sleep an extra four seconds - we need to be very sure the board is still for trim
time.sleep(4)
conn.discard_messages()
# average over 30 samples for trim
num_samples = 30
test_roll = [0] * 3
test_pitch = [0] * 3
msgs = ["RAW_IMU", "SCALED_IMU2", "SCALED_IMU3"]
for i in range(num_samples):
for j in range(3):
test_imu = conn.testmav.recv_match(type=msgs[j], blocking=True, timeout=3)
if test_imu is None:
util.failure("Lost comms to test board in ahrs trim")
(roll, pitch) = util.attitude_estimate(test_imu)
test_roll[j] += roll
test_pitch[j] += pitch
for j in range(3):
test_roll[j] /= num_samples
test_pitch[j] /= num_samples
logger.debug(
"Average Trim tilt Test1=(%.1f %.1f) Test2=(%.1f %.1f) Test3=(%.1f %.1f)"
% (test_roll[0], test_pitch[0], test_roll[1], test_pitch[1], test_roll[2], test_pitch[2])
)
# setting a positive trim value reduces the attitude that is
# read. So setting a trim of 0.1 when level results in a attitude
# reading of -5.8 degrees
# this approach assumes the mpu6000 on the FMU (IMU3) is level
# with respect to the board, and that any attitude error is due to
# the isolation board mount. We use the average of the error from
# IMU1 and IMU2
trim_x = radians((test_roll[0] + test_roll[1]) * 0.5 - test_roll[2])
trim_y = radians((test_pitch[0] + test_pitch[1]) * 0.5 - test_pitch[2])
util.param_set(conn.test, "AHRS_TRIM_X", trim_x)
time.sleep(0.2)
util.param_set(conn.test, "AHRS_TRIM_Y", trim_y)
time.sleep(0.2)
logger.debug("Set trims AHRS_TRIM_X=%.4f AHRS_TRIM_Y=%.4f" % (trim_x, trim_y))
def reject_bot_submission(bot,
update,
args=None,
to_reject=None,
verbose=True,
notify_submittant=True,
reason=None):
uid = util.uid_from_update(update)
user = User.from_update(update)
if to_reject is None:
if not update.message.reply_to_message:
bot.send_message(
update.effective_user.id,
util.failure("You must reply to a message of mine."))
return
text = update.message.reply_to_message.text
reason = reason if reason else (" ".join(args) if args else None)
try:
update.message.delete()
except:
pass
username = helpers.find_bots_in_text(text, first=True)
if not username:
bot.send_message(
update.effective_user.id,
util.failure(
"No username in the message that you replied to."))
return
try:
to_reject = Bot.by_username(username)
except Bot.DoesNotExist:
bot.send_message(update.effective_user.id,
util.failure("Rejection failed: {} is not present in the " \
"database.".format(username)))
return
if to_reject.approved is True:
msg = "{} has already been accepted, so it cannot be rejected anymore.".format(
username)
bot.sendMessage(uid, util.failure(msg))
return
Statistic.of(update, 'reject', to_reject.username)
log_msg = "{} rejected by {}.".format(to_reject.username, user)
notification_successful = None
if notify_submittant or reason:
try:
if reason:
bot.send_message(
to_reject.submitted_by.chat_id,
util.failure(
messages.REJECTION_WITH_REASON.format(
to_reject.username, reason=reason)))
else:
bot.sendMessage(
to_reject.submitted_by.chat_id,
util.failure(
messages.REJECTION_PRIVATE_MESSAGE.format(
to_reject.username)))
log_msg += "\nUser {} was notified.".format(
str(to_reject.submitted_by))
notification_successful = True
except TelegramError:
log_msg += "\nUser {} could NOT be contacted/notified in private.".format(
str(to_reject.submitted_by))
notification_successful = False
to_reject.delete_instance()
log.info(log_msg)
text = util.success("{} rejected.".format(to_reject.username))
if notification_successful is True:
text += " User {} was notified.".format(
to_reject.submitted_by.plaintext)
elif notification_successful is False:
text += ' ' + mdformat.failure("Could not contact {}.".format(
to_reject.submitted_by.plaintext))
else:
text += " No notification sent."
if verbose:
bot.sendMessage(uid, text)
if update.callback_query:
update.callback_query.answer(text=text)
def gyro_integrate(conn):
'''test gyros by integrating while rotating to the given rotations'''
conn.ref.send('set streamrate -1\n')
conn.test.send('set streamrate -1\n')
util.param_set(conn.ref, 'SR0_RAW_SENS', 20)
util.param_set(conn.test, 'SR0_RAW_SENS', 20)
logger.info("Starting gyro integration")
wait_quiescent(conn.refmav)
conn.discard_messages()
if ETE == 0:
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1+200)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS['level'].chan2+200)
if ETE == 1:
ete = PixETE()
ete.position(45, 180)
time.sleep(1)
ete.rollspeed(4000)
ete.position(45, 0)
logger.info("Starting gyro motion")
start_time = time.time()
ref_tstart = None
test_tstart = [None]*3
ref_sum = Vector3()
test_sum = [Vector3(), Vector3(), Vector3()]
msgs = { 'RAW_IMU' : 0, 'SCALED_IMU2' : 1, 'SCALED_IMU3' : 2 }
while time.time() < start_time+20:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
#gyro = util.gyro_vector(imu)
gyro = Vector3(degrees(imu.xgyro*-0.001), degrees(imu.ygyro*0.001), degrees(imu.zgyro*-0.001)) #phil change.... when running this check from back to top, I found that the reference IMU X and Z were reversed.... this hack makes this pass, but I suspect something else is wrong, this should be reverted when that is found.
tnow = imu.time_usec*1.0e-6
if ref_tstart is not None:
deltat = tnow - ref_tstart
ref_sum += gyro * deltat
ref_tstart = tnow
if time.time() - start_time > 2 and gyro.length() < GYRO_TOLERANCE:
break
imu = conn.testmav.recv_match(type=msgs.keys(), blocking=False)
if imu is not None:
idx = msgs[imu.get_type()]
gyro = util.gyro_vector(imu)
if imu.get_type().startswith("SCALED_IMU"):
tnow = imu.time_boot_ms*1.0e-3
else:
tnow = imu.time_usec*1.0e-6
if test_tstart[idx] is not None:
deltat = tnow - test_tstart[idx]
test_sum[idx] += gyro * deltat
test_tstart[idx] = tnow
logger.debug("Gyro ref sums: %s" % ref_sum)
logger.debug("Gyro test sum1: %s" % test_sum[0])
logger.debug("Gyro test sum2: %s" % test_sum[1])
logger.debug("Gyro test sum3: %s" % test_sum[2])
ete.yawspeed(5000)
ete.rollspeed(5000)
ete.position(0, 0)
wait_quiescent(conn.refmav)
for idx in range(3):
err = test_sum[idx] - ref_sum
if abs(err.x) > GYRO_SUM_TOLERANCE:
util.failure("X gyro %u error: %.1f" % (idx, err.x))
if abs(err.y) > GYRO_SUM_TOLERANCE:
util.failure("Y gyro %u error: %.1f" % (idx, err.y))
if abs(err.z) > GYRO_SUM_TOLERANCE:
util.failure("Z gyro %u error: %.1f" % (idx, err.z))
logger.debug("Gyro test finished")
def adjust_ahrs_trim(conn, level_attitude):
'''
force the AHRS trim to zero, which removes the effect of the jig not being quite
level. This is only incorrect if the accels are not aligned on the board, which
we check in check_accel_cal later
'''
util.param_set(conn.test, 'AHRS_TRIM_X', 0)
util.param_set(conn.test, 'AHRS_TRIM_Y', 0)
# get the board level
rotate.set_rotation(conn, 'level')
# be really sure it has stopped moving. This next measurement is critical
time.sleep(2)
# we need to work out what the error in attitude of the 3 IMUs on the test jig is
# to do that we start with it level, and measure the roll/pitch as compared to the reference
# then we rotate it to pitch 90 and measure the yaw error
# check all accels are in range
conn.discard_messages()
ref_imu = conn.refmav.recv_match(type='RAW_IMU', blocking=True, timeout=3)
test_imu1 = conn.testmav.recv_match(type='RAW_IMU', blocking=True, timeout=3)
test_imu2 = conn.testmav.recv_match(type='SCALED_IMU2', blocking=True, timeout=3)
test_imu3 = conn.testmav.recv_match(type='SCALED_IMU3', blocking=True, timeout=3)
if ref_imu is None:
util.failure("Lost comms to reference board in ahrs trim")
if test_imu1 is None or test_imu2 is None or test_imu3 is None:
util.failure("Lost comms to test board in ahrs trim")
ref_accel = Vector3(ref_imu.xacc, ref_imu.yacc, ref_imu.zacc)*9.81*0.001
(ref_roll, ref_pitch) = util.attitude_estimate(ref_imu)
(test_roll1, test_pitch1) = util.attitude_estimate(test_imu1)
(test_roll2, test_pitch2) = util.attitude_estimate(test_imu2)
(test_roll3, test_pitch3) = util.attitude_estimate(test_imu3)
# get the roll and pitch errors
roll_error1 = (test_roll1 - ref_roll)
roll_error2 = (test_roll2 - ref_roll)
roll_error3 = (test_roll3 - ref_roll)
pitch_error1 = (test_pitch1 - ref_pitch)
pitch_error2 = (test_pitch2 - ref_pitch)
pitch_error3 = (test_pitch3 - ref_pitch)
# rotate to up position while integrating gyros to test
# all gyros are working
rotate.gyro_integrate(conn)
# finish rotation to pitch 90 to measure the yaw error
rotate.set_rotation(conn, 'up')
conn.discard_messages()
ref_imu = conn.refmav.recv_match(type='RAW_IMU', blocking=True, timeout=3)
test_imu1 = conn.testmav.recv_match(type='RAW_IMU', blocking=True, timeout=3)
test_imu2 = conn.testmav.recv_match(type='SCALED_IMU2', blocking=True, timeout=3)
test_imu3 = conn.testmav.recv_match(type='SCALED_IMU3', blocking=True, timeout=3)
if ref_imu is None:
util.failure("Lost comms to reference board in ahrs trim")
if test_imu1 is None or test_imu2 is None or test_imu3 is None:
util.failure("Lost comms to test board in ahrs trim")
ref_accel = Vector3(ref_imu.xacc, ref_imu.yacc, ref_imu.zacc)*9.81*0.001
# start rotating back to level ready for the end of the test
rotate.set_rotation(conn, 'level', wait=False)
# when pointing straight up the roll_esimtate() actually estimates yaw error in body frame
ref_yaw = util.roll_estimate(ref_imu)
test_yaw1 = util.roll_estimate(test_imu1)
test_yaw2 = util.roll_estimate(test_imu2)
test_yaw3 = util.roll_estimate(test_imu3)
yaw_error1 = test_yaw1 - ref_yaw
yaw_error2 = test_yaw2 - ref_yaw
yaw_error3 = test_yaw3 - ref_yaw
logger.debug("Tilt Ref=(%.1f %.1f %.1f) Test1=(%.1f %.1f %.1f) Test2=(%.1f %.1f %.1f) Test3=(%.1f %.1f %.1f)" % (
ref_roll, ref_pitch, ref_yaw,
test_roll1, test_pitch1, test_yaw1,
test_roll2, test_pitch2, test_yaw2,
test_roll3, test_pitch3, test_yaw3))
if (abs(ref_roll) > ROTATION_TOLERANCE or
abs(ref_pitch) > ROTATION_TOLERANCE or
abs(ref_yaw) > ROTATION_TOLERANCE):
util.failure("Reference board rotation error")
logger.debug("Tilt offsets: Roll(%.1f %.1f %.1f) Pitch(%.1f %.1f %.1f) Yaw(%.1f %.1f %.1f) " % (
roll_error1, roll_error2, roll_error3,
pitch_error1, pitch_error2, pitch_error3,
yaw_error1, yaw_error2, yaw_error3))
if (abs(roll_error1) > TILT_TOLERANCE1 or
abs(roll_error2) > TILT_TOLERANCE1 or
abs(roll_error3) > TILT_TOLERANCE3):
util.failure("Test board roll error")
if (abs(pitch_error1) > TILT_TOLERANCE1 or
abs(pitch_error2) > TILT_TOLERANCE1 or
abs(pitch_error3) > TILT_TOLERANCE3):
util.failure("Test board pitch error")
if (abs(yaw_error1) > TILT_TOLERANCE1 or
abs(yaw_error2) > TILT_TOLERANCE1 or
abs(yaw_error3) > TILT_TOLERANCE3):
util.failure("Test board yaw error")
# setting a positive trim value reduces the attitude that is
# read. So setting a trim of 0.1 when level results in a attitude
# reading of -5.8 degrees
# this assumes the reference board always reads correct attitude
# and that there is no attitude discrepance between test and
# reference boards
trim_x = radians((roll_error1+roll_error2)/2)
trim_y = radians((pitch_error1+pitch_error2)/2)
logger.debug("OLD Set trims AHRS_TRIM_X=%.4f AHRS_TRIM_Y=%.4f" % (trim_x, trim_y))
# this new approach assumes the mpu6000 on the FMU (IMU3) is level
# with respect to the board, and that any attitude error is due to
# the isolation board mount. We use the average of the error from
# IMU1 and IMU2
trim_x = radians((test_roll1+test_roll2)*0.5 - test_roll3)
trim_y = radians((test_pitch1+test_pitch2)*0.5 - test_pitch3)
util.param_set(conn.test, 'AHRS_TRIM_X', trim_x)
time.sleep(0.2)
util.param_set(conn.test, 'AHRS_TRIM_Y', trim_y)
time.sleep(0.2)
logger.debug("Set trims AHRS_TRIM_X=%.4f AHRS_TRIM_Y=%.4f" % (trim_x, trim_y))
def notify_bot_offline(bot, update, args=None):
tg_user = update.message.from_user
user = User.from_telegram_object(tg_user)
if util.stop_banned(update, user):
return
reply_to = util.original_reply_id(update)
if args:
text = ' '.join(args)
else:
text = update.message.text
command_no_args = len(re.findall(
r'^/offline\s*$',
text)) > 0 or text.lower().strip() == '/offline@botlistbot'
if command_no_args:
update.message.reply_text(util.action_hint(
"Please use this command with an argument. For example:\n/offline @mybot"
),
reply_to_message_id=reply_to)
return
# `#offline` is already checked by handler
try:
username = re.match(settings.REGEX_BOT_IN_TEXT, text).groups()[0]
if username == '@' + settings.SELF_BOT_NAME:
log.info("Ignoring {}".format(text))
return
except AttributeError:
if args:
update.message.reply_text(util.failure(
"Sorry, but you didn't send me a bot `@username`."),
quote=True,
parse_mode=ParseMode.MARKDOWN,
reply_to_message_id=reply_to)
else:
log.info("Ignoring {}".format(text))
# no bot username, ignore update
pass
return
def already_reported():
update.message.reply_text(mdformat.none_action(
"Someone already reported this, thanks anyway 😊"),
reply_to_message_id=reply_to)
try:
offline_bot = Bot.get(
fn.lower(Bot.username)**username.lower(), Bot.approved == True)
if offline_bot.offline:
return already_reported()
if offline_bot.official:
update.message.reply_text(mdformat.none_action(
"Official bots usually don't go offline for a long time. "
"Just wait a couple hours and it will be back up ;)"),
reply_to_message_id=reply_to)
return
try:
Suggestion.get(action="offline",
subject=offline_bot,
executed=False)
return already_reported()
except Suggestion.DoesNotExist:
suggestion = Suggestion(user=user,
action="offline",
value=True,
date=datetime.date.today(),
subject=offline_bot)
suggestion.save()
update.message.reply_text(util.success(
"Thank you! We will review your suggestion and set the bot offline."
),
reply_to_message_id=reply_to)
except Bot.DoesNotExist:
update.message.reply_text(
util.action_hint("The bot you sent me is not in the @BotList."),
reply_to_message_id=reply_to)
return ConversationHandler.END
def new_bot_submission(bot, update, chat_data, args=None):
tg_user = update.message.from_user
user = User.from_telegram_object(tg_user)
if util.stop_banned(update, user):
return
reply_to = util.original_reply_id(update)
if args:
text = ' '.join(args)
else:
text = update.message.text
command_no_args = len(
re.findall(r'^/new\s*$',
text)) > 0 or text.lower().strip() == '/new@botlistbot'
if command_no_args:
update.message.reply_text(util.action_hint(
"Please use this command with an argument. For example:\n/new @mybot 🔎"
),
reply_to_message_id=reply_to)
return
# `#new` is already checked by handler
try:
username = re.match(settings.REGEX_BOT_IN_TEXT, text).groups()[0]
if username.lower() == '@' + settings.SELF_BOT_NAME.lower():
log.info("Ignoring {}".format(text))
return
except AttributeError:
if args:
update.message.reply_text(util.failure(
"Sorry, but you didn't send me a bot `@username`."),
quote=True,
parse_mode=ParseMode.MARKDOWN,
reply_to_message_id=reply_to)
log.info("Ignoring {}".format(text))
# no bot username, ignore update
return
try:
new_bot = Bot.by_username(username)
if new_bot.approved:
update.message.reply_text(util.action_hint(
"Sorry fool, but {} is already in the @BotList 😉".format(
new_bot.username)),
reply_to_message_id=reply_to)
else:
update.message.reply_text(util.action_hint(
"{} has already been submitted. Please have patience...".
format(new_bot.username)),
reply_to_message_id=reply_to)
return
except Bot.DoesNotExist:
new_bot = Bot(revision=Revision.get_instance().next,
approved=False,
username=username,
submitted_by=user)
new_bot.inlinequeries = "🔎" in text
new_bot.official = "🔹" in text
# find language
languages = Country.select().execute()
for lang in languages:
if lang.emoji in text:
new_bot.country = lang
new_bot.date_added = datetime.date.today()
description_reg = re.match(settings.REGEX_BOT_IN_TEXT + ' -\s?(.*)', text)
description_notify = ''
if description_reg:
description = description_reg.group(2)
new_bot.description = description
description_notify = ' Your description was included.'
new_bot.save()
if util.is_private_message(update) and util.uid_from_update(
update) in settings.MODERATORS:
from components.explore import send_bot_details
send_bot_details(bot, update, chat_data, new_bot)
else:
msg = update.message.reply_text(util.success(
"You submitted {} for approval.{}".format(new_bot,
description_notify)),
parse_mode=ParseMode.MARKDOWN,
reply_to_message_id=reply_to)
return ConversationHandler.END
def check_baro(conn):
'''check baros'''
ref_press = util.wait_field(conn.refmav, 'SCALED_PRESSURE', 'press_abs')
if ref_press is None:
util.failure("No reference pressure")
press1 = util.wait_field(conn.testmav, 'SCALED_PRESSURE', 'press_abs')
press2 = util.wait_field(conn.testmav, 'SCALED_PRESSURE2', 'press_abs')
if press1 is None:
util.failure("No pressure1 available")
if press2 is None:
util.failure("No pressure2 available")
if abs(ref_press - press1) > PRESSURE_TOLERANCE:
util.failure("Baro1 error pressure=%f should be %f" % (press1, ref_press))
if abs(ref_press - press2) > PRESSURE_TOLERANCE:
util.failure("Baro2 error pressure=%f should be %f" % (press2, ref_press))
ref_temp = util.wait_field(conn.refmav, 'SCALED_PRESSURE', 'temperature')*0.01
temp1 = util.wait_field(conn.testmav, 'SCALED_PRESSURE', 'temperature')*0.01
temp2 = util.wait_field(conn.testmav, 'SCALED_PRESSURE2', 'temperature')*0.01
if abs(ref_temp - temp1) > TEMPERATURE_TOLERANCE:
util.failure("Baro1 error temperature=%f should be %f" % (temp1, ref_temp))
if abs(ref_temp - temp2) > TEMPERATURE_TOLERANCE:
util.failure("Baro2 error temperature=%f should be %f" % (temp2, ref_temp))
logger.info("Baros OK")
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)
