2021-09-26 21:24:57 -03:00
|
|
|
'''
|
|
|
|
Fly Helicopter in SITL
|
|
|
|
|
|
|
|
AP_FLAKE8_CLEAN
|
|
|
|
'''
|
|
|
|
|
|
|
|
from __future__ import print_function
|
2022-02-23 08:07:15 -04:00
|
|
|
|
2021-09-26 21:24:57 -03:00
|
|
|
from arducopter import AutoTestCopter
|
2022-02-23 08:07:15 -04:00
|
|
|
|
|
|
|
import vehicle_test_suite
|
|
|
|
from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException
|
2021-09-26 21:24:57 -03:00
|
|
|
|
|
|
|
from pymavlink import mavutil
|
|
|
|
from pysim import vehicleinfo
|
|
|
|
|
2021-03-11 04:30:55 -04:00
|
|
|
import copy
|
2024-10-03 03:17:03 -03:00
|
|
|
import operator
|
2021-03-11 04:30:55 -04:00
|
|
|
|
2021-09-26 21:24:57 -03:00
|
|
|
|
|
|
|
class AutoTestHelicopter(AutoTestCopter):
|
|
|
|
|
2021-10-01 05:43:37 -03:00
|
|
|
sitl_start_loc = mavutil.location(40.072842, -105.230575, 1586, 0) # Sparkfun AVC Location
|
|
|
|
|
2021-09-26 21:24:57 -03:00
|
|
|
def vehicleinfo_key(self):
|
|
|
|
return 'Helicopter'
|
|
|
|
|
|
|
|
def log_name(self):
|
|
|
|
return "HeliCopter"
|
|
|
|
|
|
|
|
def default_frame(self):
|
|
|
|
return "heli"
|
|
|
|
|
|
|
|
def sitl_start_location(self):
|
2021-10-01 05:43:37 -03:00
|
|
|
return self.sitl_start_loc
|
2021-09-26 21:24:57 -03:00
|
|
|
|
|
|
|
def default_speedup(self):
|
|
|
|
'''Heli seems to be race-free'''
|
|
|
|
return 100
|
|
|
|
|
|
|
|
def is_heli(self):
|
|
|
|
return True
|
|
|
|
|
|
|
|
def rc_defaults(self):
|
|
|
|
ret = super(AutoTestHelicopter, self).rc_defaults()
|
|
|
|
ret[8] = 1000
|
|
|
|
ret[3] = 1000 # collective
|
|
|
|
return ret
|
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_position_armable_modes_list():
|
|
|
|
'''filter THROW mode out of armable modes list; Heli is special-cased'''
|
|
|
|
ret = AutoTestCopter.get_position_armable_modes_list()
|
|
|
|
ret = filter(lambda x : x != "THROW", ret)
|
|
|
|
return ret
|
|
|
|
|
|
|
|
def loiter_requires_position(self):
|
|
|
|
self.progress("Skipping loiter-requires-position for heli; rotor runup issues")
|
|
|
|
|
|
|
|
def get_collective_out(self):
|
|
|
|
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True)
|
|
|
|
chan_pwm = (servo.servo1_raw + servo.servo2_raw + servo.servo3_raw)/3.0
|
|
|
|
return chan_pwm
|
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def RotorRunup(self):
|
|
|
|
'''Test rotor runip'''
|
2021-09-26 21:24:57 -03:00
|
|
|
# Takeoff and landing in Loiter
|
|
|
|
TARGET_RUNUP_TIME = 10
|
|
|
|
self.zero_throttle()
|
|
|
|
self.change_mode('LOITER')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True)
|
|
|
|
coll = servo.servo1_raw
|
|
|
|
coll = coll + 50
|
|
|
|
self.set_parameter("H_RSC_RUNUP_TIME", TARGET_RUNUP_TIME)
|
|
|
|
self.progress("Initiate Runup by putting some throttle")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.set_rc(3, 1700)
|
|
|
|
self.progress("Collective threshold PWM %u" % coll)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.progress("Wait that collective PWM pass threshold value")
|
|
|
|
servo = self.mav.recv_match(condition='SERVO_OUTPUT_RAW.servo1_raw>%u' % coll, blocking=True)
|
|
|
|
runup_time = self.get_sim_time() - tstart
|
|
|
|
self.progress("Collective is now at PWM %u" % servo.servo1_raw)
|
|
|
|
self.mav.wait_heartbeat()
|
|
|
|
if runup_time < TARGET_RUNUP_TIME:
|
|
|
|
self.zero_throttle()
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
self.disarm_vehicle()
|
|
|
|
self.mav.wait_heartbeat()
|
|
|
|
raise NotAchievedException("Takeoff initiated before runup time complete %u" % runup_time)
|
|
|
|
self.progress("Runup time %u" % runup_time)
|
|
|
|
self.zero_throttle()
|
|
|
|
self.land_and_disarm()
|
|
|
|
self.mav.wait_heartbeat()
|
|
|
|
|
|
|
|
# fly_avc_test - fly AVC mission
|
2022-09-09 22:24:28 -03:00
|
|
|
def AVCMission(self):
|
|
|
|
'''fly AVC mission'''
|
2021-09-26 21:24:57 -03:00
|
|
|
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!")
|
|
|
|
|
|
|
|
def takeoff(self,
|
|
|
|
alt_min=30,
|
|
|
|
takeoff_throttle=1700,
|
|
|
|
require_absolute=True,
|
|
|
|
mode="STABILIZE",
|
|
|
|
timeout=120):
|
|
|
|
"""Takeoff get to 30m altitude."""
|
|
|
|
self.progress("TAKEOFF")
|
|
|
|
self.change_mode(mode)
|
|
|
|
if not self.armed():
|
|
|
|
self.wait_ready_to_arm(require_absolute=require_absolute, timeout=timeout)
|
|
|
|
self.zero_throttle()
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
|
|
|
self.progress("Raising rotor speed")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.progress("wait for rotor runup to complete")
|
2023-05-30 00:38:27 -03:00
|
|
|
if self.get_parameter("H_RSC_MODE") == 4:
|
|
|
|
self.context_collect('STATUSTEXT')
|
|
|
|
self.wait_statustext("Governor Engaged", check_context=True)
|
|
|
|
elif self.get_parameter("H_RSC_MODE") == 3:
|
|
|
|
self.wait_rpm(1, 1300, 1400)
|
|
|
|
else:
|
|
|
|
self.wait_servo_channel_value(8, 1659, timeout=10)
|
2023-02-11 00:37:27 -04:00
|
|
|
|
|
|
|
# wait for motor runup
|
|
|
|
self.delay_sim_time(20)
|
2021-09-26 21:24:57 -03:00
|
|
|
|
|
|
|
if mode == 'GUIDED':
|
|
|
|
self.user_takeoff(alt_min=alt_min)
|
|
|
|
else:
|
|
|
|
self.set_rc(3, takeoff_throttle)
|
2023-08-04 07:55:44 -03:00
|
|
|
self.wait_altitude(alt_min-1, alt_min+5, relative=True, timeout=timeout)
|
2021-09-26 21:24:57 -03:00
|
|
|
self.hover()
|
|
|
|
self.progress("TAKEOFF COMPLETE")
|
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def FlyEachFrame(self):
|
|
|
|
'''Fly each supported internal frame'''
|
2021-09-26 21:24:57 -03:00
|
|
|
vinfo = vehicleinfo.VehicleInfo()
|
|
|
|
vinfo_options = vinfo.options[self.vehicleinfo_key()]
|
|
|
|
known_broken_frames = {
|
|
|
|
}
|
|
|
|
for frame in sorted(vinfo_options["frames"].keys()):
|
|
|
|
self.start_subtest("Testing frame (%s)" % str(frame))
|
|
|
|
if frame in known_broken_frames:
|
|
|
|
self.progress("Actually, no I'm not - it is known-broken (%s)" %
|
|
|
|
(known_broken_frames[frame]))
|
|
|
|
continue
|
|
|
|
frame_bits = vinfo_options["frames"][frame]
|
|
|
|
print("frame_bits: %s" % str(frame_bits))
|
|
|
|
if frame_bits.get("external", False):
|
|
|
|
self.progress("Actually, no I'm not - it is an external simulation")
|
|
|
|
continue
|
|
|
|
model = frame_bits.get("model", frame)
|
|
|
|
# the model string for Callisto has crap in it.... we
|
|
|
|
# should really have another entry in the vehicleinfo data
|
|
|
|
# to carry the path to the JSON.
|
|
|
|
actual_model = model.split(":")[0]
|
|
|
|
defaults = self.model_defaults_filepath(actual_model)
|
2023-08-31 18:45:12 -03:00
|
|
|
if not isinstance(defaults, list):
|
2021-09-26 21:24:57 -03:00
|
|
|
defaults = [defaults]
|
|
|
|
self.customise_SITL_commandline(
|
2024-04-17 22:12:37 -03:00
|
|
|
[],
|
|
|
|
defaults_filepath=defaults,
|
2021-09-26 21:24:57 -03:00
|
|
|
model=model,
|
|
|
|
wipe=True,
|
|
|
|
)
|
|
|
|
self.takeoff(10)
|
|
|
|
self.do_RTL()
|
|
|
|
|
2023-05-30 00:38:27 -03:00
|
|
|
def governortest(self):
|
|
|
|
'''Test Heli Internal Throttle Curve and Governor'''
|
|
|
|
self.customise_SITL_commandline(
|
2024-04-17 22:12:37 -03:00
|
|
|
[],
|
|
|
|
defaults_filepath=self.model_defaults_filepath('heli-gas'),
|
2023-05-30 00:38:27 -03:00
|
|
|
model="heli-gas",
|
|
|
|
wipe=True,
|
|
|
|
)
|
|
|
|
self.set_parameter("H_RSC_MODE", 4)
|
|
|
|
self.takeoff(10)
|
|
|
|
self.do_RTL()
|
|
|
|
|
2021-09-26 21:24:57 -03:00
|
|
|
def hover(self):
|
|
|
|
self.progress("Setting hover collective")
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def PosHoldTakeOff(self):
|
2021-09-26 21:24:57 -03:00
|
|
|
"""ensure vehicle stays put until it is ready to fly"""
|
2024-07-26 10:56:24 -03:00
|
|
|
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, 1659, timeout=10)
|
|
|
|
self.delay_sim_time(20)
|
|
|
|
# check we are still on the ground...
|
|
|
|
max_relalt = 1 # metres
|
|
|
|
relative_alt = self.get_altitude(relative=True)
|
|
|
|
if abs(relative_alt) > max_relalt:
|
|
|
|
raise NotAchievedException("Took off prematurely (abs(%f)>%f)" %
|
|
|
|
(relative_alt, max_relalt))
|
|
|
|
|
|
|
|
self.progress("Pushing collective past half-way")
|
|
|
|
self.set_rc(3, 1600)
|
|
|
|
self.delay_sim_time(0.5)
|
|
|
|
self.hover()
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
# make sure we haven't already reached alt:
|
|
|
|
relative_alt = self.get_altitude(relative=True)
|
|
|
|
max_initial_alt = 1.5 # metres
|
|
|
|
if abs(relative_alt) > max_initial_alt:
|
|
|
|
raise NotAchievedException("Took off too fast (%f > %f" %
|
|
|
|
(abs(relative_alt), max_initial_alt))
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
self.progress("Monitoring takeoff-to-alt")
|
|
|
|
self.wait_altitude(6, 8, relative=True, minimum_duration=5)
|
|
|
|
self.progress("takeoff OK")
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
self.land_and_disarm()
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def StabilizeTakeOff(self):
|
|
|
|
"""Fly stabilize takeoff"""
|
2024-07-26 10:56:24 -03:00
|
|
|
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, 1659, timeout=10)
|
|
|
|
self.delay_sim_time(20)
|
|
|
|
# check we are still on the ground...
|
|
|
|
relative_alt = self.get_altitude(relative=True)
|
|
|
|
if abs(relative_alt) > 0.1:
|
|
|
|
raise NotAchievedException("Took off prematurely")
|
|
|
|
self.progress("Pushing throttle past half-way")
|
|
|
|
self.set_rc(3, 1650)
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
self.progress("Monitoring takeoff")
|
|
|
|
self.wait_altitude(6.9, 8, relative=True)
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
self.progress("takeoff OK")
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
self.land_and_disarm()
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def SplineWaypoint(self, timeout=600):
|
2021-09-26 21:24:57 -03:00
|
|
|
"""ensure basic spline functionality works"""
|
|
|
|
self.load_mission("copter_spline_mission.txt", strict=False)
|
|
|
|
self.change_mode("LOITER")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.progress("Raising rotor speed")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.delay_sim_time(20)
|
|
|
|
self.change_mode("AUTO")
|
|
|
|
self.set_rc(3, 1500)
|
2024-07-26 10:56:24 -03:00
|
|
|
self.wait_disarmed(timeout=600)
|
2021-09-26 21:24:57 -03:00
|
|
|
self.progress("Lowering rotor speed")
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
def Autorotation(self, timeout=600):
|
2022-09-09 22:24:28 -03:00
|
|
|
"""Check engine-out behaviour"""
|
2024-08-10 13:22:40 -03:00
|
|
|
self.context_push()
|
2021-09-26 21:24:57 -03:00
|
|
|
start_alt = 100 # metres
|
2024-08-10 13:22:40 -03:00
|
|
|
self.set_parameters({
|
|
|
|
"AROT_ENABLE": 1,
|
|
|
|
"H_RSC_AROT_ENBL": 1,
|
|
|
|
})
|
|
|
|
bail_out_time = self.get_parameter('H_RSC_AROT_RUNUP')
|
2021-09-26 21:24:57 -03:00
|
|
|
self.change_mode('POSHOLD')
|
|
|
|
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")
|
2023-02-11 00:37:27 -04:00
|
|
|
self.wait_servo_channel_value(8, 1659, timeout=10)
|
2021-09-26 21:24:57 -03:00
|
|
|
self.delay_sim_time(20)
|
|
|
|
self.set_rc(3, 2000)
|
|
|
|
self.wait_altitude(start_alt - 1,
|
|
|
|
(start_alt + 5),
|
|
|
|
relative=True,
|
|
|
|
timeout=timeout)
|
|
|
|
self.context_collect('STATUSTEXT')
|
2024-08-10 13:22:40 -03:00
|
|
|
|
|
|
|
# Reset collective to enter hover
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
# Change to the autorotation flight mode
|
|
|
|
self.progress("Triggering autorotate mode")
|
|
|
|
self.change_mode('AUTOROTATE')
|
|
|
|
self.delay_sim_time(2)
|
|
|
|
|
|
|
|
# Disengage the interlock to remove power
|
2021-09-26 21:24:57 -03:00
|
|
|
self.set_rc(8, 1000)
|
2024-04-25 00:21:26 -03:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
# Ensure we have progressed through the mode's state machine
|
2021-09-26 21:24:57 -03:00
|
|
|
self.wait_statustext("SS Glide Phase", check_context=True)
|
2024-04-25 00:21:26 -03:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
self.progress("Testing bailout from autorotation")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
# See if the output ramps to a value close to expected with the prescribed time
|
|
|
|
self.wait_servo_channel_value(8, 1659, timeout=bail_out_time+1, comparator=operator.ge)
|
|
|
|
|
|
|
|
# Successfully bailed out, disengage the interlock and allow autorotation to progress
|
|
|
|
self.set_rc(8, 1000)
|
2021-09-26 21:24:57 -03:00
|
|
|
self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
|
|
|
|
check_context=True,
|
|
|
|
regex=True)
|
|
|
|
speed = float(self.re_match.group(1))
|
|
|
|
if speed > 30:
|
|
|
|
raise NotAchievedException("Hit too hard")
|
2024-08-10 13:22:40 -03:00
|
|
|
|
|
|
|
# Set throttle low to trip auto disarm
|
|
|
|
self.set_rc(3, 1000)
|
|
|
|
|
2021-09-26 21:24:57 -03:00
|
|
|
self.wait_disarmed()
|
2024-08-10 13:22:40 -03:00
|
|
|
self.context_pop()
|
2021-09-26 21:24:57 -03:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
def ManAutorotation(self, timeout=600):
|
2023-02-16 23:17:01 -04:00
|
|
|
"""Check autorotation power recovery behaviour"""
|
2024-08-10 13:22:40 -03:00
|
|
|
RSC_CHAN = 8
|
|
|
|
|
|
|
|
def check_rsc_output(self, throttle, timeout):
|
|
|
|
# Check we get a sensible throttle output
|
|
|
|
expected_pwm = int(throttle * 0.01 * 1000 + 1000)
|
|
|
|
|
|
|
|
# Help out the detection by accepting some margin
|
|
|
|
margin = 2
|
|
|
|
|
|
|
|
# See if the output ramps to a value close to expected with the prescribed time
|
|
|
|
self.wait_servo_channel_in_range(RSC_CHAN, expected_pwm-margin, expected_pwm+margin, timeout=timeout)
|
|
|
|
|
|
|
|
def TestAutorotationConfig(self, rsc_idle, arot_ramp_time, arot_idle, cool_down):
|
|
|
|
RAMP_TIME = 10
|
|
|
|
RUNUP_TIME = 15
|
|
|
|
AROT_RUNUP_TIME = arot_ramp_time + 4
|
|
|
|
RSC_SETPOINT = 66
|
|
|
|
self.set_parameters({
|
|
|
|
"H_RSC_AROT_ENBL": 1,
|
|
|
|
"H_RSC_AROT_RAMP": arot_ramp_time,
|
|
|
|
"H_RSC_AROT_RUNUP": AROT_RUNUP_TIME,
|
|
|
|
"H_RSC_AROT_IDLE": arot_idle,
|
|
|
|
"H_RSC_RAMP_TIME": RAMP_TIME,
|
|
|
|
"H_RSC_RUNUP_TIME": RUNUP_TIME,
|
|
|
|
"H_RSC_IDLE": rsc_idle,
|
|
|
|
"H_RSC_SETPOINT": RSC_SETPOINT,
|
|
|
|
"H_RSC_CLDWN_TIME": cool_down
|
|
|
|
})
|
2023-02-16 23:17:01 -04:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
# Check the RSC config so we know what to expect on the throttle output
|
|
|
|
if self.get_parameter("H_RSC_MODE") != 2:
|
|
|
|
self.set_parameter("H_RSC_MODE", 2)
|
|
|
|
self.reboot_sitl()
|
2023-07-08 00:47:51 -03:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
self.change_mode('POSHOLD')
|
|
|
|
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")
|
|
|
|
check_rsc_output(self, RSC_SETPOINT, RUNUP_TIME+1)
|
|
|
|
|
|
|
|
self.delay_sim_time(20)
|
|
|
|
self.set_rc(3, 2000)
|
|
|
|
self.wait_altitude(100,
|
|
|
|
105,
|
|
|
|
relative=True,
|
|
|
|
timeout=timeout)
|
|
|
|
self.context_collect('STATUSTEXT')
|
|
|
|
self.change_mode('STABILIZE')
|
2023-02-16 23:17:01 -04:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
self.progress("Triggering manual autorotation by disabling interlock")
|
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.set_rc(8, 1000)
|
2023-07-08 00:47:51 -03:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
self.wait_statustext(r"RSC: In Autorotation", check_context=True)
|
2023-02-16 23:17:01 -04:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
# Check we are using the correct throttle output. This should happen instantly on ramp down.
|
|
|
|
idle_thr = rsc_idle
|
|
|
|
if (arot_idle > 0):
|
|
|
|
idle_thr = arot_idle
|
2023-02-16 23:17:01 -04:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
check_rsc_output(self, idle_thr, 1)
|
2023-02-16 23:17:01 -04:00
|
|
|
|
2024-08-10 13:22:40 -03:00
|
|
|
self.progress("RSC is outputting correct idle throttle")
|
|
|
|
|
|
|
|
# Wait to establish autorotation.
|
|
|
|
self.delay_sim_time(2)
|
|
|
|
|
|
|
|
# Re-engage interlock to start bailout sequence
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
|
|
|
|
# Ensure we see the bailout state
|
|
|
|
self.wait_statustext("RSC: Bailing Out", check_context=True)
|
|
|
|
|
|
|
|
# Check we are back up to flight throttle. Autorotation ramp up time should be used
|
|
|
|
check_rsc_output(self, RSC_SETPOINT, arot_ramp_time+1)
|
|
|
|
|
|
|
|
# Give time for engine to power up
|
|
|
|
self.set_rc(3, 1400)
|
|
|
|
self.delay_sim_time(2)
|
|
|
|
|
|
|
|
self.progress("in-flight power recovery")
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
self.delay_sim_time(5)
|
|
|
|
|
|
|
|
# Initiate autorotation again
|
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
|
|
|
|
check_context=True,
|
|
|
|
regex=True)
|
|
|
|
speed = float(self.re_match.group(1))
|
|
|
|
if speed > 30:
|
|
|
|
raise NotAchievedException("Hit too hard")
|
|
|
|
|
|
|
|
# Check that cool down is still used correctly if set
|
|
|
|
# First wait until we are out of the autorotation state
|
|
|
|
self.wait_statustext("RSC: Autorotation Stopped")
|
|
|
|
if (cool_down > 0):
|
|
|
|
check_rsc_output(self, rsc_idle*1.5, cool_down)
|
|
|
|
|
|
|
|
# Verify RSC output resets to RSC_IDLE after land complete
|
|
|
|
check_rsc_output(self, rsc_idle, 20)
|
|
|
|
self.wait_disarmed()
|
|
|
|
|
|
|
|
# We test the bailout behavior of two different configs
|
|
|
|
# First we test config with a regular throttle curve
|
|
|
|
self.progress("testing autorotation with throttle curve config")
|
|
|
|
self.context_push()
|
|
|
|
TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=0, cool_down=0)
|
|
|
|
|
|
|
|
# Now we test a config that would be used with an ESC with internal governor and an autorotation window
|
|
|
|
self.progress("testing autorotation with ESC autorotation window config")
|
|
|
|
TestAutorotationConfig(self, rsc_idle=0.0, arot_ramp_time=0.0, arot_idle=20.0, cool_down=0)
|
|
|
|
|
|
|
|
# Check rsc output behavior when using the cool down feature
|
|
|
|
self.progress("testing autorotation with cool down enabled and zero autorotation idle")
|
|
|
|
TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=0, cool_down=5.0)
|
|
|
|
|
|
|
|
self.progress("testing that H_RSC_AROT_IDLE is used over RSC_IDLE when cool down is enabled")
|
|
|
|
TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=10, cool_down=5.0)
|
|
|
|
|
|
|
|
self.context_pop()
|
2023-02-16 23:17:01 -04:00
|
|
|
|
2021-03-11 04:30:55 -04:00
|
|
|
def mission_item_home(self, target_system, target_component):
|
|
|
|
'''returns a mission_item_int which can be used as home in a mission'''
|
|
|
|
return self.mav.mav.mission_item_int_encode(
|
|
|
|
target_system,
|
|
|
|
target_component,
|
|
|
|
0, # seq
|
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
|
|
|
|
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
|
|
|
|
0, # current
|
|
|
|
0, # autocontinue
|
|
|
|
3, # p1
|
|
|
|
0, # p2
|
|
|
|
0, # p3
|
|
|
|
0, # p4
|
|
|
|
int(1.0000 * 1e7), # latitude
|
|
|
|
int(2.0000 * 1e7), # longitude
|
|
|
|
31.0000, # altitude
|
|
|
|
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
|
|
|
|
|
|
|
|
def mission_item_takeoff(self, target_system, target_component):
|
|
|
|
'''returns a mission_item_int which can be used as takeoff in a mission'''
|
|
|
|
return self.mav.mav.mission_item_int_encode(
|
|
|
|
target_system,
|
|
|
|
target_component,
|
|
|
|
1, # seq
|
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
|
|
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
|
|
|
|
0, # current
|
|
|
|
0, # autocontinue
|
|
|
|
0, # p1
|
|
|
|
0, # p2
|
|
|
|
0, # p3
|
|
|
|
0, # p4
|
|
|
|
int(1.0000 * 1e7), # latitude
|
|
|
|
int(1.0000 * 1e7), # longitude
|
|
|
|
31.0000, # altitude
|
|
|
|
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
|
|
|
|
|
|
|
|
def mission_item_rtl(self, target_system, target_component):
|
|
|
|
'''returns a mission_item_int which can be used as takeoff in a mission'''
|
|
|
|
return self.mav.mav.mission_item_int_encode(
|
|
|
|
target_system,
|
|
|
|
target_component,
|
|
|
|
1, # seq
|
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL,
|
|
|
|
mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
|
|
|
|
0, # current
|
|
|
|
0, # autocontinue
|
|
|
|
0, # p1
|
|
|
|
0, # p2
|
|
|
|
0, # p3
|
|
|
|
0, # p4
|
|
|
|
0, # latitude
|
|
|
|
0, # longitude
|
|
|
|
0.0000, # altitude
|
|
|
|
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
|
|
|
|
|
|
|
|
def scurve_nasty_mission(self, target_system=1, target_component=1):
|
|
|
|
'''returns a mission which attempts to give the SCurve library
|
|
|
|
indigestion. The same destination is given several times.'''
|
|
|
|
|
|
|
|
wp2_loc = self.mav.location()
|
|
|
|
wp2_offset_n = 20
|
|
|
|
wp2_offset_e = 30
|
|
|
|
self.location_offset_ne(wp2_loc, wp2_offset_n, wp2_offset_e)
|
|
|
|
|
|
|
|
wp2_by_three = self.mav.mav.mission_item_int_encode(
|
|
|
|
target_system,
|
|
|
|
target_component,
|
|
|
|
2, # seq
|
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
|
|
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
|
|
|
|
0, # current
|
|
|
|
0, # autocontinue
|
|
|
|
3, # p1
|
|
|
|
0, # p2
|
|
|
|
0, # p3
|
|
|
|
0, # p4
|
|
|
|
int(wp2_loc.lat * 1e7), # latitude
|
|
|
|
int(wp2_loc.lng * 1e7), # longitude
|
|
|
|
31.0000, # altitude
|
|
|
|
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
|
|
|
|
|
|
|
|
wp5_loc = self.mav.location()
|
|
|
|
wp5_offset_n = -20
|
|
|
|
wp5_offset_e = 30
|
|
|
|
self.location_offset_ne(wp5_loc, wp5_offset_n, wp5_offset_e)
|
|
|
|
|
|
|
|
wp5_by_three = self.mav.mav.mission_item_int_encode(
|
|
|
|
target_system,
|
|
|
|
target_component,
|
|
|
|
5, # seq
|
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
|
|
mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT,
|
|
|
|
0, # current
|
|
|
|
0, # autocontinue
|
|
|
|
3, # p1
|
|
|
|
0, # p2
|
|
|
|
0, # p3
|
|
|
|
0, # p4
|
|
|
|
int(wp5_loc.lat * 1e7), # latitude
|
|
|
|
int(wp5_loc.lng * 1e7), # longitude
|
|
|
|
31.0000, # altitude
|
|
|
|
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
|
|
|
|
|
|
|
|
ret = copy.copy([
|
|
|
|
# slot 0 is home
|
|
|
|
self.mission_item_home(target_system=target_system, target_component=target_component),
|
|
|
|
# slot 1 is takeoff
|
|
|
|
self.mission_item_takeoff(target_system=target_system, target_component=target_component),
|
|
|
|
# now three spline waypoints right on top of one another:
|
|
|
|
copy.copy(wp2_by_three),
|
|
|
|
copy.copy(wp2_by_three),
|
|
|
|
copy.copy(wp2_by_three),
|
|
|
|
# now three MORE spline waypoints right on top of one another somewhere else:
|
|
|
|
copy.copy(wp5_by_three),
|
|
|
|
copy.copy(wp5_by_three),
|
|
|
|
copy.copy(wp5_by_three),
|
|
|
|
self.mission_item_rtl(target_system=target_system, target_component=target_component),
|
|
|
|
])
|
2023-09-11 23:15:56 -03:00
|
|
|
self.correct_wp_seq_numbers(ret)
|
2021-03-11 04:30:55 -04:00
|
|
|
return ret
|
|
|
|
|
|
|
|
def scurve_nasty_up_mission(self, target_system=1, target_component=1):
|
|
|
|
'''returns a mission which attempts to give the SCurve library
|
|
|
|
indigestion. The same destination is given several times but with differing altitudes.'''
|
|
|
|
|
|
|
|
wp2_loc = self.mav.location()
|
|
|
|
wp2_offset_n = 20
|
|
|
|
wp2_offset_e = 30
|
|
|
|
self.location_offset_ne(wp2_loc, wp2_offset_n, wp2_offset_e)
|
|
|
|
|
|
|
|
wp2 = self.mav.mav.mission_item_int_encode(
|
|
|
|
target_system,
|
|
|
|
target_component,
|
|
|
|
2, # seq
|
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
|
|
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
|
|
|
|
0, # current
|
|
|
|
0, # autocontinue
|
|
|
|
3, # p1
|
|
|
|
0, # p2
|
|
|
|
0, # p3
|
|
|
|
0, # p4
|
|
|
|
int(wp2_loc.lat * 1e7), # latitude
|
|
|
|
int(wp2_loc.lng * 1e7), # longitude
|
|
|
|
31.0000, # altitude
|
|
|
|
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
|
|
|
|
wp3 = copy.copy(wp2)
|
|
|
|
wp3.alt = 40
|
|
|
|
wp4 = copy.copy(wp2)
|
|
|
|
wp4.alt = 31
|
|
|
|
|
|
|
|
wp5_loc = self.mav.location()
|
|
|
|
wp5_offset_n = -20
|
|
|
|
wp5_offset_e = 30
|
|
|
|
self.location_offset_ne(wp5_loc, wp5_offset_n, wp5_offset_e)
|
|
|
|
|
|
|
|
wp5 = self.mav.mav.mission_item_int_encode(
|
|
|
|
target_system,
|
|
|
|
target_component,
|
|
|
|
5, # seq
|
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
|
|
mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT,
|
|
|
|
0, # current
|
|
|
|
0, # autocontinue
|
|
|
|
3, # p1
|
|
|
|
0, # p2
|
|
|
|
0, # p3
|
|
|
|
0, # p4
|
|
|
|
int(wp5_loc.lat * 1e7), # latitude
|
|
|
|
int(wp5_loc.lng * 1e7), # longitude
|
|
|
|
31.0000, # altitude
|
|
|
|
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
|
|
|
|
wp6 = copy.copy(wp5)
|
|
|
|
wp6.alt = 41
|
|
|
|
wp7 = copy.copy(wp5)
|
|
|
|
wp7.alt = 51
|
|
|
|
|
|
|
|
ret = copy.copy([
|
|
|
|
# slot 0 is home
|
|
|
|
self.mission_item_home(target_system=target_system, target_component=target_component),
|
|
|
|
# slot 1 is takeoff
|
|
|
|
self.mission_item_takeoff(target_system=target_system, target_component=target_component),
|
|
|
|
wp2,
|
|
|
|
wp3,
|
|
|
|
wp4,
|
|
|
|
# now three MORE spline waypoints right on top of one another somewhere else:
|
|
|
|
wp5,
|
|
|
|
wp6,
|
|
|
|
wp7,
|
|
|
|
self.mission_item_rtl(target_system=target_system, target_component=target_component),
|
|
|
|
])
|
2023-09-11 23:15:56 -03:00
|
|
|
self.correct_wp_seq_numbers(ret)
|
2021-03-11 04:30:55 -04:00
|
|
|
return ret
|
|
|
|
|
|
|
|
def fly_mission_points(self, points):
|
|
|
|
'''takes a list of waypoints and flies them, expecting a disarm at end'''
|
|
|
|
self.check_mission_upload_download(points)
|
|
|
|
self.set_parameter("AUTO_OPTIONS", 3)
|
|
|
|
self.change_mode('AUTO')
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.progress("Raising rotor speed")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.wait_waypoint(0, len(points)-1)
|
|
|
|
self.wait_disarmed()
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
def NastyMission(self):
|
|
|
|
'''constructs and runs missions designed to test scurves'''
|
|
|
|
self.fly_mission_points(self.scurve_nasty_mission())
|
|
|
|
# hopefully we don't need this step forever:
|
|
|
|
self.progress("Restting mission state machine by changing into LOITER")
|
|
|
|
self.change_mode('LOITER')
|
|
|
|
self.fly_mission_points(self.scurve_nasty_up_mission())
|
|
|
|
|
2021-09-26 21:24:57 -03:00
|
|
|
def set_rc_default(self):
|
|
|
|
super(AutoTestHelicopter, self).set_rc_default()
|
|
|
|
self.progress("Lowering rotor speed")
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
2021-10-01 05:43:37 -03:00
|
|
|
def fly_mission(self, filename, strict=True):
|
|
|
|
num_wp = self.load_mission(filename, strict=strict)
|
|
|
|
self.change_mode("LOITER")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.set_rc(8, 2000) # Raise rotor speed
|
|
|
|
self.delay_sim_time(20)
|
|
|
|
self.change_mode("AUTO")
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
self.wait_waypoint(1, num_wp-1)
|
|
|
|
self.wait_disarmed()
|
|
|
|
self.set_rc(8, 1000) # Lower rotor speed
|
|
|
|
|
|
|
|
# FIXME move this & plane's version to common
|
2022-09-09 22:24:28 -03:00
|
|
|
def AirspeedDrivers(self, timeout=600):
|
|
|
|
'''Test AirSpeed drivers'''
|
2021-10-01 05:43:37 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
# Copter's airspeed sensors are off by default
|
|
|
|
self.set_parameters({
|
|
|
|
"ARSPD_ENABLE": 1,
|
|
|
|
"ARSPD_TYPE": 2, # Analog airspeed driver
|
|
|
|
"ARSPD_PIN": 1, # Analog airspeed driver pin for SITL
|
|
|
|
})
|
2021-10-01 05:43:37 -03:00
|
|
|
# set the start location to CMAC to use same test script as other vehicles
|
2024-07-26 10:56:24 -03:00
|
|
|
|
2021-10-01 05:43:37 -03:00
|
|
|
self.sitl_start_loc = mavutil.location(-35.362881, 149.165222, 582.000000, 90.0) # CMAC
|
|
|
|
self.customise_SITL_commandline(["--home", "%s,%s,%s,%s"
|
|
|
|
% (-35.362881, 149.165222, 582.000000, 90.0)])
|
|
|
|
|
|
|
|
# insert listener to compare airspeeds:
|
|
|
|
airspeed = [None, None]
|
|
|
|
|
|
|
|
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]))
|
|
|
|
|
|
|
|
airspeed_sensors = [
|
|
|
|
("MS5525", 3, 1),
|
|
|
|
("DLVR", 7, 2),
|
|
|
|
]
|
|
|
|
for (name, t, bus) in airspeed_sensors:
|
|
|
|
self.context_push()
|
|
|
|
if bus is not None:
|
|
|
|
self.set_parameter("ARSPD2_BUS", bus)
|
|
|
|
self.set_parameter("ARSPD2_TYPE", t)
|
|
|
|
self.reboot_sitl()
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
|
|
|
self.install_message_hook_context(check_airspeeds)
|
|
|
|
self.fly_mission("ap1.txt", strict=False)
|
|
|
|
|
|
|
|
if airspeed[0] is None:
|
|
|
|
raise NotAchievedException("Never saw an airspeed1")
|
|
|
|
if airspeed[1] is None:
|
|
|
|
raise NotAchievedException("Never saw an airspeed2")
|
|
|
|
if not self.current_onboard_log_contains_message("ARSP"):
|
|
|
|
raise NotAchievedException("Expected ARSP log message")
|
2024-07-07 23:33:30 -03:00
|
|
|
self.disarm_vehicle()
|
|
|
|
self.context_pop()
|
2021-10-01 05:43:37 -03:00
|
|
|
|
2024-10-03 03:17:03 -03:00
|
|
|
def TurbineCoolDown(self, timeout=200):
|
|
|
|
"""Check Turbine Cool Down Feature"""
|
2024-10-07 15:10:03 -03:00
|
|
|
self.context_push()
|
2024-10-03 03:17:03 -03:00
|
|
|
# set option for Turbine
|
|
|
|
RAMP_TIME = 4
|
|
|
|
SETPOINT = 66
|
|
|
|
IDLE = 15
|
|
|
|
COOLDOWN_TIME = 5
|
2024-10-07 15:10:03 -03:00
|
|
|
self.set_parameters({"RC6_OPTION": 161,
|
|
|
|
"H_RSC_RAMP_TIME": RAMP_TIME,
|
|
|
|
"H_RSC_SETPOINT": SETPOINT,
|
|
|
|
"H_RSC_IDLE": IDLE,
|
|
|
|
"H_RSC_CLDWN_TIME": COOLDOWN_TIME})
|
2024-10-03 03:17:03 -03:00
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
self.progress("Starting turbine")
|
|
|
|
self.wait_ready_to_arm()
|
2024-10-07 15:10:03 -03:00
|
|
|
self.context_collect("STATUSTEXT")
|
2024-10-03 03:17:03 -03:00
|
|
|
self.arm_vehicle()
|
|
|
|
|
|
|
|
self.set_rc(6, 2000)
|
2024-10-07 15:10:03 -03:00
|
|
|
self.wait_statustext('Turbine startup', check_context=True)
|
2024-10-03 03:17:03 -03:00
|
|
|
|
|
|
|
# Engage interlock to run up to head speed
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
|
|
|
|
# Check throttle gets to setpoint
|
|
|
|
expected_thr = SETPOINT * 0.01 * 1000 + 1000 - 1 # servo end points are 1000 to 2000
|
|
|
|
self.wait_servo_channel_value(8, expected_thr, timeout=RAMP_TIME+1, comparator=operator.ge)
|
|
|
|
|
|
|
|
self.progress("Checking cool down behaviour, idle x 1.5")
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
expected_thr = IDLE * 1.5 * 0.01 * 1000 + 1000 + 1
|
|
|
|
self.wait_servo_channel_value(8, expected_thr, timeout=2, comparator=operator.le)
|
|
|
|
|
|
|
|
# Check that the throttle drops to idle after cool down time
|
|
|
|
expected_thr = IDLE * 0.01 * 1000 + 1000 + 1
|
|
|
|
self.wait_servo_channel_value(8, expected_thr, timeout=COOLDOWN_TIME+1, comparator=operator.le)
|
|
|
|
|
|
|
|
measured_time = self.get_sim_time() - tstart
|
|
|
|
if (abs(measured_time - COOLDOWN_TIME) > 1.0):
|
|
|
|
raise NotAchievedException('Throttle did not reduce to idle within H_RSC_CLDWN_TIME')
|
|
|
|
|
|
|
|
self.set_rc(6, 1000)
|
|
|
|
self.wait_disarmed(timeout=20)
|
2024-10-07 15:10:03 -03:00
|
|
|
self.context_pop()
|
2024-10-03 03:17:03 -03:00
|
|
|
|
2023-02-18 23:55:29 -04:00
|
|
|
def TurbineStart(self, timeout=200):
|
|
|
|
"""Check Turbine Start Feature"""
|
|
|
|
RAMP_TIME = 4
|
|
|
|
# set option for Turbine Start
|
|
|
|
self.set_parameter("RC6_OPTION", 161)
|
|
|
|
self.set_parameter("H_RSC_RAMP_TIME", RAMP_TIME)
|
|
|
|
self.set_parameter("H_RSC_SETPOINT", 66)
|
|
|
|
self.set_parameter("DISARM_DELAY", 0)
|
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
# check that turbine start doesn't activate while disarmed
|
|
|
|
self.progress("Checking Turbine Start doesn't activate while disarmed")
|
|
|
|
self.set_rc(6, 2000)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time() - tstart < 2:
|
|
|
|
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True)
|
|
|
|
if servo.servo8_raw > 1050:
|
|
|
|
raise NotAchievedException("Turbine Start activated while disarmed")
|
|
|
|
self.set_rc(6, 1000)
|
|
|
|
|
|
|
|
# check that turbine start doesn't activate armed with interlock enabled
|
|
|
|
self.progress("Checking Turbine Start doesn't activate while armed with interlock enabled")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.set_rc(6, 2000)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time() - tstart < 5:
|
|
|
|
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True)
|
|
|
|
if servo.servo8_raw > 1660:
|
|
|
|
raise NotAchievedException("Turbine Start activated with interlock enabled")
|
|
|
|
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
self.set_rc(6, 1000)
|
|
|
|
self.disarm_vehicle()
|
|
|
|
|
|
|
|
# check that turbine start activates as designed (armed with interlock disabled)
|
|
|
|
self.progress("Checking Turbine Start activates as designed (armed with interlock disabled)")
|
|
|
|
self.delay_sim_time(2)
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
|
|
|
self.set_rc(6, 2000)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time() - tstart > 5:
|
|
|
|
raise AutoTestTimeoutException("Turbine Start did not activate")
|
|
|
|
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True)
|
|
|
|
if servo.servo8_raw > 1800:
|
|
|
|
break
|
|
|
|
|
|
|
|
self.wait_servo_channel_value(8, 1000, timeout=3)
|
|
|
|
self.set_rc(6, 1000)
|
|
|
|
|
|
|
|
# check that turbine start will not reactivate after interlock enabled
|
|
|
|
self.progress("Checking Turbine Start doesn't activate once interlock is enabled after start)")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.set_rc(6, 2000)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time() - tstart < 5:
|
|
|
|
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True)
|
|
|
|
if servo.servo8_raw > 1660:
|
|
|
|
raise NotAchievedException("Turbine Start activated with interlock enabled")
|
|
|
|
self.set_rc(6, 1000)
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
self.disarm_vehicle()
|
|
|
|
|
2023-07-27 13:34:43 -03:00
|
|
|
def PIDNotches(self):
|
|
|
|
"""Use dynamic harmonic notch to control motor noise."""
|
|
|
|
self.progress("Flying with PID notches")
|
2024-07-26 10:56:24 -03:00
|
|
|
self.set_parameters({
|
|
|
|
"FILT1_TYPE": 1,
|
|
|
|
"FILT2_TYPE": 1,
|
|
|
|
"AHRS_EKF_TYPE": 10,
|
|
|
|
"INS_LOG_BAT_MASK": 3,
|
|
|
|
"INS_LOG_BAT_OPT": 0,
|
|
|
|
"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour
|
|
|
|
"LOG_BITMASK": 65535,
|
|
|
|
"LOG_DISARMED": 0,
|
|
|
|
"SIM_VIB_FREQ_X": 120, # roll
|
|
|
|
"SIM_VIB_FREQ_Y": 120, # pitch
|
|
|
|
"SIM_VIB_FREQ_Z": 180, # yaw
|
|
|
|
"FILT1_NOTCH_FREQ": 120,
|
|
|
|
"FILT2_NOTCH_FREQ": 180,
|
|
|
|
"ATC_RAT_RLL_NEF": 1,
|
|
|
|
"ATC_RAT_PIT_NEF": 1,
|
|
|
|
"ATC_RAT_YAW_NEF": 2,
|
|
|
|
"SIM_GYR1_RND": 5,
|
|
|
|
})
|
|
|
|
self.reboot_sitl()
|
2023-07-27 13:34:43 -03:00
|
|
|
|
2024-07-26 10:56:24 -03:00
|
|
|
self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True, takeoff=True)
|
2023-07-27 13:34:43 -03:00
|
|
|
|
2024-02-28 19:21:16 -04:00
|
|
|
def AutoTune(self):
|
|
|
|
"""Test autotune mode"""
|
|
|
|
# test roll and pitch FF tuning
|
|
|
|
self.set_parameters({
|
|
|
|
"ATC_ANG_RLL_P": 4.5,
|
|
|
|
"ATC_RAT_RLL_P": 0,
|
|
|
|
"ATC_RAT_RLL_I": 0.1,
|
|
|
|
"ATC_RAT_RLL_D": 0,
|
|
|
|
"ATC_RAT_RLL_FF": 0.15,
|
|
|
|
"ATC_ANG_PIT_P": 4.5,
|
|
|
|
"ATC_RAT_PIT_P": 0,
|
|
|
|
"ATC_RAT_PIT_I": 0.1,
|
|
|
|
"ATC_RAT_PIT_D": 0,
|
|
|
|
"ATC_RAT_PIT_FF": 0.15,
|
|
|
|
"ATC_ANG_YAW_P": 4.5,
|
|
|
|
"ATC_RAT_YAW_P": 0.18,
|
|
|
|
"ATC_RAT_YAW_I": 0.024,
|
|
|
|
"ATC_RAT_YAW_D": 0.003,
|
|
|
|
"ATC_RAT_YAW_FF": 0.024,
|
|
|
|
"AUTOTUNE_AXES": 3,
|
|
|
|
"AUTOTUNE_SEQ": 1,
|
|
|
|
})
|
|
|
|
|
|
|
|
# Conduct testing from althold
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
|
|
|
|
# hold position in loiter
|
|
|
|
self.change_mode('AUTOTUNE')
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_statustext('AutoTune: Success', timeout=1000)
|
|
|
|
now = self.get_sim_time()
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
2024-04-22 08:27:25 -03:00
|
|
|
self.autotune_land_and_save_gains()
|
2024-02-28 19:21:16 -04:00
|
|
|
|
|
|
|
# test pitch rate P and Rate D tuning
|
|
|
|
self.set_parameters({
|
|
|
|
"AUTOTUNE_AXES": 2,
|
|
|
|
"AUTOTUNE_SEQ": 2,
|
2024-04-18 23:37:06 -03:00
|
|
|
"AUTOTUNE_GN_MAX": 1.8,
|
2024-02-28 19:21:16 -04:00
|
|
|
})
|
|
|
|
|
|
|
|
# Conduct testing from althold
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
|
|
|
|
# hold position in loiter
|
|
|
|
self.change_mode('AUTOTUNE')
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_statustext('AutoTune: Success', timeout=1000)
|
|
|
|
now = self.get_sim_time()
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
2024-04-22 08:27:25 -03:00
|
|
|
self.autotune_land_and_save_gains()
|
2024-02-28 19:21:16 -04:00
|
|
|
|
|
|
|
# test Roll rate P and Rate D tuning
|
|
|
|
self.set_parameters({
|
|
|
|
"AUTOTUNE_AXES": 1,
|
|
|
|
"AUTOTUNE_SEQ": 2,
|
2024-04-18 23:37:06 -03:00
|
|
|
"AUTOTUNE_GN_MAX": 1.6,
|
2024-02-28 19:21:16 -04:00
|
|
|
})
|
|
|
|
|
|
|
|
# Conduct testing from althold
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
|
|
|
|
# hold position in loiter
|
|
|
|
self.change_mode('AUTOTUNE')
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_statustext('AutoTune: Success', timeout=1000)
|
|
|
|
now = self.get_sim_time()
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
2024-04-22 08:27:25 -03:00
|
|
|
self.autotune_land_and_save_gains()
|
2024-02-28 19:21:16 -04:00
|
|
|
|
|
|
|
# test Roll and pitch angle P tuning
|
|
|
|
self.set_parameters({
|
|
|
|
"AUTOTUNE_AXES": 3,
|
|
|
|
"AUTOTUNE_SEQ": 4,
|
2024-04-18 23:37:06 -03:00
|
|
|
"AUTOTUNE_FRQ_MIN": 5,
|
|
|
|
"AUTOTUNE_FRQ_MAX": 50,
|
|
|
|
"AUTOTUNE_GN_MAX": 1.6,
|
|
|
|
})
|
|
|
|
|
|
|
|
# Conduct testing from althold
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
|
|
|
|
# hold position in loiter
|
|
|
|
self.change_mode('AUTOTUNE')
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_statustext('AutoTune: Success', timeout=1000)
|
|
|
|
now = self.get_sim_time()
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
2024-04-22 08:27:25 -03:00
|
|
|
self.autotune_land_and_save_gains()
|
2024-04-18 23:37:06 -03:00
|
|
|
|
|
|
|
# test yaw FF and rate P and Rate D
|
|
|
|
self.set_parameters({
|
|
|
|
"AUTOTUNE_AXES": 4,
|
|
|
|
"AUTOTUNE_SEQ": 3,
|
|
|
|
"AUTOTUNE_FRQ_MIN": 10,
|
|
|
|
"AUTOTUNE_FRQ_MAX": 70,
|
|
|
|
"AUTOTUNE_GN_MAX": 1.4,
|
2024-02-28 19:21:16 -04:00
|
|
|
})
|
|
|
|
|
|
|
|
# Conduct testing from althold
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
|
|
|
|
# hold position in loiter
|
|
|
|
self.change_mode('AUTOTUNE')
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_statustext('AutoTune: Success', timeout=1000)
|
|
|
|
now = self.get_sim_time()
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
2024-04-22 08:27:25 -03:00
|
|
|
self.autotune_land_and_save_gains()
|
2024-02-28 19:21:16 -04:00
|
|
|
|
2024-04-18 23:37:06 -03:00
|
|
|
# test yaw angle P tuning
|
2024-02-28 19:21:16 -04:00
|
|
|
self.set_parameters({
|
|
|
|
"AUTOTUNE_AXES": 4,
|
2024-04-18 23:37:06 -03:00
|
|
|
"AUTOTUNE_SEQ": 4,
|
|
|
|
"AUTOTUNE_FRQ_MIN": 5,
|
|
|
|
"AUTOTUNE_FRQ_MAX": 50,
|
|
|
|
"AUTOTUNE_GN_MAX": 1.5,
|
2024-02-28 19:21:16 -04:00
|
|
|
})
|
|
|
|
|
|
|
|
# Conduct testing from althold
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
|
|
|
|
# hold position in loiter
|
|
|
|
self.change_mode('AUTOTUNE')
|
|
|
|
|
2024-04-22 08:27:25 -03:00
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_statustext('AutoTune: Success', timeout=1000)
|
|
|
|
now = self.get_sim_time()
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
|
|
|
self.autotune_land_and_save_gains()
|
|
|
|
|
|
|
|
# tune check
|
|
|
|
self.set_parameters({
|
|
|
|
"AUTOTUNE_AXES": 7,
|
|
|
|
"AUTOTUNE_SEQ": 16,
|
|
|
|
"AUTOTUNE_FRQ_MIN": 10,
|
|
|
|
"AUTOTUNE_FRQ_MAX": 80,
|
|
|
|
})
|
|
|
|
|
|
|
|
# Conduct testing from althold
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
|
|
|
|
# hold position in loiter
|
|
|
|
self.change_mode('AUTOTUNE')
|
|
|
|
|
2024-02-28 19:21:16 -04:00
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_statustext('AutoTune: Success', timeout=1000)
|
|
|
|
now = self.get_sim_time()
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
|
|
|
self.land_and_disarm()
|
2024-04-19 20:40:30 -03:00
|
|
|
|
2024-04-22 08:27:25 -03:00
|
|
|
def autotune_land_and_save_gains(self):
|
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.context_collect('STATUSTEXT')
|
|
|
|
self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
|
|
|
|
check_context=True,
|
|
|
|
regex=True)
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
self.wait_disarmed()
|
|
|
|
|
2024-04-19 20:40:30 -03:00
|
|
|
def land_and_disarm(self, **kwargs):
|
|
|
|
super(AutoTestHelicopter, self).land_and_disarm(**kwargs)
|
|
|
|
self.progress("Killing rotor speed")
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
def do_RTL(self, **kwargs):
|
|
|
|
super(AutoTestHelicopter, self).do_RTL(**kwargs)
|
|
|
|
self.progress("Killing rotor speed")
|
2024-02-28 19:21:16 -04:00
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
2021-09-26 21:24:57 -03:00
|
|
|
def tests(self):
|
|
|
|
'''return list of all tests'''
|
2022-02-23 08:07:15 -04:00
|
|
|
ret = vehicle_test_suite.TestSuite.tests(self)
|
2021-09-26 21:24:57 -03:00
|
|
|
ret.extend([
|
2022-09-09 22:24:28 -03:00
|
|
|
self.AVCMission,
|
|
|
|
self.RotorRunup,
|
|
|
|
self.PosHoldTakeOff,
|
|
|
|
self.StabilizeTakeOff,
|
|
|
|
self.SplineWaypoint,
|
2024-08-10 13:22:40 -03:00
|
|
|
self.Autorotation,
|
|
|
|
self.ManAutorotation,
|
2023-05-30 00:38:27 -03:00
|
|
|
self.governortest,
|
2022-09-09 22:24:28 -03:00
|
|
|
self.FlyEachFrame,
|
|
|
|
self.AirspeedDrivers,
|
2023-02-18 23:55:29 -04:00
|
|
|
self.TurbineStart,
|
2024-10-03 03:17:03 -03:00
|
|
|
self.TurbineCoolDown,
|
2021-03-11 04:30:55 -04:00
|
|
|
self.NastyMission,
|
2023-07-27 13:34:43 -03:00
|
|
|
self.PIDNotches,
|
2024-02-28 19:21:16 -04:00
|
|
|
self.AutoTune,
|
2021-09-26 21:24:57 -03:00
|
|
|
])
|
|
|
|
return ret
|
|
|
|
|
|
|
|
def disabled_tests(self):
|
|
|
|
return {
|
|
|
|
}
|