Beispiel #1
0
    def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):

        # find a motor peak
        self.takeoff(10, mode="QHOVER")

        hover_time = 15
        tstart = self.get_sim_time()
        self.progress("Hovering for %u seconds" % hover_time)
        while self.get_sim_time_cached() < tstart + hover_time:
            self.mav.recv_match(type='ATTITUDE', blocking=True)
        vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
        tend = self.get_sim_time()

        self.do_RTL()
        psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)

        # batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
        freq = psd["F"][numpy.argmax(psd["X"][minhz:maxhz]) + minhz] * (1000. / 1024.)
        peakdb = numpy.amax(psd["X"][minhz:maxhz])
        if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
            raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
        else:
            self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb))

        # we have a peak make sure that the FFT detected something close
        # logging is at 10Hz
        mlog = self.dfreader_for_current_onboard_log()
        # accuracy is determined by sample rate and fft length, given our use of quinn we could probably use half of this
        freqDelta = 1000. / fftLength
        pkAvg = freq

        while True:
            m = mlog.recv_match(
                type='FTN1',
                blocking=True,
                condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
            if m is None:
                break
            pkAvg = pkAvg + (0.1 * (m.PkAvg - pkAvg))
        # peak within resolution of FFT length
        if abs(pkAvg - freq) > freqDelta:
            raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq))

        return freq
Beispiel #2
0
    def test_wheelencoders(self):
        '''make sure wheel encoders are generally working'''
        ex = None
        try:
            self.set_parameter("ATC_BAL_SPD_FF", 0)
            self.set_parameter("WENC_TYPE", 10)
            self.set_parameter("AHRS_EKF_TYPE", 10)
            self.reboot_sitl()
            self.set_parameter("WENC2_TYPE", 10)
            self.set_parameter("WENC_POS_Y", 0.075)
            self.set_parameter("WENC2_POS_Y", -0.075)
            self.reboot_sitl()
            self.change_mode("HOLD")
            self.wait_ready_to_arm()
            self.change_mode("ACRO")
            self.arm_vehicle()
            self.set_rc(3, 1600)

            m = self.mav.recv_match(type='WHEEL_DISTANCE',
                                    blocking=True,
                                    timeout=5)
            if m is None:
                raise NotAchievedException("Did not get WHEEL_DISTANCE")

            tstart = self.get_sim_time()
            while True:
                if self.get_sim_time_cached() - tstart > 10:
                    break
                dist_home = self.distance_to_home(use_cached_home=True)
                m = self.mav.messages.get("WHEEL_DISTANCE")
                delta = abs(m.distance[0] - dist_home)
                self.progress("dist-home=%f wheel-distance=%f delta=%f" %
                              (dist_home, m.distance[0], delta))
                if delta > 5:
                    raise NotAchievedException("wheel distance incorrect")
            self.disarm_vehicle()
        except Exception as e:
            self.progress("Caught exception: %s" %
                          self.get_exception_stacktrace(e))
            self.disarm_vehicle()
            ex = e
        self.reboot_sitl()
        if ex is not None:
            raise ex
Beispiel #3
0
 def enum_state_name(self, enum_name, state, pretrim=None):
     e = mavutil.mavlink.enums[enum_name]
     e_value = e[state]
     name = e_value.name
     if pretrim is not None:
         if not pretrim.startswith(pretrim):
             raise NotAchievedException("Expected %s to pretrim" %
                                        (pretrim))
         name = name.replace(pretrim, "")
     return name
