def fly_flaps(self): """Test flaps functionality.""" filename = os.path.join(testdir, "flaps.txt") self.context_push() ex = None try: flaps_ch = 5 servo_ch = 5 self.set_parameter("SERVO%u_FUNCTION" % servo_ch, 3) # flapsauto self.set_parameter("FLAP_IN_CHANNEL", flaps_ch) self.set_parameter("LAND_FLAP_PERCNT", 50) self.set_parameter("LOG_DISARMED", 1) flaps_ch_min = 1000 flaps_ch_trim = 1500 flaps_ch_max = 2000 self.set_parameter("RC%u_MIN" % flaps_ch, flaps_ch_min) self.set_parameter("RC%u_MAX" % flaps_ch, flaps_ch_max) self.set_parameter("RC%u_TRIM" % flaps_ch, flaps_ch_trim) servo_ch_min = 1200 servo_ch_trim = 1300 servo_ch_max = 1800 self.set_parameter("SERVO%u_MIN" % servo_ch, servo_ch_min) self.set_parameter("SERVO%u_MAX" % servo_ch, servo_ch_max) self.set_parameter("SERVO%u_TRIM" % servo_ch, servo_ch_trim) # check flaps are not deployed: self.set_rc(flaps_ch, flaps_ch_min) self.wait_servo_channel_value(servo_ch, servo_ch_min) # deploy the flaps: self.set_rc(flaps_ch, flaps_ch_max) tstart = self.get_sim_time() self.wait_servo_channel_value(servo_ch, servo_ch_max) tstop = self.get_sim_time_cached() delta_time = tstop - tstart delta_time_min = 0.5 delta_time_max = 1.5 if delta_time < delta_time_min or delta_time > delta_time_max: raise NotAchievedException( ("Flaps Slew not working (%f seconds)" % (delta_time, ))) # undeploy flaps: self.set_rc(flaps_ch, flaps_ch_min) self.wait_servo_channel_value(servo_ch, servo_ch_min) self.progress("Flying mission %s" % filename) self.wp_load(filename) self.mavproxy.send('wp set 1\n') self.mavproxy.send('switch 1\n') # auto mode self.wait_mode('AUTO') self.wait_ready_to_arm() self.arm_vehicle() tstart = self.get_sim_time_cached() last_mission_current_msg = 0 last_seq = None while self.armed(): m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) time_delta = (self.get_sim_time_cached() - last_mission_current_msg) if (time_delta > 1 or m.seq != last_seq): dist = None x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None) if x is not None: dist = x.wp_dist self.progress("MISSION_CURRENT.seq=%u (dist=%s)" % (m.seq, str(dist))) last_mission_current_msg = self.get_sim_time_cached() last_seq = m.seq # flaps should undeploy at the end self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=30) # do a short flight in FBWA, watching for flaps # self.mavproxy.send('switch 4\n') # self.wait_mode('FBWA') # self.wait_seconds(10) # self.mavproxy.send('switch 6\n') # self.wait_mode('MANUAL') # self.wait_seconds(10) self.progress("Flaps OK") except Exception as e: ex = e self.context_pop() if ex: if self.armed(): self.disarm_vehicle() raise ex
def set_attitude_target(self): """Test setting of attitude target in guided mode.""" # mode guided: self.mavproxy.send('mode GUIDED\n') self.wait_mode('GUIDED') target_roll_degrees = 70 state_roll_over = "roll-over" state_stabilize_roll = "stabilize-roll" state_hold = "hold" state_roll_back = "roll-back" state_done = "done" tstart = self.get_sim_time() try: state = state_roll_over while state != state_done: if self.get_sim_time() - tstart > 20: raise AutoTestTimeoutException("Manuevers not completed") m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=0.1) if m is None: continue r = math.degrees(m.roll) if state == state_roll_over: target_roll_degrees = 70 if abs(r - target_roll_degrees) < 10: state = state_stabilize_roll stabilize_start = self.get_sim_time() elif state == state_stabilize_roll: # just give it a little time to sort it self out if self.get_sim_time() - stabilize_start > 2: state = state_hold hold_start = self.get_sim_time() elif state == state_hold: target_roll_degrees = 70 if self.get_sim_time() - hold_start > 10: state = state_roll_back if abs(r - target_roll_degrees) > 10: raise NotAchievedException("Failed to hold attitude") elif state == state_roll_back: target_roll_degrees = 0 if abs(r - target_roll_degrees) < 10: state = state_done else: raise ValueError("Unknown state %s" % str(state)) self.progress("%s Roll: %f desired=%f" % (state, r, target_roll_degrees)) time_boot_millis = 0 # FIXME target_system = 1 # FIXME target_component = 1 # FIXME type_mask = 0b10000001 ^ 0xFF # FIXME # attitude in radians: q = quaternion.Quaternion( [math.radians(target_roll_degrees), 0, 0]) roll_rate_radians = 0.5 pitch_rate_radians = 0 yaw_rate_radians = 0 thrust = 1.0 self.mav.mav.set_attitude_target_send( time_boot_millis, target_system, target_component, type_mask, q, roll_rate_radians, pitch_rate_radians, yaw_rate_radians, thrust) except Exception as e: self.mavproxy.send('mode FBWA\n') self.wait_mode('FBWA') self.set_rc(3, 1700) raise e # back to FBWA self.mavproxy.send('mode FBWA\n') self.wait_mode('FBWA') self.set_rc(3, 1700) self.wait_level_flight()
def test_FBWB(self, mode='FBWB'): """Fly FBWB or CRUISE mode.""" self.mavproxy.send("mode %s\n" % mode) self.wait_mode(mode) self.set_rc(3, 1700) self.set_rc(2, 1500) # lock in the altitude by asking for an altitude change then releasing self.set_rc(2, 1000) self.wait_distance(50, accuracy=20) self.set_rc(2, 1500) self.wait_distance(50, accuracy=20) m = self.mav.recv_match(type='VFR_HUD', blocking=True) initial_alt = m.alt self.progress("Initial altitude %u\n" % initial_alt) self.progress("Flying right circuit") # do 4 turns for i in range(0, 4): # hard left self.progress("Starting turn %u" % i) self.set_rc(1, 1800) try: self.wait_heading(0 + (90 * i), accuracy=20, timeout=60) except Exception as e: self.set_rc(1, 1500) raise e self.set_rc(1, 1500) self.progress("Starting leg %u" % i) self.wait_distance(100, accuracy=20) self.progress("Circuit complete") self.progress("Flying rudder left circuit") # do 4 turns for i in range(0, 4): # hard left self.progress("Starting turn %u" % i) self.set_rc(4, 1900) try: self.wait_heading(360 - (90 * i), accuracy=20, timeout=60) except Exception as e: self.set_rc(4, 1500) raise e self.set_rc(4, 1500) self.progress("Starting leg %u" % i) self.wait_distance(100, accuracy=20) self.progress("Circuit complete") m = self.mav.recv_match(type='VFR_HUD', blocking=True) final_alt = m.alt self.progress("Final altitude %u initial %u\n" % (final_alt, initial_alt)) # back to FBWA self.mavproxy.send('mode FBWA\n') self.wait_mode('FBWA') if abs(final_alt - initial_alt) > 20: raise NotAchievedException("Failed to maintain altitude") return self.wait_level_flight()
def drive_square(self, side=50): """Drive a square, Driving N then E .""" self.context_push() ex = None try: self.progress("TEST SQUARE") self.set_parameter("RC7_OPTION", 7) self.set_parameter("RC8_OPTION", 58) self.clear_wp() self.mavproxy.send('switch 5\n') self.wait_mode('MANUAL') self.wait_ready_to_arm() self.arm_vehicle() # first aim north self.progress("\nTurn right towards north") self.reach_heading_manual(10) # save bottom left corner of box as home AND waypoint self.progress("Save HOME") self.save_wp() self.progress("Save WP") self.save_wp() # pitch forward to fly north self.progress("\nGoing north %u meters" % side) self.reach_distance_manual(side) # save top left corner of square as waypoint self.progress("Save WP") self.save_wp() # roll right to fly east self.progress("\nGoing east %u meters" % side) self.reach_heading_manual(100) self.reach_distance_manual(side) # save top right corner of square as waypoint self.progress("Save WP") self.save_wp() # pitch back to fly south self.progress("\nGoing south %u meters" % side) self.reach_heading_manual(190) self.reach_distance_manual(side) # save bottom right corner of square as waypoint self.progress("Save WP") self.save_wp() # roll left to fly west self.progress("\nGoing west %u meters" % side) self.reach_heading_manual(280) self.reach_distance_manual(side) # save bottom left corner of square (should be near home) as waypoint self.progress("Save WP") self.save_wp() self.progress("Checking number of saved waypoints") num_wp = self.save_mission_to_file( os.path.join(testdir, "rover-ch7_mission.txt")) if num_wp != 6: raise NotAchievedException("Did not get 6 waypoints") # TODO: actually drive the mission self.clear_wp() except Exception as e: self.progress("Caught exception: %s" % str(e)) ex = e self.disarm_vehicle() self.context_pop() if ex: raise ex
def test_throttle_failsafe(self): self.change_mode('MANUAL') m = self.mav.recv_match(type='SYS_STATUS', blocking=True) receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER self.progress("Testing receiver enabled") if (not (m.onboard_control_sensors_enabled & receiver_bit)): raise PreconditionFailedException() self.progress("Testing receiver present") if (not (m.onboard_control_sensors_present & receiver_bit)): raise PreconditionFailedException() self.progress("Testing receiver health") if (not (m.onboard_control_sensors_health & receiver_bit)): raise PreconditionFailedException() self.progress("Ensure we know original throttle value") self.wait_rc_channel_value(3, 1000) self.set_parameter("THR_FS_VALUE", 960) self.progress("Failing receiver (throttle-to-950)") self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950 self.wait_mode('CIRCLE') # short failsafe self.wait_mode('RTL') # long failsafe self.progress("Ensure we've had our throttle squashed to 950") self.wait_rc_channel_value(3, 950) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) self.progress("Testing receiver enabled") if (not (m.onboard_control_sensors_enabled & receiver_bit)): raise NotAchievedException("Receiver not enabled") self.progress("Testing receiver present") if (not (m.onboard_control_sensors_present & receiver_bit)): raise NotAchievedException("Receiver not present") # skip this until RC is fixed # self.progress("Testing receiver health") # if (m.onboard_control_sensors_health & receiver_bit): # raise NotAchievedException("Sensor healthy when it shouldn't be") self.set_parameter("SIM_RC_FAIL", 0) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Testing receiver enabled") if (not (m.onboard_control_sensors_enabled & receiver_bit)): raise NotAchievedException("Receiver not enabled") self.progress("Testing receiver present") if (not (m.onboard_control_sensors_present & receiver_bit)): raise NotAchievedException("Receiver not present") self.progress("Testing receiver health") if (not (m.onboard_control_sensors_health & receiver_bit)): raise NotAchievedException("Receiver not healthy") self.change_mode('MANUAL') self.progress("Failing receiver (no-pulses)") self.set_parameter("SIM_RC_FAIL", 1) # no-pulses self.wait_mode('CIRCLE') # short failsafe self.wait_mode('RTL') # long failsafe m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) print("%s" % str(m)) self.progress("Testing receiver enabled") if (not (m.onboard_control_sensors_enabled & receiver_bit)): raise NotAchievedException("Receiver not enabled") self.progress("Testing receiver present") if (not (m.onboard_control_sensors_present & receiver_bit)): raise NotAchievedException("Receiver not present") self.progress("Testing receiver health") if (m.onboard_control_sensors_health & receiver_bit): raise NotAchievedException("Sensor healthy when it shouldn't be") self.set_parameter("SIM_RC_FAIL", 0) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Testing receiver enabled") if (not (m.onboard_control_sensors_enabled & receiver_bit)): raise NotAchievedException("Receiver not enabled") self.progress("Testing receiver present") if (not (m.onboard_control_sensors_present & receiver_bit)): raise NotAchievedException("Receiver not present") self.progress("Testing receiver health") if (not (m.onboard_control_sensors_health & receiver_bit)): raise NotAchievedException("Receiver not healthy") self.change_mode('MANUAL')
def fly_gps_glitch_auto_test(self, timeout=120): # set-up gps glitch array glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] glitch_num = len(glitch_lat) self.progress("GPS Glitches:") for i in range(1, glitch_num): self.progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) # Fly mission #1 self.progress("# Load copter_glitch_mission") # load the waypoint count global num_wp num_wp = self.load_mission("copter_glitch_mission.txt") if not num_wp: self.progress("load copter_glitch_mission failed") raise NotAchievedException() # turn on simulator display of gps and actual position if self.use_map: self.show_gps_and_sim_positions(True) self.progress("test: Fly a mission from 1 to %u" % num_wp) self.mavproxy.send('wp set 1\n') # switch into AUTO mode and raise throttle self.mavproxy.send('switch 4\n') # auto mode self.wait_mode('AUTO') self.set_rc(3, 1500) # wait until 100m from home try: self.wait_distance(100, 5, 60) except Exception as e: if self.use_map: self.show_gps_and_sim_positions(False) raise e # record time and position tstart = self.get_sim_time() # initialise current glitch glitch_current = 0 self.progress("Apply first glitch") self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) # record position for 30 seconds while glitch_current < glitch_num: tnow = self.get_sim_time() desired_glitch_num = int((tnow - tstart) * 2.2) if desired_glitch_num > glitch_current and glitch_current != -1: glitch_current = desired_glitch_num # apply next glitch if glitch_current < glitch_num: self.progress("Applying glitch %u" % glitch_current) self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) # turn off glitching self.progress("Completed Glitches") self.mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') # continue with the mission self.wait_waypoint(0, num_wp-1, timeout=500) # wait for arrival back home self.mav.recv_match(type='VFR_HUD', blocking=True) pos = self.mav.location() dist_to_home = self.get_distance(HOME, pos) while dist_to_home > 5: if self.get_sim_time() > (tstart + timeout): self.progress("GPS Glitch testing failed" "- exceeded timeout %u seconds" % timeout) raise AutoTestTimeoutException() self.mav.recv_match(type='VFR_HUD', blocking=True) pos = self.mav.location() dist_to_home = self.get_distance(HOME, pos) self.progress("Dist from home: %u" % dist_to_home) # turn off simulator display of gps and actual position if self.use_map: self.show_gps_and_sim_positions(False) self.progress("GPS Glitch test Auto completed: passed!")
def fly_motor_fail(self, fail_servo=0, fail_mul=0.0, holdtime=30): """Test flight with reduced motor efficiency""" # we only expect an octocopter to survive ATM: servo_counts = { # 2: 6, # hexa 3: 8, # octa # 5: 6, # Y6 } frame_class = int(self.get_parameter("FRAME_CLASS")) if frame_class not in servo_counts: self.progress("Test not relevant for frame_class %u" % frame_class) return servo_count = servo_counts[frame_class] if fail_servo < 0 or fail_servo > servo_count: raise ValueError('fail_servo outside range for frame class') self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') self.change_alt(alt_min=50) # Get initial values start_hud = self.mav.recv_match(type='VFR_HUD', blocking=True) start_attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) hover_time = 5 try: tstart = self.get_sim_time() int_error_alt = 0 int_error_yaw_rate = 0 int_error_yaw = 0 self.progress("Hovering for %u seconds" % hover_time) failed = False while self.get_sim_time() < tstart + holdtime + hover_time: ti = self.get_sim_time() servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True) hud = self.mav.recv_match(type='VFR_HUD', blocking=True) attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) if not failed and self.get_sim_time() - tstart > hover_time: self.progress("Killing motor %u (%u%%)" % (fail_servo+1, fail_mul)) self.set_parameter("SIM_ENGINE_FAIL", fail_servo) self.set_parameter("SIM_ENGINE_MUL", fail_mul) failed = True if failed: self.progress("Hold Time: %f/%f" % (self.get_sim_time()-tstart, holdtime)) servo_pwm = [servo.servo1_raw, servo.servo2_raw, servo.servo3_raw, servo.servo4_raw, servo.servo5_raw, servo.servo6_raw, servo.servo7_raw, servo.servo8_raw] self.progress("PWM output per motor") for i, pwm in enumerate(servo_pwm[0:servo_count]): if pwm > 1900: state = "oversaturated" elif pwm < 1200: state = "undersaturated" else: state = "OK" if failed and i==fail_servo: state += " (failed)" self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state)) alt_delta = hud.alt - start_hud.alt yawrate_delta = attitude.yawspeed - start_attitude.yawspeed yaw_delta = attitude.yaw - start_attitude.yaw self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta)) self.progress("Yaw rate=%f (delta=%f) (rad/s)" % (attitude.yawspeed, yawrate_delta)) self.progress("Yaw=%f (delta=%f) (deg)" % (attitude.yaw, yaw_delta)) dt = self.get_sim_time() - ti int_error_alt += abs(alt_delta/dt) int_error_yaw_rate += abs(yawrate_delta/dt) int_error_yaw += abs(yaw_delta/dt) self.progress("## Error Integration ##") self.progress(" Altitude: %fm" % int_error_alt) self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate) self.progress(" Yaw: %f deg" % int_error_yaw) self.progress("----") if alt_delta < -20: self.progress("Vehicle is descending") raise NotAchievedException() self.set_parameter("SIM_ENGINE_FAIL", 0) self.set_parameter("SIM_ENGINE_MUL", 1.0) except Exception as e: self.set_parameter("SIM_ENGINE_FAIL", 0) self.set_parameter("SIM_ENGINE_MUL", 1.0) raise e return True
def fly_heli_poshold_takeoff(self): """ensure vehicle stays put until it is ready to fly""" self.context_push() ex = None try: self.set_parameter("PILOT_TKOFF_ALT", 700) self.change_mode('POSHOLD') self.zero_throttle() self.set_rc(8, 1000) self.wait_ready_to_arm() # Arm self.arm_vehicle() self.progress("Raising rotor speed") 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) max_relalt_mm = 1000 if abs(m.relative_alt) > max_relalt_mm: raise NotAchievedException( "Took off prematurely (abs(%f)>%f)" % (m.relative_alt, max_relalt_mm)) self.progress("Pushing collective past half-way") self.set_rc(3, 1600) self.delay_sim_time(0.5) self.hover() # make sure we haven't already reached alt: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) max_initial_alt = 1500 if abs(m.relative_alt) > max_initial_alt: raise NotAchievedException( "Took off too fast (%f > %f" % (abs(m.relative_alt), max_initial_alt)) self.progress("Monitoring takeoff-to-alt") self.wait_altitude(6.9, 8, relative=True) self.progress("Making sure we stop at our takeoff altitude") tstart = self.get_sim_time() while self.get_sim_time() - tstart < 5: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) delta = abs(7000 - m.relative_alt) self.progress("alt=%f delta=%f" % (m.relative_alt / 1000, delta / 1000)) if delta > 1000: raise NotAchievedException( "Failed to maintain takeoff alt") 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
def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20): """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') # turn on simulator display of gps and actual position if self.use_map: self.show_gps_and_sim_positions(True) # set-up gps glitch array glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] glitch_num = len(glitch_lat) self.progress("GPS Glitches:") for i in range(1, glitch_num): self.progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) # turn south east self.progress("turn south east") self.set_rc(4, 1580) try: self.wait_heading(150) self.set_rc(4, 1500) # fly forward (south east) at least 60m self.set_rc(2, 1100) self.wait_distance(60) self.set_rc(2, 1500) # wait for copter to slow down except Exception as e: if self.use_map: self.show_gps_and_sim_positions(False) raise e # record time and position tstart = self.get_sim_time() tnow = tstart start_pos = self.sim_location() # initialise current glitch glitch_current = 0 self.progress("Apply first glitch") self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) # record position for 30 seconds while tnow < tstart + timeout: tnow = self.get_sim_time() desired_glitch_num = int((tnow - tstart) * 2.2) if desired_glitch_num > glitch_current and glitch_current != -1: glitch_current = desired_glitch_num # turn off glitching if we've reached the end of glitch list if glitch_current >= glitch_num: glitch_current = -1 self.progress("Completed Glitches") self.mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') else: self.progress("Applying glitch %u" % glitch_current) # move onto the next glitch self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) # start displaying distance moved after all glitches applied if glitch_current == -1: m = self.mav.recv_match(type='VFR_HUD', blocking=True) curr_pos = self.sim_location() moved_distance = self.get_distance(curr_pos, start_pos) self.progress("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) if moved_distance > max_distance: self.progress("Moved over %u meters, Failed!" % max_distance) raise NotAchievedException() # disable gps glitch if glitch_current != -1: glitch_current = -1 self.mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') if self.use_map: self.show_gps_and_sim_positions(False) self.progress("GPS glitch test passed!" " stayed within %u meters for %u seconds" % (max_distance, timeout))
def fly_gyro_fft(self): """Use dynamic harmonic notch to control motor noise.""" # basic gyro sample rate test self.progress("Flying with gyro FFT - Gyro sample rate") self.context_push() ex = None try: self.set_rc_default() # magic tridge EKF type that dramatically speeds up the test self.set_parameters({ "AHRS_EKF_TYPE": 10, "INS_LOG_BAT_MASK": 3, "INS_LOG_BAT_OPT": 0, "INS_GYRO_FILTER": 100, "LOG_BITMASK": 45054, "LOG_DISARMED": 0, "SIM_DRIFT_SPEED": 0, "SIM_DRIFT_TIME": 0, # enable a noisy motor peak "SIM_GYR1_RND": 20, # enabling FFT will also enable the arming check: self-testing the functionality "FFT_ENABLE": 1, "FFT_MINHZ": 80, "FFT_MAXHZ": 350, "FFT_SNR_REF": 10, "FFT_WINDOW_SIZE": 128, "FFT_WINDOW_OLAP": 0.75, }) # Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft # can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so # a 250Hz peak should be detectable within 5% self.set_parameters({ "SIM_VIB_FREQ_X": 250, "SIM_VIB_FREQ_Y": 250, "SIM_VIB_FREQ_Z": 250, }) self.reboot_sitl() # find a motor peak self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250) # Step 2: inject actual motor noise and use the standard length FFT to track it self.set_parameters({ "SIM_VIB_MOT_MAX": 350, "FFT_WINDOW_SIZE": 32, "FFT_WINDOW_OLAP": 0.5, }) self.reboot_sitl() # find a motor peak freq = self.hover_and_check_matched_frequency(-15, 200, 300, 32) # Step 3: add a FFT dynamic notch and check that the peak is squashed self.set_parameters({ "INS_LOG_BAT_OPT": 2, "INS_HNTCH_ENABLE": 1, "INS_HNTCH_FREQ": freq, "INS_HNTCH_REF": 1.0, "INS_HNTCH_ATT": 50, "INS_HNTCH_BW": freq / 2, "INS_HNTCH_MODE": 4, }) self.reboot_sitl() self.takeoff(10, mode="QHOVER") hover_time = 15 ignore_bins = 20 self.progress("Hovering for %u seconds" % hover_time) tstart = self.get_sim_time() while self.get_sim_time_cached() < tstart + hover_time: self.mav.recv_match(type='ATTITUDE', blocking=True) tend = self.get_sim_time() self.do_RTL() psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] peakdB = numpy.amax(psd["X"][ignore_bins:]) if peakdB < -10: self.progress("No motor peak, %f at %f dB" % (freq, peakdB)) else: raise NotAchievedException( "Detected peak at %f Hz of %.2f dB" % (freq, peakdB)) # Step 4: take off as a copter land as a plane, make sure we track self.progress("Flying with gyro FFT - vtol to plane") self.load_mission("quadplane-gyro-mission.txt") if self.mavproxy is not None: self.mavproxy.send('wp list\n') self.change_mode('AUTO') self.wait_ready_to_arm() self.arm_vehicle() self.wait_waypoint(1, 7, max_dist=60, timeout=1200) self.wait_disarmed( timeout=120) # give quadplane a long time to land # prevent update parameters from messing with the settings when we pop the context self.set_parameter("FFT_ENABLE", 0) self.reboot_sitl() except Exception as e: self.progress("Exception caught: %s" % (self.get_exception_stacktrace(e))) ex = e self.context_pop() self.reboot_sitl() if ex is not None: raise ex
def test_airmode(self): """Check that plane.air_mode turns on and off as required""" self.progress("########## Testing AirMode operation") self.set_parameter("AHRS_EKF_TYPE", 10) self.change_mode('QSTABILIZE') self.wait_ready_to_arm() """ SPIN_ARM and SPIN_MIN default to 0.10 and 0.15 when armed with zero throttle in AirMode, motor PWM should be at SPIN_MIN If AirMode is off, motor PWM will drop to SPIN_ARM """ self.progress("Verify that SERVO5 is Motor1 (default)") motor1_servo_function_lp = 33 if (self.get_parameter('SERVO5_FUNCTION') != motor1_servo_function_lp): raise PreconditionFailedException("SERVO5_FUNCTION not %d" % motor1_servo_function_lp) self.progress("Verify that flightmode channel is 5 (default)") default_fltmode_ch = 5 if (self.get_parameter("FLTMODE_CH") != default_fltmode_ch): raise PreconditionFailedException("FLTMODE_CH not %d" % default_fltmode_ch) """When disarmed, motor PWM will drop to min_pwm""" min_pwm = self.get_parameter("Q_THR_MIN_PWM") self.progress("Verify Motor1 is at min_pwm when disarmed") self.wait_servo_channel_value(5, min_pwm, comparator=operator.eq) armdisarm_option = 154 arm_ch = 8 self.set_parameter("RC%d_OPTION" % arm_ch, armdisarm_option) self.progress("Configured RC%d as ARMDISARM switch" % arm_ch) """arm with GCS, record Motor1 SPIN_ARM PWM output and disarm""" spool_delay = self.get_parameter("Q_M_SPOOL_TIME") + 0.25 self.zero_throttle() self.arm_vehicle() self.progress("Waiting for Motor1 to spool up to SPIN_ARM") self.delay_sim_time(spool_delay) spin_arm_pwm = self.wait_servo_channel_value(5, min_pwm, comparator=operator.gt) self.progress("spin_arm_pwm: %d" % spin_arm_pwm) self.disarm_vehicle() """arm with switch, record Motor1 SPIN_MIN PWM output and disarm""" self.set_rc(8, 2000) self.delay_sim_time(spool_delay) self.progress("Waiting for Motor1 to spool up to SPIN_MIN") spin_min_pwm = self.wait_servo_channel_value(5, spin_arm_pwm, comparator=operator.gt) self.progress("spin_min_pwm: %d" % spin_min_pwm) self.set_rc(8, 1000) if (spin_arm_pwm >= spin_min_pwm): raise PreconditionFailedException( "SPIN_MIN pwm not greater than SPIN_ARM pwm") self.start_subtest("Test auxswitch arming with AirMode Switch") for mode in ('QSTABILIZE', 'QACRO'): """verify that arming with switch results in higher PWM output""" self.progress("Testing %s mode" % mode) self.change_mode(mode) self.zero_throttle() self.progress("Arming with switch at zero throttle") self.arm_motors_with_switch(arm_ch) self.progress("Waiting for Motor1 to speed up") self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge) self.progress("Verify that rudder disarm is disabled") try: self.disarm_motors_with_rc_input() except NotAchievedException: pass if not self.armed(): raise NotAchievedException("Rudder disarm not disabled") self.progress("Disarming with switch") self.disarm_motors_with_switch(arm_ch) self.progress("Waiting for Motor1 to stop") self.wait_servo_channel_value(5, min_pwm, comparator=operator.le) self.wait_ready_to_arm() self.start_subtest( "Verify that arming with switch does not spin motors in other modes" ) # introduce a large attitude error to verify that stabilization is not active ahrs_trim_x = self.get_parameter("AHRS_TRIM_X") self.set_parameter("AHRS_TRIM_X", math.radians(-60)) self.wait_roll(60, 1) # test all modes except QSTABILIZE, QACRO, AUTO and QAUTOTUNE and QLAND and QRTL # QRTL and QLAND aren't tested because we can't arm in that mode for mode in ( 'ACRO', 'AUTOTUNE', 'AVOID_ADSB', 'CIRCLE', 'CRUISE', 'FBWA', 'FBWB', 'GUIDED', 'LOITER', 'QHOVER', 'QLOITER', 'RTL', 'STABILIZE', 'TRAINING', ): self.progress("Testing %s mode" % mode) self.change_mode(mode) self.zero_throttle() self.progress("Arming with switch at zero throttle") self.arm_motors_with_switch(arm_ch) self.progress("Waiting for Motor1 to (not) speed up") self.delay_sim_time(spool_delay) self.wait_servo_channel_value(5, spin_arm_pwm, comparator=operator.le) self.wait_servo_channel_value(6, spin_arm_pwm, comparator=operator.le) self.wait_servo_channel_value(7, spin_arm_pwm, comparator=operator.le) self.wait_servo_channel_value(8, spin_arm_pwm, comparator=operator.le) self.progress("Disarming with switch") self.disarm_motors_with_switch(arm_ch) self.progress("Waiting for Motor1 to stop") self.wait_servo_channel_value(5, min_pwm, comparator=operator.le) self.wait_ready_to_arm() # remove attitude error self.set_parameter("AHRS_TRIM_X", ahrs_trim_x) self.start_subtest( "verify that AIRMODE auxswitch turns airmode on/off while armed") """set RC7_OPTION to AIRMODE""" option_airmode = 84 self.set_parameter("RC7_OPTION", option_airmode) for mode in ('QSTABILIZE', 'QACRO'): self.progress("Testing %s mode" % mode) self.change_mode(mode) self.zero_throttle() self.progress("Arming with GCS at zero throttle") self.arm_vehicle() self.progress("Turn airmode on with auxswitch") self.set_rc(7, 2000) self.progress("Waiting for Motor1 to speed up") self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge) self.progress("Turn airmode off with auxswitch") self.set_rc(7, 1000) self.progress("Waiting for Motor1 to slow down") self.wait_servo_channel_value(5, spin_arm_pwm, comparator=operator.le) self.disarm_vehicle() self.wait_ready_to_arm() self.start_subtest("Test GCS arming") for mode in ('QSTABILIZE', 'QACRO'): self.progress("Testing %s mode" % mode) self.change_mode(mode) self.zero_throttle() self.progress("Arming with GCS at zero throttle") self.arm_vehicle() self.progress("Turn airmode on with auxswitch") self.set_rc(7, 2000) self.progress("Waiting for Motor1 to speed up") self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge) self.progress("Disarm/rearm with GCS") self.disarm_vehicle() self.arm_vehicle() self.progress("Verify that airmode is still on") self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge) self.disarm_vehicle() self.wait_ready_to_arm()
def fly_gyro_fft(self): """Use dynamic harmonic notch to control motor noise.""" # basic gyro sample rate test self.progress("Flying with gyro FFT - Gyro sample rate") self.context_push() ex = None try: self.set_rc_default() # magic tridge EKF type that dramatically speeds up the test self.set_parameter("AHRS_EKF_TYPE", 10) self.set_parameter("INS_LOG_BAT_MASK", 3) self.set_parameter("INS_LOG_BAT_OPT", 0) self.set_parameter("INS_GYRO_FILTER", 100) self.set_parameter("LOG_BITMASK", 45054) self.set_parameter("LOG_DISARMED", 0) self.set_parameter("SIM_DRIFT_SPEED", 0) self.set_parameter("SIM_DRIFT_TIME", 0) # enable a noisy motor peak self.set_parameter("SIM_GYR_RND", 20) # enabling FFT will also enable the arming check, self-testing the functionality self.set_parameter("FFT_ENABLE", 1) self.set_parameter("FFT_MINHZ", 80) self.set_parameter("FFT_MAXHZ", 350) self.set_parameter("FFT_SNR_REF", 10) self.set_parameter("FFT_WINDOW_SIZE", 128) self.set_parameter("FFT_WINDOW_OLAP", 0.75) # Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft # can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so # a 250Hz peak should be detectable within 5% self.set_parameter("SIM_VIB_FREQ_X", 250) self.set_parameter("SIM_VIB_FREQ_Y", 250) self.set_parameter("SIM_VIB_FREQ_Z", 250) self.reboot_sitl() # find a motor peak self.hover_and_check_matched_frequency(-15, 100, 350, 250) # Step 2: inject actual motor noise and use the standard length FFT to track it self.set_parameter("SIM_VIB_MOT_MAX", 350) self.set_parameter("FFT_WINDOW_SIZE", 32) self.set_parameter("FFT_WINDOW_OLAP", 0.5) self.reboot_sitl() # find a motor peak freq = self.hover_and_check_matched_frequency(-15, 200, 300) # Step 3: add a FFT dynamic notch and check that the peak is squashed self.set_parameter("INS_LOG_BAT_OPT", 2) self.set_parameter("INS_HNTCH_ENABLE", 1) self.set_parameter("INS_HNTCH_FREQ", freq) self.set_parameter("INS_HNTCH_REF", 1.0) self.set_parameter("INS_HNTCH_ATT", 50) self.set_parameter("INS_HNTCH_BW", freq/2) self.set_parameter("INS_HNTCH_MODE", 4) self.reboot_sitl() self.takeoff(10, mode="QHOVER") hover_time = 15 ignore_bins = 20 self.progress("Hovering for %u seconds" % hover_time) tstart = self.get_sim_time() while self.get_sim_time_cached() < tstart + hover_time: self.mav.recv_match(type='ATTITUDE', blocking=True) tend = self.get_sim_time() self.do_RTL() psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] if numpy.amax(psd["X"][ignore_bins:]) < -15: self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:]))) else: raise NotAchievedException("Detected motor peak at %f Hz" % (freq)) # Step 4: take off as a copter land as a plane, make sure we track self.progress("Flying with gyro FFT - vtol to plane") self.load_mission(os.path.join(testdir, GYRO_MISSION)) self.mavproxy.send('wp list\n') self.mavproxy.expect('Requesting [0-9]+ waypoints') self.wait_ready_to_arm() self.arm_vehicle() self.mavproxy.send('mode AUTO\n') self.wait_mode('AUTO') self.wait_waypoint(1, 7, max_dist=60, timeout=1200) self.mav.motors_disarmed_wait() # prevent update parameters from messing with the settings when we pop the context self.set_parameter("FFT_ENABLE", 0) self.reboot_sitl() except Exception as e: self.progress("Exception caught: %s" % ( self.get_exception_stacktrace(e))) ex = e self.context_pop() self.reboot_sitl() if ex is not None: raise ex
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 scale = 1000. / 1024. sminhz = int(minhz * scale) smaxhz = int(maxhz * scale) freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz] peakdb = numpy.amax(psd["X"][sminhz:smaxhz]) 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 freqs = [] 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 freqs.append(m.PkAvg) # peak within resolution of FFT length pkAvg = numpy.median(numpy.asarray(freqs)) 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
def test_alt_hold(self): """Test ALT_HOLD mode """ self.wait_ready_to_arm() self.arm_vehicle() self.change_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() # Make sure the code can handle buoyancy changes self.set_parameter("SIM_BUOYANCY", 10) self.watch_altitude_maintained() self.set_parameter("SIM_BUOYANCY", -10) self.watch_altitude_maintained() # Make sure that the ROV will dive with a small input down even if there is a 10N buoyancy force upwards self.set_parameter("SIM_BUOYANCY", 10) self.set_rc(Joystick.Throttle, 1350) self.wait_altitude(altitude_min=-6, altitude_max=-5.5) self.set_rc(Joystick.Throttle, 1500) self.watch_altitude_maintained() self.disarm_vehicle()
def set_attitude_target(self, tolerance=10): """Test setting of attitude target in guided mode.""" self.change_mode("GUIDED") # self.set_parameter("STALL_PREVENTION", 0) state_roll_over = "roll-over" state_stabilize_roll = "stabilize-roll" state_hold = "hold" state_roll_back = "roll-back" state_done = "done" tstart = self.get_sim_time() try: state = state_roll_over while state != state_done: m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=0.1) now = self.get_sim_time_cached() if now - tstart > 20: raise AutoTestTimeoutException("Manuevers not completed") if m is None: continue r = math.degrees(m.roll) if state == state_roll_over: target_roll_degrees = 60 if abs(r - target_roll_degrees) < tolerance: state = state_stabilize_roll stabilize_start = now elif state == state_stabilize_roll: # just give it a little time to sort it self out if now - stabilize_start > 2: state = state_hold hold_start = now elif state == state_hold: target_roll_degrees = 60 if now - hold_start > tolerance: state = state_roll_back if abs(r - target_roll_degrees) > tolerance: raise NotAchievedException("Failed to hold attitude") elif state == state_roll_back: target_roll_degrees = 0 if abs(r - target_roll_degrees) < tolerance: state = state_done else: raise ValueError("Unknown state %s" % str(state)) m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT'] self.progress("%s Roll: %f desired=%f set=%f" % (state, r, m_nav.nav_roll, target_roll_degrees)) time_boot_millis = 0 # FIXME target_system = 1 # FIXME target_component = 1 # FIXME type_mask = 0b10000001 ^ 0xFF # FIXME # attitude in radians: q = quaternion.Quaternion( [math.radians(target_roll_degrees), 0, 0]) roll_rate_radians = 0.5 pitch_rate_radians = 0 yaw_rate_radians = 0 thrust = 1.0 self.mav.mav.set_attitude_target_send( time_boot_millis, target_system, target_component, type_mask, q, roll_rate_radians, pitch_rate_radians, yaw_rate_radians, thrust) except Exception as e: self.mavproxy.send('mode FBWA\n') self.wait_mode('FBWA') self.set_rc(3, 1700) raise e # back to FBWA self.mavproxy.send('mode FBWA\n') self.wait_mode('FBWA') self.set_rc(3, 1700) self.wait_level_flight()
def fly_do_change_speed(self): # the following lines ensure we revert these parameter values # - DO_CHANGE_AIRSPEED is a permanent vehicle change! self.set_parameter("TRIM_ARSPD_CM", self.get_parameter("TRIM_ARSPD_CM")) self.set_parameter("MIN_GNDSPD_CM", self.get_parameter("MIN_GNDSPD_CM")) self.progress("Takeoff") self.takeoff(alt=100) self.set_rc(3, 1500) # ensure we know what the airspeed is: self.progress("Entering guided and flying somewhere constant") self.change_mode("GUIDED") self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, 0, 0, 0, 0, 12345, # lat*1e7 12345, # lon*1e7 100 # alt ) self.delay_sim_time(10) self.progress("Ensuring initial speed is known and relatively constant") initial_speed = 21.5; timeout = 10 tstart = self.get_sim_time() while True: if self.get_sim_time_cached() - tstart > timeout: break m = self.mav.recv_match(type='VFR_HUD', blocking=True) self.progress("GroundSpeed: %f want=%f" % (m.groundspeed, initial_speed)) if abs(initial_speed - m.groundspeed) > 1: raise NotAchievedException("Initial speed not as expected (want=%f got=%f" % (initial_speed, m.groundspeed)) self.progress("Setting groundspeed") new_target_groundspeed = initial_speed + 5 self.run_cmd( mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 1, # groundspeed new_target_groundspeed, -1, # throttle / no change 0, # absolute values 0, 0, 0) self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) self.progress("Adding some wind, ensuring groundspeed holds") self.set_parameter("SIM_WIND_SPD", 5) self.delay_sim_time(5) self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) self.set_parameter("SIM_WIND_SPD", 0) self.progress("Setting airspeed") new_target_airspeed = initial_speed + 5 self.run_cmd( mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, # airspeed new_target_airspeed, -1, # throttle / no change 0, # absolute values 0, 0, 0) self.wait_groundspeed(new_target_airspeed-0.5, new_target_airspeed+0.5) self.progress("Adding some wind, hoping groundspeed increases/decreases") self.set_parameter("SIM_WIND_SPD", 5) self.set_parameter("SIM_WIND_DIR", 270) self.delay_sim_time(5) timeout = 10 tstart = self.get_sim_time() while True: if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Did not achieve groundspeed delta") m = self.mav.recv_match(type='VFR_HUD', blocking=True) delta = abs(m.airspeed - m.groundspeed) want_delta = 4 self.progress("groundspeed and airspeed should be different (have=%f want=%f)" % (delta, want_delta)) if delta > want_delta: break filename = os.path.join(testdir, "flaps.txt") self.progress("Using %s to fly home" % filename) self.load_mission(filename) self.change_mode("AUTO") self.mavproxy.send('wp set 7\n') self.mav.motors_disarmed_wait()