mirror of https://github.com/ArduPilot/ardupilot
autotest: add test for FETtec OneWire ESCs
make sure the SERVO_FWT_MASK is valid: - it can have bit gaps between active channels, but channels higher than 12 are not allowed (AP_EST_TELEM limitation) - Explain that the FETtec ESC IDs inside the FETtec Firmware need to be contiguous and start at 1. add tests for ESC power outages add test that safety switch zeroes PWM for FETtec ESC a gap in the beginning of the SERVO_FTW_MASK is now allowed, test it
This commit is contained in:
Peter Barker
Randy Mackay
parent
1943938bc5
commit
4903224b47
|
|
@ -0,0 +1,14 @@
|
|||
QGC WPL 110
|
||||
0 1 0 16 0.000000 0.000000 0.000000 0.000000 -35.362881 149.165222 582.000000 1
|
||||
1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.362881 149.165222 20.000000 1
|
||||
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364652 149.163501 0.000000 1
|
||||
3 0 3 115 640.000000 20.000000 1.000000 1.000000 0.000000 0.000000 0.000000 1
|
||||
4 0 3 19 5.000000 0.000000 0.000000 1.000000 0.000000 0.000000 20.000000 1
|
||||
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365361 149.163501 0.000000 1
|
||||
6 0 3 16 1.000000 0.000000 0.000000 0.000000 -35.365361 149.163995 40.000000 1
|
||||
7 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365361 149.164563 20.000000 1
|
||||
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364652 149.164531 20.000000 1
|
||||
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364652 149.163995 20.000000 1
|
||||
10 0 3 177 8.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
|
||||
11 0 3 16 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
|
||||
12 0 3 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
|
||||
|
|
@ -2157,6 +2157,12 @@ class AutoTestCopter(AutoTest):
|
|||
if not num_wp:
|
||||
raise NotAchievedException("load copter_mission failed")
|
||||
|
||||
self.fly_loaded_mission(num_wp)
|
||||
|
||||
self.progress("Auto mission completed: passed!")
|
||||
|
||||
def fly_loaded_mission(self, num_wp):
|
||||
'''fly mission loaded on vehicle. FIXME: get num_wp from vehicle'''
|
||||
self.progress("test: Fly a mission from 1 to %u" % num_wp)
|
||||
self.set_current_waypoint(1)
|
||||
|
||||
|
|
@ -2178,8 +2184,6 @@ class AutoTestCopter(AutoTest):
|
|||
self.wait_disarmed()
|
||||
self.progress("MOTORS DISARMED OK")
|
||||
|
||||
self.progress("Auto mission completed: passed!")
|
||||
|
||||
# fly_auto_test using CAN GPS - fly mission which tests normal operation alongside CAN GPS
|
||||
def fly_auto_test_using_can_gps(self):
|
||||
self.set_parameter("CAN_P1_DRIVER", 1)
|
||||
|
|
@ -7084,6 +7088,126 @@ class AutoTestCopter(AutoTest):
|
|||
ret.extend(self.tests1e())
|
||||
return ret
|
||||
|
||||
def FETtecESC_flight(self):
|
||||
'''fly with servo outputs from FETtec ESC'''
|
||||
self.start_subtest("FETtec ESC flight")
|
||||
num_wp = self.load_mission("copter_mission.txt", strict=False)
|
||||
self.fly_loaded_mission(num_wp)
|
||||
|
||||
def FETtecESC_esc_power_checks(self):
|
||||
'''Make sure state machine copes with ESCs rebooting'''
|
||||
self.start_subtest("FETtec ESC reboot")
|
||||
self.wait_ready_to_arm()
|
||||
self.context_collect('STATUSTEXT')
|
||||
self.progress("Turning off an ESC off ")
|
||||
mask = int(self.get_parameter("SIM_FTOWESC_POW"))
|
||||
|
||||
for mot_id_to_kill in 1, 2:
|
||||
self.progress("Turning ESC=%u off" % mot_id_to_kill)
|
||||
self.set_parameter("SIM_FTOWESC_POW", mask & ~(1 << mot_id_to_kill))
|
||||
self.assert_prearm_failure("are not sending tel")
|
||||
self.progress("Turning it back on")
|
||||
self.set_parameter("SIM_FTOWESC_POW", mask)
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
self.progress("Turning ESC=%u off (again)" % mot_id_to_kill)
|
||||
self.set_parameter("SIM_FTOWESC_POW", mask & ~(1 << mot_id_to_kill))
|
||||
self.assert_prearm_failure("are not sending tel")
|
||||
self.progress("Turning it back on")
|
||||
self.set_parameter("SIM_FTOWESC_POW", mask)
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
self.progress("Turning all ESCs off")
|
||||
self.set_parameter("SIM_FTOWESC_POW", 0)
|
||||
self.assert_prearm_failure("are not sending tel")
|
||||
self.progress("Turning them back on")
|
||||
self.set_parameter("SIM_FTOWESC_POW", mask)
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
def fettec_assert_bad_mask(self, mask):
|
||||
'''assert the mask is bad for fettec driver'''
|
||||
self.start_subsubtest("Checking mask (%s) is bad" % (mask,))
|
||||
self.context_push()
|
||||
self.set_parameter("SERVO_FTW_MASK", mask)
|
||||
self.reboot_sitl()
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.get_sim_time_cached() - tstart > 20:
|
||||
raise NotAchievedException("Expected mask to be only problem within 20 seconds")
|
||||
try:
|
||||
self.assert_prearm_failure("Invalid motor mask")
|
||||
break
|
||||
except NotAchievedException:
|
||||
self.delay_sim_time(1)
|
||||
self.context_pop()
|
||||
self.reboot_sitl()
|
||||
|
||||
def fettec_assert_good_mask(self, mask):
|
||||
'''assert the mask is bad for fettec driver'''
|
||||
self.start_subsubtest("Checking mask (%s) is good" % (mask,))
|
||||
self.context_push()
|
||||
self.set_parameter("SERVO_FTW_MASK", mask)
|
||||
self.reboot_sitl()
|
||||
self.wait_ready_to_arm()
|
||||
self.context_pop()
|
||||
self.reboot_sitl()
|
||||
|
||||
def FETtecESC_safety_switch(self):
|
||||
mot = self.find_first_set_bit(int(self.get_parameter("SERVO_FTW_MASK"))) + 1
|
||||
self.wait_esc_telem_rpm(mot, 0, 0)
|
||||
self.wait_ready_to_arm()
|
||||
self.context_push()
|
||||
self.set_parameter("DISARM_DELAY", 0)
|
||||
self.arm_vehicle()
|
||||
# we have to wait for a while for the arming tone to go out
|
||||
# before the motors will spin:
|
||||
self.wait_esc_telem_rpm(
|
||||
esc=mot,
|
||||
rpm_min=17640,
|
||||
rpm_max=17640,
|
||||
minimum_duration=2,
|
||||
timeout=5,
|
||||
)
|
||||
self.set_safetyswitch_on()
|
||||
self.wait_esc_telem_rpm(mot, 0, 0)
|
||||
self.set_safetyswitch_off()
|
||||
self.wait_esc_telem_rpm(
|
||||
esc=mot,
|
||||
rpm_min=17640,
|
||||
rpm_max=17640,
|
||||
minimum_duration=2,
|
||||
timeout=5,
|
||||
)
|
||||
self.context_pop()
|
||||
self.wait_disarmed()
|
||||
|
||||
def FETtecESC_btw_mask_checks(self):
|
||||
'''ensure prearm checks work as expected'''
|
||||
for bad_mask in [0b1000000000000, 0b10100000000000]:
|
||||
self.fettec_assert_bad_mask(bad_mask)
|
||||
for good_mask in [0b00001, 0b00101, 0b110000000000]:
|
||||
self.fettec_assert_good_mask(good_mask)
|
||||
|
||||
def FETtecESC(self):
|
||||
self.set_parameters({
|
||||
"SERIAL5_PROTOCOL": 38,
|
||||
"SERVO_FTW_MASK": 0b11101000,
|
||||
"SIM_FTOWESC_ENA": 1,
|
||||
"SERVO1_FUNCTION": 0,
|
||||
"SERVO2_FUNCTION": 0,
|
||||
"SERVO3_FUNCTION": 0,
|
||||
"SERVO4_FUNCTION": 33,
|
||||
"SERVO5_FUNCTION": 0,
|
||||
"SERVO6_FUNCTION": 34,
|
||||
"SERVO7_FUNCTION": 35,
|
||||
"SERVO8_FUNCTION": 36,
|
||||
})
|
||||
self.customise_SITL_commandline(["--uartF=sim:fetteconewireesc"])
|
||||
self.FETtecESC_safety_switch()
|
||||
self.FETtecESC_esc_power_checks()
|
||||
self.FETtecESC_btw_mask_checks()
|
||||
self.FETtecESC_flight()
|
||||
|
||||
def tests1a(self):
|
||||
'''return list of all tests'''
|
||||
ret = super(AutoTestCopter, self).tests() # about 5 mins and ~20 initial tests from autotest/common.py
|
||||
|
|
@ -7535,6 +7659,10 @@ class AutoTestCopter(AutoTest):
|
|||
"Test Replay",
|
||||
self.test_replay),
|
||||
|
||||
Test("FETtecESC",
|
||||
"Test FETtecESC",
|
||||
self.FETtecESC),
|
||||
|
||||
Test("GroundEffectCompensation_touchDownExpected",
|
||||
"Test EKF's handling of touchdown-expected",
|
||||
self.GroundEffectCompensation_touchDownExpected),
|
||||
|
|
|
|||
|
|
@ -3605,6 +3605,18 @@ class AutoTest(ABC):
|
|||
else:
|
||||
return None
|
||||
|
||||
def set_safetyswitch_on(self):
|
||||
self.set_safetyswitch(1)
|
||||
|
||||
def set_safetyswitch_off(self):
|
||||
self.set_safetyswitch(0)
|
||||
|
||||
def set_safetyswitch(self, value, target_system=1, target_component=1):
|
||||
self.mav.mav.set_mode_send(
|
||||
target_system,
|
||||
mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY,
|
||||
value)
|
||||
|
||||
def armed(self):
|
||||
"""Return true if vehicle is armed and safetyoff"""
|
||||
return self.mav.motors_armed()
|
||||
|
|
@ -4737,6 +4749,38 @@ class AutoTest(ABC):
|
|||
return msg.relative_alt / 1000.0 # mm -> m
|
||||
return msg.alt / 1000.0 # mm -> m
|
||||
|
||||
def get_esc_rpm(self, esc):
|
||||
if esc > 4:
|
||||
raise ValueError("Only does 1-4")
|
||||
m = self.mav.recv_match(type='ESC_TELEMETRY_1_TO_4', blocking=True, timeout=1)
|
||||
if m is None:
|
||||
raise NotAchievedException("Did not get ESC_TELEMETRY_1_TO_4")
|
||||
self.progress("%s" % str(m))
|
||||
return m.rpm[esc-1]
|
||||
|
||||
def find_first_set_bit(self, mask):
|
||||
'''returns offset of first-set-bit (counting from right) in mask. Returns None if no bits set'''
|
||||
pos = 0
|
||||
while mask != 0:
|
||||
if mask & 0x1:
|
||||
return pos
|
||||
mask = mask >> 1
|
||||
pos += 1
|
||||
return None
|
||||
|
||||
def wait_esc_telem_rpm(self, esc, rpm_min, rpm_max, **kwargs):
|
||||
'''wait for ESC to be between rpm_min and rpm_max'''
|
||||
def validator(value2, target2=None):
|
||||
return rpm_min <= value2 <= rpm_max
|
||||
self.wait_and_maintain(
|
||||
value_name="ESC %u RPM" % esc,
|
||||
target=(rpm_min+rpm_max)/2.0,
|
||||
current_value_getter=lambda: self.get_esc_rpm(esc),
|
||||
accuracy=rpm_max-rpm_min,
|
||||
validator=lambda value2, target2: validator(value2, target2),
|
||||
**kwargs
|
||||
)
|
||||
|
||||
def wait_altitude(self, altitude_min, altitude_max, relative=False, timeout=30, **kwargs):
|
||||
"""Wait for a given altitude range."""
|
||||
assert altitude_min <= altitude_max, "Minimum altitude should be less than maximum altitude."
|
||||
|
|
|
|||
Loading…
Reference in New Issue