Beispiel #4
0
    def test_sysid_enforce(self):
        '''Run the same arming code with correct then incorrect SYSID'''
        self.context_push()
        ex = None
        try:
            # if set_parameter is ever changed to not use MAVProxy
            # this test is going to break horribly.  Sorry.
            self.set_parameter("SYSID_MYGCS",
                               255)  # assume MAVProxy does this!
            self.set_parameter("SYSID_ENFORCE",
                               1)  # assume MAVProxy does this!

            self.change_mode('MANUAL')

            self.progress("make sure I can arm ATM")
            self.wait_ready_to_arm()
            self.arm_vehicle(timeout=5)
            self.disarm_vehicle()

            # temporarily set a different system ID than MAVProxy:
            self.progress("Attempting to arm vehicle myself")
            old_srcSystem = self.mav.mav.srcSystem
            try:
                self.mav.mav.srcSystem = 243
                self.arm_vehicle(timeout=5)
                self.disarm_vehicle()
                success = False
            except AutoTestTimeoutException as e:
                success = True
                pass
            self.mav.srcSystem = old_srcSystem
            if not success:
                raise NotAchievedException(
                    "Managed to arm with SYSID_ENFORCE set")

            self.progress("Attempting to arm vehicle from vehicle component")
            old_srcSystem = self.mav.mav.srcSystem
            comp_arm_exception = None
            try:
                self.mav.mav.srcSystem = 1
                self.arm_vehicle(timeout=5)
                self.disarm_vehicle()
            except Exception as e:
                comp_arm_exception = e
                pass
            self.mav.srcSystem = old_srcSystem
            if comp_arm_exception is not None:
                raise comp_arm_exception

        except Exception as e:
            self.progress("Exception caught")
            ex = e
        self.context_pop()
        if ex is not None:
            raise ex
Beispiel #5
0
    def test_gcs_fence(self):
        self.progress("Testing FENCE_POINT protocol")
        self.set_parameter("FENCE_TOTAL", 1)
        target_system = 1
        target_component = 1

        lat = 1.2345
        lng = 5.4321
        self.mav.mav.fence_point_send(target_system, target_component, 0, 1,
                                      lat, lng)
        self.progress("Requesting fence return point")
        self.mav.mav.fence_fetch_point_send(target_system, target_component, 0)
        m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1)
        print("m: %s" % str(m))
        if m is None:
            raise NotAchievedException("Did not get fence return point back")
        if abs(m.lat - lat) > 0.000001:
            raise NotAchievedException(
                "Did not get correct lat in fencepoint: got=%f want=%f" %
                (m.lat, lat))
        if abs(m.lng - lng) > 0.000001:
            raise NotAchievedException(
                "Did not get correct lng in fencepoint: got=%f want=%f" %
                (m.lng, lng))

        self.progress("Now testing a different value")
        lat = 2.345
        lng = 4.321
        self.mav.mav.fence_point_send(target_system, target_component, 0, 1,
                                      lat, lng)
        self.progress("Requesting fence return point")
        self.mav.mav.fence_fetch_point_send(target_system, target_component, 0)
        m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1)
        print("m: %s" % str(m))
        if abs(m.lat - lat) > 0.000001:
            raise NotAchievedException(
                "Did not get correct lat in fencepoint: got=%f want=%f" %
                (m.lat, lat))
        if abs(m.lng - lng) > 0.000001:
            raise NotAchievedException(
                "Did not get correct lng in fencepoint: got=%f want=%f" %
                (m.lng, lng))
Beispiel #6
0
 def wait_distance_home_gt(self, distance, timeout=60):
     home_distance = None
     tstart = self.get_sim_time()
     while self.get_sim_time() - tstart < timeout:
         # m = self.mav.recv_match(type='VFR_HUD', blocking=True)
         pos = self.mav.location()
         home_distance = self.get_distance(HOME, pos)
         if home_distance > distance:
             return
     raise NotAchievedException("Failed to get %fm from home (now=%f)" %
                                (distance, home_distance))
