From 6c0d540afe78a39b60863bcf035c80ac5e479238 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 9 Nov 2023 14:56:36 +1100 Subject: [PATCH] autotest: add simulator Hirth EFI --- Tools/autotest/arduplane.py | 87 ++++++++++++++++++++++++++++++------- 1 file changed, 72 insertions(+), 15 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index a580e70d5e..02c16a8482 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -8,7 +8,6 @@ from __future__ import print_function import math import os import signal -import sys import time from pymavlink import quaternion @@ -4199,24 +4198,81 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.fly_home_land_and_disarm() - def MegaSquirt(self): - '''Test MegaSquirt EFI''' + def EFITest(self, efi_type, name, sim_name, check_fuel_flow=True): + '''method to be called by EFI tests''' + self.start_subtest("EFI Test for (%s)" % name) self.assert_not_receiving_message('EFI_STATUS') self.set_parameters({ - 'SIM_EFI_TYPE': 1, - 'EFI_TYPE': 1, + 'SIM_EFI_TYPE': efi_type, + 'EFI_TYPE': efi_type, 'SERIAL5_PROTOCOL': 24, + 'RPM1_TYPE': 10, }) - self.customise_SITL_commandline(["--uartF=sim:megasquirt"]) - self.delay_sim_time(5) - m = self.assert_receive_message('EFI_STATUS') - mavutil.dump_message_verbose(sys.stdout, m) - if m.throttle_out != 0: - raise NotAchievedException("Expected zero throttle") - if m.health != 1: - raise NotAchievedException("Not healthy") - if m.intake_manifold_temperature < 20: - raise NotAchievedException("Bad intake manifold temperature") + + self.customise_SITL_commandline( + ["--uartF=sim:%s" % sim_name, + ], + ) + self.wait_ready_to_arm() + + baro_m = self.assert_receive_message("SCALED_PRESSURE") + self.progress(self.dump_message_verbose(baro_m)) + baro_temperature = baro_m.temperature / 100.0 # cDeg->deg + m = self.assert_received_message_field_values("EFI_STATUS", { + "throttle_out": 0, + "health": 1, + }, very_verbose=1) + + if abs(baro_temperature - m.intake_manifold_temperature) > 1: + raise NotAchievedException( + "Bad intake manifold temperature (want=%f got=%f)" % + (baro_temperature, m.intake_manifold_temperature)) + + self.arm_vehicle() + + self.set_rc(3, 1300) + + tstart = self.get_sim_time() + while True: + now = self.get_sim_time_cached() + if now - tstart > 10: + raise NotAchievedException("RPM1 and EFI_STATUS.rpm did not match") + rpm_m = self.assert_receive_message("RPM", verbose=1) + want_rpm = 1000 + if rpm_m.rpm1 < want_rpm: + continue + + m = self.assert_receive_message("EFI_STATUS", verbose=1) + if abs(m.rpm - rpm_m.rpm1) > 100: + continue + + break + + self.progress("now we're started, check a few more values") + # note that megasquirt drver doesn't send throttle, so throttle_out is zero! + m = self.assert_received_message_field_values("EFI_STATUS", { + "health": 1, + }, very_verbose=1) + m = self.wait_message_field_values("EFI_STATUS", { + "throttle_position": 31, + "intake_manifold_temperature": 28, + }, very_verbose=1, epsilon=2) + if check_fuel_flow: + if abs(m.fuel_flow - 0.2) > 0.0001: + raise NotAchievedException("Expected fuel flow") + + self.disarm_vehicle() + + def MegaSquirt(self): + '''test MegaSquirt driver''' + self.EFITest( + 1, "MegaSquirt", "megasquirt", + check_fuel_flow=False, + ) + + def Hirth(self): + '''Test Hirth EFI''' + self.EFITest(8, "Hirth", "hirth") def GlideSlopeThresh(self): '''Test rebuild glide slope if above and climbing''' @@ -5313,6 +5369,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.AUTOTUNE, self.AutotuneFiltering, self.MegaSquirt, + self.Hirth, self.MSP_DJI, self.SpeedToFly, self.GlideSlopeThresh,