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()