Beispiel #7
0
    def test_alt_hold(self):
        """Test ALT_HOLD mode
        """
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.mavproxy.send('mode ALT_HOLD\n')
        self.wait_mode('ALT_HOLD')

        msg = self.mav.recv_match(type='GLOBAL_POSITION_INT',
                                  blocking=True,
                                  timeout=5)
        if msg is None:
            raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
        pwm = 1000
        if msg.relative_alt / 1000.0 < -5.5:
            # need to g`o up, not down!
            pwm = 2000
        self.set_rc(Joystick.Throttle, pwm)
        self.wait_altitude(altitude_min=-6, altitude_max=-5)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)

        self.watch_altitude_maintained()

        self.set_rc(Joystick.Throttle, 1000)
        self.wait_altitude(altitude_min=-20, altitude_max=-19)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)

        self.watch_altitude_maintained()

        self.set_rc(Joystick.Throttle, 1900)
        self.wait_altitude(altitude_min=-14, altitude_max=-13)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)

        self.watch_altitude_maintained()

        self.set_rc(Joystick.Throttle, 1900)
        self.wait_altitude(altitude_min=-5, altitude_max=-4)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)

        self.watch_altitude_maintained()

        self.disarm_vehicle()
 def achieve_attitude(self, desyaw, despitch, tolerance=1, target_system=2, target_component=1):
     '''use set_attitude_target to achieve desyaw / despitch'''
     tstart = self.get_sim_time()
     last_attitude_target_sent = 0
     last_debug = 0
     self.progress("Using set_attitude_target to achieve attitude")
     while True:
         now = self.get_sim_time()
         if now - tstart > 60:
             raise NotAchievedException("Did not achieve attitude")
         if now - last_attitude_target_sent > 0.5:
             last_attitude_target_sent = now
             type_mask = (
                 1 << 0 | # ignore roll rate
                 1 << 6 # ignore throttle
             )
             self.mav.mav.set_attitude_target_send(
                 0, # time_boot_ms
                 target_system, # target sysid
                 target_component, # target compid
                 type_mask, # bitmask of things to ignore
                 mavextra.euler_to_quat([0,
                                         math.radians(despitch),
                                         math.radians(desyaw)]), # att
                 0, # yaw rate (rad/s)
                 0, # pitch rate
                 0, # yaw rate
                 0) # thrust, 0 to 1, translated to a climb/descent rate
         m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=2)
         if m is None:
             raise NotAchievedException("Did not get ATTITUDE")
         if now - last_debug > 1:
             last_debug = now
             self.progress("yaw=%f desyaw=%f pitch=%f despitch=%f" %
                           (math.degrees(m.yaw), desyaw,
                            math.degrees(m.pitch), despitch))
         yaw_ok = abs(math.degrees(m.yaw) - desyaw) < tolerance
         pitch_ok = abs(math.degrees(m.pitch) - despitch) < tolerance
         if yaw_ok and pitch_ok:
             self.progress("Achieved attitude")
             break
Beispiel #9
0
 def wait_distance_home_gt(self, distance, timeout=60):
     home_distance = None
     tstart = self.get_sim_time()
     while self.get_sim_time_cached() - tstart < timeout:
         # m = self.mav.recv_match(type='VFR_HUD', blocking=True)
         distance_home = self.distance_to_home(use_cached_home=True)
         self.progress("distance_home=%f want=%f" % (distance_home, distance))
         if distance_home > distance:
             return
         self.drain_mav()
     raise NotAchievedException("Failed to get %fm from home (now=%f)" %
                                (distance, home_distance))
