Add EFI Hirth simulator

Tools/autotest/arduplane.py

@@ -8,7 +8,6 @@
 import math
 import os
 import signal
-import sys
 import time
 
 from pymavlink import quaternion
@@ -4199,24 +4198,81 @@ def ForcedDCM(self):
 
         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(6, "Hirth", "hirth")
 
     def GlideSlopeThresh(self):
         '''Test rebuild glide slope if above and climbing'''
@@ -5313,6 +5369,7 @@ def tests(self):
             self.AUTOTUNE,
             self.AutotuneFiltering,
             self.MegaSquirt,
+            self.Hirth,
             self.MSP_DJI,
             self.SpeedToFly,
             self.GlideSlopeThresh,

Tools/autotest/vehicle_test_suite.py

@@ -3915,16 +3915,30 @@ def assert_received_message_field_values(self,
         return m
 
     # FIXME: try to use wait_and_maintain here?
-    def wait_message_field_values(self, message, fieldvalues, timeout=10, epsilon=None, instance=None, minimum_duration=None):
+    def wait_message_field_values(self,
+                                  message,
+                                  fieldvalues,
+                                  timeout=10,
+                                  epsilon=None,
+                                  instance=None,
+                                  minimum_duration=None,
+                                  verbose=False,
+                                  very_verbose=False,
+                                  ):
 
         tstart = self.get_sim_time_cached()
         pass_start = None
         while True:
             now = self.get_sim_time_cached()
             if now - tstart > timeout:
                 raise NotAchievedException("Field never reached values")
-            m = self.assert_receive_message(message, instance=instance)
-            if self.message_has_field_values(m, fieldvalues, epsilon=epsilon):
+            m = self.assert_receive_message(
+                message,
+                instance=instance,
+                verbose=verbose,
+                very_verbose=very_verbose,
+            )
+            if self.message_has_field_values(m, fieldvalues, epsilon=epsilon, verbose=verbose):
                 if minimum_duration is not None:
                     if pass_start is None:
                         pass_start = now

libraries/AP_HAL/SIMState.h

@@ -33,6 +33,8 @@
 #include <SITL/SIM_AIS.h>
 #include <SITL/SIM_GPS.h>
 
+#include <SITL/SIM_EFI_Hirth.h>
+
 #include <SITL/SIM_Frsky_D.h>
 #include <SITL/SIM_CRSF.h>
 #include <SITL/SIM_PS_RPLidarA2.h>
@@ -206,6 +208,9 @@ class AP_HAL::SIMState {
     // simulated EFI MegaSquirt device:
     SITL::EFI_MegaSquirt *efi_ms;
 
+    // simulated EFI Hirth device:
+    SITL::EFI_Hirth *efi_hirth;
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     // output socket for flightgear viewing
     SocketAPM fg_socket{true};

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -245,6 +245,12 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
         }
         efi_ms = new SITL::EFI_MegaSquirt();
         return efi_ms;
+    } else if (streq(name, "hirth")) {
+        if (efi_hirth != nullptr) {
+            AP_HAL::panic("Only one hirth at a time");
+        }
+        efi_hirth = new SITL::EFI_Hirth();
+        return efi_hirth;
     } else if (streq(name, "VectorNav")) {
         if (vectornav != nullptr) {
             AP_HAL::panic("Only one VectorNav at a time");
@@ -363,6 +369,9 @@ void SITL_State_Common::sim_update(void)
     if (efi_ms != nullptr) {
         efi_ms->update();
     }
+    if (efi_hirth != nullptr) {
+        efi_hirth->update();
+    }
 
     if (frsky_d != nullptr) {
         frsky_d->update();

libraries/AP_HAL_SITL/SITL_State_common.h

@@ -11,6 +11,7 @@
 #include <SITL/SIM_Gimbal.h>
 #include <SITL/SIM_ADSB.h>
 #include <SITL/SIM_ADSB_Sagetech_MXS.h>
+#include <SITL/SIM_EFI_Hirth.h>
 #include <SITL/SIM_Vicon.h>
 #include <SITL/SIM_RF_Benewake_TF02.h>
 #include <SITL/SIM_RF_Benewake_TF03.h>
@@ -209,6 +210,9 @@ class HALSITL::SITL_State_Common {
     // simulated EFI MegaSquirt device:
     SITL::EFI_MegaSquirt *efi_ms;
 
+    // simulated EFI MegaSquirt device:
+    SITL::EFI_Hirth *efi_hirth;
+
     // output socket for flightgear viewing
     SocketAPM fg_socket{true};
 

libraries/AP_Math/crc.cpp

@@ -578,3 +578,14 @@ uint8_t parity(uint8_t byte)
 
     return p;
 }
+
+// sums the bytes in the supplied buffer, returns that sum mod 256
+// (i.e. shoved into a uint8_t)
+uint8_t sum_of_bytes_in_buffer_mod_256(uint8_t *data, uint16_t count)
+{
+    uint8_t ret = 0;
+    for (uint8_t i=0; i<count; i++) {
+        ret += data[i];
+    }
+    return ret;
+}

libraries/AP_Math/crc.h

@@ -60,3 +60,7 @@ uint64_t crc_crc64(const uint32_t *data, uint16_t num_words);
 // return the parity of byte - "1" if there is an odd number of bits
 // set, "0" if there is an even number of bits set
 uint8_t parity(uint8_t byte);
+
+// sums the bytes in the supplied buffer, returns that sum mod 256
+// (i.e. shoved into a uint8_t)
+uint8_t sum_of_bytes_in_buffer_mod_256(uint8_t *data, uint16_t count);

libraries/AP_Math/definitions.h

@@ -74,7 +74,9 @@ static const double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F));
 
 #define C_TO_KELVIN(temp) (temp + 273.15f)
 #define KELVIN_TO_C(temp) (temp - 273.15f)
-#define F_TO_KELVIN(temp) C_TO_KELVIN(((temp - 32) * 5/9))
+#define F_TO_C(temp) ((temp - 32) * 5/9)
+#define F_TO_KELVIN(temp) C_TO_KELVIN(F_TO_C(temp))
+#define C_TO_F(temp) ((temp * 9/5) + 32)
 
 #define M_PER_SEC_TO_KNOTS 1.94384449f
 #define KNOTS_TO_M_PER_SEC (1/M_PER_SEC_TO_KNOTS)