Beispiel #1
0
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')
Beispiel #2
0
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)        
Beispiel #3
0
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)
Beispiel #4
0
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()
Beispiel #5
0
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")
Beispiel #6
0
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)
Beispiel #7
0
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)
Beispiel #8
0
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
Beispiel #9
0
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
Beispiel #10
0
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()
Beispiel #11
0
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)
Beispiel #12
0
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)
Beispiel #13
0
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")
Beispiel #14
0
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)
Beispiel #15
0
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')
Beispiel #16
0
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")
Beispiel #17
0
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
Beispiel #18
0
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")
Beispiel #19
0
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
Beispiel #20
0
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)
Beispiel #21
0
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)
Beispiel #22
0
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)
Beispiel #23
0
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)
Beispiel #24
0
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")
Beispiel #25
0
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")
Beispiel #26
0
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)
Beispiel #27
0
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))
Beispiel #28
0
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))
Beispiel #29
0
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
Beispiel #30
0
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))
Beispiel #31
0
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)
Beispiel #32
0
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)
Beispiel #33
0
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,
    )
Beispiel #34
0
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
Beispiel #35
0
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))
Beispiel #36
0
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")
Beispiel #37
0
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
Beispiel #38
0
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")
Beispiel #39
0
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)        
Beispiel #40
0
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))
Beispiel #41
0
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))
Beispiel #42
0
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
Beispiel #44
0
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")
Beispiel #45
0
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")
Beispiel #46
0
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")
Beispiel #47
0
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")
Beispiel #48
0
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
Beispiel #49
0
    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)
Beispiel #50
0
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))
Beispiel #51
0
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)
Beispiel #52
0
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")
Beispiel #53
0
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))
Beispiel #54
0
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
Beispiel #55
0
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
Beispiel #56
0
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")
Beispiel #57
0
    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)