Beispiel #10
0
    def loiter(self, holdtime=10, maxaltchange=5, maxdistchange=5):
        """Hold loiter position."""
        self.mavproxy.send('switch 5\n')  # loiter mode
        self.wait_mode('LOITER')

        # first aim south east
        self.progress("turn south east")
        self.set_rc(4, 1580)
        self.wait_heading(170)
        self.set_rc(4, 1500)

        # fly south east 50m
        self.set_rc(2, 1100)
        self.wait_distance(50)
        self.set_rc(2, 1500)

        # wait for copter to slow moving
        self.wait_groundspeed(0, 2)

        m = self.mav.recv_match(type='VFR_HUD', blocking=True)
        start_altitude = m.alt
        start = self.mav.location()
        tstart = self.get_sim_time()
        self.progress("Holding loiter at %u meters for %u seconds" %
                      (start_altitude, holdtime))
        while self.get_sim_time() < tstart + holdtime:
            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
            pos = self.mav.location()
            delta = self.get_distance(start, pos)
            alt_delta = math.fabs(m.alt - start_altitude)
            self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
            if alt_delta > maxaltchange:
                self.progress("Loiter alt shifted %u meters (> limit of %u)" %
                              (alt_delta, maxaltchange))
                raise NotAchievedException()
            if delta > maxdistchange:
                self.progress("Loiter shifted %u meters (> limit of %u)" %
                              (delta, maxdistchange))
                raise NotAchievedException()
        self.progress("Loiter OK for %u seconds" % holdtime)
    def test_button(self):
        self.set_parameter("SIM_PIN_MASK", 0)
        self.set_parameter("BTN_ENABLE", 1)
        btn = 2
        pin = 3
        self.set_parameter("BTN_PIN%u" % btn, pin)
        self.drain_mav()
        m = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
        self.progress("m: %s" % str(m))
        if m is None:
            raise NotAchievedException("Did not get BUTTON_CHANGE event")
        mask = 1<<btn
        if m.state & mask:
            raise NotAchievedException("Bit incorrectly set in mask (got=%u dontwant=%u)" % (m.state, mask))
        # SITL instantly reverts the pin to its old value
        m2 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
        self.progress("m2: %s" % str(m2))
        if m2 is None:
            raise NotAchievedException("Did not get repeat message")
        # wait for messages to stop coming:
        self.drain_mav_seconds(15)

        self.set_parameter("SIM_PIN_MASK", 0)
        m3 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
        self.progress("m3: %s" % str(m3))
        if m3 is None:
            raise NotAchievedException("Did not get new message")
        if m.last_change_ms == m3.last_change_ms:
            raise NotAchievedException("last_change_ms same as first message")
        if m3.state != 0:
            raise NotAchievedException("Didn't get expected mask back in message (mask=0 state=%u" % (m3.state))
Beispiel #12
0
    def test_mot_thst_hover_ignore(self):
        """Test if we are ignoring MOT_THST_HOVER parameter
        """

        # Test default parameter value
        mot_thst_hover_value = self.get_parameter("MOT_THST_HOVER")
        if mot_thst_hover_value != 0.5:
            raise NotAchievedException("Unexpected default MOT_THST_HOVER parameter value {}".format(mot_thst_hover_value))

        # Test if parameter is being ignored
        for value in [0.25, 0.75]:
            self.set_parameter("MOT_THST_HOVER", value)
            self.test_alt_hold()
Beispiel #13
0
 def check_airspeeds(mav, m):
     m_type = m.get_type()
     if (m_type == 'NAMED_VALUE_FLOAT' and m.name == 'AS2'):
         airspeed[1] = m.value
     elif m_type == 'VFR_HUD':
         airspeed[0] = m.airspeed
     else:
         return
     if airspeed[0] is None or airspeed[1] is None:
         return
     delta = abs(airspeed[0] - airspeed[1])
     if delta > 3:
         raise NotAchievedException(
             "Airspeed mismatch (as1=%f as2=%f)" %
             (airspeed[0], airspeed[1]))
Beispiel #14
0
 def test_rc_option_camera_trigger(self):
     '''test toggling channel 12 takes picture'''
     x = self.mav.messages.get("CAMERA_FEEDBACK", None)
     if x is not None:
         raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
     self.set_rc(12, 2000)
     tstart = self.get_sim_time()
     while self.get_sim_time() - tstart < 10:
         x = self.mav.messages.get("CAMERA_FEEDBACK", None)
         if x is not None:
             break
         self.mav.wait_heartbeat()
     self.set_rc(12, 1000)
     if x is None:
         raise NotAchievedException("No CAMERA_FEEDBACK message received")
