From dab3b93dc3e378143fbd3f8099fb88d058947046 Mon Sep 17 00:00:00 2001 From: Guilherme Sousa Date: Mon, 10 Apr 2017 19:48:48 +0200 Subject: [PATCH] autotest: Improved motor failure test This is based on https://github.com/ArduPilot/ardupilot/pull/6028 by GuilhermeGSousa --- Tools/autotest/arducopter.py | 126 +++++++++++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 186877a440..66b354741e 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -992,6 +992,117 @@ class AutoTestCopter(AutoTest): self.progress("AVC mission 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_mission(self, height_accuracy=-1.0, target_altitude=None): """Fly a mission from a file.""" global num_wp @@ -1154,6 +1265,21 @@ class AutoTestCopter(AutoTest): # RTL self.run_test("RTL after CIRCLE mode", self.fly_RTL) + # Arm + self.set_rc(3, 1000) + self.mavproxy.send('mode stabilize\n') # stabilize mode + self.wait_mode('STABILIZE') + + # Takeoff + self.run_test("Takeoff to test motor failure", + lambda: self.takeoff(10, arm=True)) + + self.run_test("Fly motor failure test", + self.fly_motor_fail) + + # RTL + self.run_test("RTL after motor failure test", self.fly_RTL) + # Arm self.set_rc(3, 1000) self.mavproxy.send('mode stabilize\n') # stabilize mode