Beispiel #15
0
 def assert_extended_sys_state(self, vtol_state, landed_state):
     m = self.mav.recv_match(type='EXTENDED_SYS_STATE',
                             blocking=True,
                             timeout=1)
     if m is None:
         raise NotAchievedException(
             "Did not get extended_sys_state message")
     if m.vtol_state != vtol_state:
         raise ValueError("Bad MAV_VTOL_STATE.  Want=%s got=%s" %
                          (self.vtol_state_name(vtol_state),
                           self.vtol_state_name(m.vtol_state)))
     if m.landed_state != landed_state:
         raise ValueError("Bad MAV_LANDED_STATE.  Want=%s got=%s" %
                          (self.landed_state_name(landed_state),
                           self.landed_state_name(m.landed_state)))
Beispiel #16
0
    def test_camera_mission_items(self):
        self.context_push()
        ex = None
        try:
            self.mavproxy.send(
                'wp load %s\n' %
                os.path.join(testdir, "rover-camera-mission.txt"))
            self.wait_ready_to_arm()
            self.mavproxy.send('mode auto\n')
            self.wait_mode('AUTO')
            self.wait_ready_to_arm()
            self.arm_vehicle()
            prev_cf = None
            while True:
                cf = self.mav.recv_match(type='CAMERA_FEEDBACK', blocking=True)
                if prev_cf is None:
                    prev_cf = cf
                    continue
                dist_travelled = self.get_distance_int(prev_cf, cf)
                prev_cf = cf
                mc = self.mav.messages.get("MISSION_CURRENT", None)
                if mc is None:
                    continue
                elif mc.seq == 2:
                    expected_distance = 2
                elif mc.seq == 4:
                    expected_distance = 5
                elif mc.seq == 5:
                    break
                else:
                    continue
                self.progress("Expected distance %f got %f" %
                              (expected_distance, dist_travelled))
                error = abs(expected_distance - dist_travelled)
                # Rover moves at ~5m/s; we appear to do something at
                # 5Hz, so we do see over a meter of error!
                max_error = 1.5
                if error > max_error:
                    raise NotAchievedException(
                        "Camera distance error: %f (%f)" % (error, max_error))

            self.disarm_vehicle()
        except Exception as e:
            self.progress("Exception caught")
            ex = e
        self.context_pop()
        if ex is not None:
            raise ex
Beispiel #17
0
 def wait_level_flight(self, accuracy=5, timeout=30):
     """Wait for level flight."""
     tstart = self.get_sim_time()
     self.progress("Waiting for level flight")
     self.set_rc(1, 1500)
     self.set_rc(2, 1500)
     self.set_rc(4, 1500)
     while self.get_sim_time_cached() < tstart + timeout:
         m = self.mav.recv_match(type='ATTITUDE', blocking=True)
         roll = math.degrees(m.roll)
         pitch = math.degrees(m.pitch)
         self.progress("Roll=%.1f Pitch=%.1f" % (roll, pitch))
         if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy:
             self.progress("Attained level flight")
             return
     raise NotAchievedException("Failed to attain level flight")
Beispiel #18
0
 def wait_servo_channel_value(self, channel, value, timeout=2):
     channel_field = "servo%u_raw" % channel
     tstart = self.get_sim_time()
     while True:
         remaining = timeout - (self.get_sim_time_cached() - tstart)
         if remaining <= 0:
             raise NotAchievedException()
         m = self.mav.recv_match(type='SERVO_OUTPUT_RAW',
                                 blocking=True,
                                 timeout=remaining)
         m_value = getattr(m, channel_field, None)
         self.progress("SERVO_OUTPUT_RAW.%s=%u want=%u" %
                       (channel_field, m_value, value))
         if m_value is None:
             raise ValueError() #?
         if m_value == value:
             return
Beispiel #19
0
    def test_rc_option_camera_trigger(self):
        '''test toggling channel 12 takes picture'''
        self.set_parameter("RC12_OPTION", 9)  # CameraTrigger
        self.reboot_sitl()  # needed for RC12_OPTION to take effect

        x = self.mav.messages.get("CAMERA_FEEDBACK", None)
        if x is not None:
            raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
        self.set_rc(12, 2000)
        tstart = self.get_sim_time()
        while self.get_sim_time_cached() - tstart < 10:
            x = self.mav.messages.get("CAMERA_FEEDBACK", None)
            if x is not None:
                break
            self.wait_heartbeat()
        self.set_rc(12, 1000)
        if x is None:
            raise NotAchievedException("No CAMERA_FEEDBACK message received")
Beispiel #20
0
 def airspeed_autocal(self):
     self.progress("Ensure no AIRSPEED_AUTOCAL on ground")
     self.set_parameter("ARSPD_AUTOCAL", 1)
     m = self.mav.recv_match(type='AIRSPEED_AUTOCAL',
                             blocking=True,
                             timeout=5)
     if m is not None:
         raise NotAchievedException("Got autocal on ground")
     mission_filepath = os.path.join(testdir, "flaps.txt")
     self.load_mission(mission_filepath)
     self.wait_ready_to_arm()
     self.arm_vehicle()
     self.change_mode("AUTO")
     self.progress("Ensure AIRSPEED_AUTOCAL in air")
     m = self.mav.recv_match(type='AIRSPEED_AUTOCAL',
                             blocking=True,
                             timeout=5)
     self.mav.motors_disarmed_wait()
Beispiel #21
0
    def dive_set_position_target(self):
        self.change_mode('GUIDED')
        self.wait_ready_to_arm()
        self.arm_vehicle()

        startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
                                       blocking=True)

        lat = 5
        lon = 5
        alt = -10

        # send a position-control command
        self.mav.mav.set_position_target_global_int_send(
            0,  # timestamp
            1,  # target system_id
            1,  # target component id
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
            0b1111111111111000,  # mask specifying use-only-lat-lon-alt
            lat,  # lat
            lon,  # lon
            alt,  # alt
            0,  # vx
            0,  # vy
            0,  # vz
            0,  # afx
            0,  # afy
            0,  # afz
            0,  # yaw
            0,  # yawrate
        )

        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 200:
                raise NotAchievedException("Did not move far enough")
            pos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
                                      blocking=True)
            delta = self.get_distance_int(startpos, pos)
            self.progress("delta=%f (want >10)" % delta)
            if delta > 10:
                break
        self.change_mode('MANUAL')
        self.disarm_vehicle()
Beispiel #22
0
    def drive_rtl_mission(self):
        self.wait_ready_to_arm()
        self.arm_vehicle()

        mission_filepath = os.path.join("ArduRover-Missions", "rtl.txt")
        self.load_mission(mission_filepath)
        self.mavproxy.send('switch 4\n')  # auto mode
        self.set_rc(3, 1500)
        self.wait_mode('AUTO')
        self.mavproxy.expect('Executing RTL')

        m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
                                blocking=True,
                                timeout=0.1)
        if m is None:
            raise MsgRcvTimeoutException(
                "Did not receive NAV_CONTROLLER_OUTPUT message")

        wp_dist_min = 5
        if m.wp_dist < wp_dist_min:
            raise PreconditionFailedException(
                "Did not start at least %u metres from destination" %
                (wp_dist_min))

        self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
            m.wp_dist,
            wp_dist_min,
        ))

        self.wait_mode('HOLD', timeout=600)  # balancebot can take a long time!

        pos = self.mav.location()
        home_distance = self.get_distance(HOME, pos)
        home_distance_max = 5
        if home_distance > home_distance_max:
            raise NotAchievedException(
                "Did not get home (%u metres distant > %u)" %
                (home_distance, home_distance_max))
        self.mavproxy.send('switch 6\n')
        self.wait_mode('MANUAL')
        self.disarm_vehicle()
        self.progress("RTL Mission OK")
Beispiel #23
0
    def tailsitter(self):
        '''tailsitter test'''
        self.set_parameter('Q_FRAME_CLASS', 10)
        self.set_parameter('Q_ENABLE', 1)

        self.reboot_sitl()
        self.wait_ready_to_arm()
        value_before = self.get_servo_channel_value(3)
        self.progress("Before: %u" % value_before)
        self.change_mode('QHOVER')
        tstart = self.get_sim_time()
        while True:
            now = self.get_sim_time_cached()
            if now - tstart > 60:
                break
            value_after = self.get_servo_channel_value(3)
            self.progress("After: t=%f output=%u" % ((now - tstart), value_after))
            if value_before != value_after:
                raise NotAchievedException("Changed throttle output on mode change to QHOVER")
        self.disarm_vehicle()
Beispiel #24
0
    def watch_altitude_maintained(self, delta=1, timeout=5.0):
        """Watch and wait for the actual altitude to be maintained

        Keyword Arguments:
            delta {float} -- Maximum altitude range to be allowed from actual point (default: {0.5})
            timeout {float} -- Timeout time in simulation seconds (default: {5.0})

        Raises:
            NotAchievedException: Exception when altitude fails to hold inside the time and
                altitude range
        """
        tstart = self.get_sim_time_cached()
        previous_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt
        self.progress('Altitude to be watched: %f' % (previous_altitude))
        while True:
            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
            if self.get_sim_time_cached() - tstart > timeout:
                self.progress('Altitude hold done: %f' % (previous_altitude))
                return
            if abs(m.alt - previous_altitude) > delta:
                raise NotAchievedException("Altitude not maintained: want %.2f (+/- %.2f) got=%.2f" % (previous_altitude, delta, m.alt))
Beispiel #25
0
 def reboot_sitl(self):
     """Reboot SITL instance and wait it to reconnect."""
     # out battery is reset to full on reboot.  So reduce it to 10%
     # and wait for it to go above 50.
     self.run_cmd(mavutil.mavlink.MAV_CMD_BATTERY_RESET,
                  255,  # battery mask
                  10,  # percentage
                  0,
                  0,
                  0,
                  0,
                  0,
                  0)
     self.run_cmd_reboot()
     tstart = time.time()
     while True:
         if time.time() - tstart > 30:
             raise NotAchievedException("Did not detect reboot")
         # ask for the message:
         batt = None
         try:
             self.send_cmd(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,
                           mavutil.mavlink.MAVLINK_MSG_ID_BATTERY_STATUS,
                           0,
                           0,
                           0,
                           0,
                           0,
                           0)
             batt = self.mav.recv_match(type='BATTERY_STATUS',
                                        blocking=True,
                                        timeout=1)
         except ConnectionResetError:
             pass
         self.progress("Battery: %s" % str(batt))
         if batt is None:
             continue
         if batt.battery_remaining > 50:
             break
     self.initialise_after_reboot_sitl()
Beispiel #26
0
    def fly_avc_test(self):
        # Arm
        self.change_mode('STABILIZE')
        self.wait_ready_to_arm()

        self.arm_vehicle()
        self.progress("Raising rotor speed")
        self.set_rc(8, 2000)

        # upload mission from file
        self.progress("# Load copter_AVC2013_mission")
        # load the waypoint count
        num_wp = self.load_mission("copter_AVC2013_mission.txt", strict=False)
        if not num_wp:
            raise NotAchievedException("load copter_AVC2013_mission failed")

        self.progress("Fly AVC mission from 1 to %u" % num_wp)
        self.set_current_waypoint(1)

        # wait for motor runup
        self.delay_sim_time(20)

        # switch into AUTO mode and raise throttle
        self.change_mode('AUTO')
        self.set_rc(3, 1500)

        # fly the mission
        self.wait_waypoint(0, num_wp - 1, timeout=500)

        # set throttle to minimum
        self.zero_throttle()

        # wait for disarm
        self.wait_disarmed()
        self.progress("MOTORS DISARMED OK")

        self.progress("Lowering rotor speed")
        self.set_rc(8, 1000)

        self.progress("AVC mission completed: passed!")
Beispiel #27
0
    def drive_rtl_mission(self):
        mission_filepath = os.path.join(testdir, "ArduRover-Missions",
                                        "rtl.txt")
        self.mavproxy.send('wp load %s\n' % mission_filepath)
        self.mavproxy.expect('Flight plan received')
        self.mavproxy.send('switch 4\n')  # auto mode
        self.set_rc(3, 1500)
        self.wait_mode('AUTO')
        self.mavproxy.expect('Executing RTL')

        m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
                                blocking=True,
                                timeout=0.1)
        if m is None:
            self.progress("Did not receive NAV_CONTROLLER_OUTPUT message")
            raise MsgRcvTimeoutException()

        wp_dist_min = 5
        if m.wp_dist < wp_dist_min:
            self.progress("Did not start at least 5 metres from destination")
            raise PreconditionFailedException()

        self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
            m.wp_dist,
            wp_dist_min,
        ))

        self.wait_mode('HOLD')

        pos = self.mav.location()
        home_distance = self.get_distance(HOME, pos)
        home_distance_max = 5
        if home_distance > home_distance_max:
            self.progress("Did not get home (%u metres distant > %u)" %
                          (home_distance, home_distance_max))
            raise NotAchievedException()
        self.mavproxy.send('switch 6\n')
        self.wait_mode('MANUAL')
        self.progress("RTL Mission OK")
Beispiel #28
0
    def wait_extended_sys_state(self, vtol_state, landed_state):
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time() - tstart > 10:
                raise NotAchievedException("Did not achieve vol/landed states")
            self.progress("Waiting for MAV_VTOL_STATE=%s MAV_LANDED_STATE=%s" %
                          (self.vtol_state_name(vtol_state),
                           self.landed_state_name(landed_state)))
            m = self.assert_receive_message('EXTENDED_SYS_STATE', verbose=True)
            if m.landed_state != landed_state:
                self.progress("Wrong MAV_LANDED_STATE (want=%s got=%s)" %
                              (self.landed_state_name(landed_state),
                               self.landed_state_name(m.landed_state)))
                continue
            if m.vtol_state != vtol_state:
                self.progress("Wrong MAV_VTOL_STATE (want=%s got=%s)" %
                              (self.vtol_state_name(vtol_state),
                               self.vtol_state_name(m.vtol_state)))
                continue

            self.progress("vtol and landed states match")
            return
Beispiel #29
0
    def fly_heli_stabilize_takeoff(self):
        """"""
        self.context_push()

        ex = None
        try:
            self.change_mode('STABILIZE')
            self.set_rc(3, 1000)
            self.set_rc(8, 1000)
            self.wait_ready_to_arm()
            self.arm_vehicle()
            self.set_rc(8, 2000)
            self.progress("wait for rotor runup to complete")
            self.wait_servo_channel_value(8, 1660, timeout=10)
            self.delay_sim_time(20)
            # check we are still on the ground...
            m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
            if abs(m.relative_alt) > 100:
                raise NotAchievedException("Took off prematurely")
            self.progress("Pushing throttle past half-way")
            self.set_rc(3, 1600)

            self.progress("Monitoring takeoff")
            self.wait_altitude(6.9, 8, relative=True)

            self.progress("takeoff OK")
        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        self.land_and_disarm()
        self.set_rc(8, 1000)

        self.context_pop()

        if ex is not None:
            raise ex
Beispiel #30
0
    def drive_brake(self):
        old_using_brake = self.get_parameter('ATC_BRAKE')
        old_cruise_speed = self.get_parameter('CRUISE_SPEED')

        self.set_parameter('CRUISE_SPEED', 15)
        self.set_parameter('ATC_BRAKE', 0)

        distance_without_brakes = self.drive_brake_get_stopping_distance(15)

        # brakes on:
        self.set_parameter('ATC_BRAKE', 1)
        distance_with_brakes = self.drive_brake_get_stopping_distance(15)
        # revert state:
        self.set_parameter('ATC_BRAKE', old_using_brake)
        self.set_parameter('CRUISE_SPEED', old_cruise_speed)

        delta = distance_without_brakes - distance_with_brakes
        if delta < distance_without_brakes * 0.05:  # 5% isn't asking for much
            raise NotAchievedException("""
Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
""" % (distance_with_brakes, distance_without_brakes, delta))

        self.progress("Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" %
                      (distance_with_brakes, distance_without_brakes, delta))