diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.cpp b/libraries/SITL/SIM_EFI_MegaSquirt.cpp
new file mode 100644
index 0000000000..9520a90c67
--- /dev/null
+++ b/libraries/SITL/SIM_EFI_MegaSquirt.cpp
@@ -0,0 +1,148 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ simulate MegaSquirt EFI system
+*/
+
+#include "SIM_Aircraft.h"
+#include
+#include
+#include
+
+using namespace SITL;
+
+static uint32_t CRC32_MS(const uint8_t *buf, uint32_t len)
+{
+ uint32_t crc = 0;
+ while (len--) {
+ crc ^= ~0U;
+ crc = crc_crc32(crc, buf++, 1);
+ crc ^= ~0U;
+ }
+ return crc;
+}
+
+void EFI_MegaSquirt::update()
+{
+ auto sitl = AP::sitl();
+ if (!sitl || sitl->efi_type == SITL::EFI_TYPE_NONE) {
+ return;
+ }
+ if (!connected) {
+ connected = sock.connect("127.0.0.1", 5763);
+ }
+ if (!connected) {
+ return;
+ }
+ float rpm = sitl->state.rpm1;
+
+ table7.rpm = rpm;
+ table7.fuelload = 20;
+ table7.dwell = 2.0;
+ table7.baro_hPa = 1000;
+ table7.map_hPa = 895;
+ table7.mat_cF = 3013;
+ table7.fuelPressure = 6280;
+ table7.throttle_pos = 580;
+ table7.ct_cF = 3940;
+ table7.afr_target1 = 148;
+
+ if (!sock.pollin(0)) {
+ return;
+ }
+
+ // receive command
+ while (ofs < sizeof(r_command)) {
+ if (sock.recv(&buf[ofs], 1, 0) != 1) {
+ break;
+ }
+ switch (ofs) {
+ case 0:
+ if (buf[ofs] == 0) {
+ ofs++;
+ }
+ break;
+ case 1:
+ if (buf[ofs] != 7) {
+ ofs = 0;
+ } else {
+ ofs++;
+ }
+ break;
+ case 2:
+ if (buf[ofs] != 0x72) {
+ ofs = 0;
+ } else {
+ ofs++;
+ }
+ break;
+ case 3:
+ if (buf[ofs] != 0x00) {
+ ofs = 0;
+ } else {
+ ofs++;
+ }
+ break;
+ case 4:
+ if (buf[ofs] != 0x07) {
+ ofs = 0;
+ } else {
+ ofs++;
+ }
+ break;
+ case 5 ... 12:
+ ofs++;
+ break;
+ }
+ }
+ if (ofs >= sizeof(r_command)) {
+ // check CRC
+ uint32_t crc = CRC32_MS(&buf[2], sizeof(r_command)-6);
+ uint32_t crc2 = be32toh(r_command.crc);
+ if (crc == crc2) {
+ send_table();
+ } else {
+ printf("BAD EFI CRC: 0x%08x 0x%08x\n", crc, crc2);
+ }
+ ofs = 0;
+ }
+}
+
+/*
+ send table response
+ */
+void EFI_MegaSquirt::send_table(void)
+{
+ uint16_t table_offset = be16toh(r_command.table_offset);
+ uint16_t table_size = be16toh(r_command.table_size);
+
+ if (table_offset >= sizeof(table7)) {
+ printf("EFI_MS: bad table_offset %u\n", table_offset);
+ return;
+ }
+ if (table_size+table_offset > sizeof(table7)) {
+ table_size = sizeof(table7) - table_offset;
+ }
+
+ uint16_t len = htobe16(table_size+1);
+ uint8_t outbuf[1+table_size];
+ outbuf[0] = 0;
+ swab(table_offset+(const uint8_t *)&table7, &outbuf[1], table_size);
+
+ sock.send(&len, sizeof(len));
+ sock.send(outbuf, sizeof(outbuf));
+ uint32_t crc = htobe32(CRC32_MS(outbuf, sizeof(outbuf)));
+ sock.send((const uint8_t *)&crc, sizeof(crc));
+}
diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.h b/libraries/SITL/SIM_EFI_MegaSquirt.h
new file mode 100644
index 0000000000..bc199ea88d
--- /dev/null
+++ b/libraries/SITL/SIM_EFI_MegaSquirt.h
@@ -0,0 +1,91 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ simulate MegaSquirt EFI system
+*/
+
+#pragma once
+
+#include
+#include
+
+namespace SITL {
+
+class EFI_MegaSquirt {
+public:
+ void update();
+
+private:
+ void send_table();
+ SocketAPM sock{false};
+
+ uint32_t time_send_ms;
+ bool connected;
+
+ struct PACKED {
+ uint16_t size;
+ uint8_t command;
+ uint8_t CANid;
+ uint8_t table;
+ uint16_t table_offset;
+ uint16_t table_size;
+ uint32_t crc;
+ } r_command;
+ uint8_t *buf = (uint8_t *)&r_command;
+ uint8_t ofs;
+
+ struct PACKED {
+ uint16_t uptime_s;
+ uint16_t pulseWidth1_us;
+ uint16_t pulseWidth2_us;
+ uint16_t rpm;
+ int16_t advance_cdeg;
+ int8_t squirt;
+ int8_t engine_status;
+ uint8_t afr_target1;
+ uint8_t afr_target2;
+ uint8_t wbo2_en1;
+ uint8_t wbo2_en2;
+ int16_t baro_hPa;
+ int16_t map_hPa;
+ int16_t mat_cF;
+ int16_t ct_cF;
+ int16_t throttle_pos;
+ int16_t afr1;
+ int16_t afr2;
+ int16_t knock;
+ int16_t egocor1;
+ int16_t egocor2;
+ int16_t aircor;
+ int16_t warmcor;
+ int16_t accel_enrich;
+ int16_t tps_fuel_cut;
+ int16_t baroCorrection;
+ int16_t gammaEnrich;
+ int16_t ve1;
+ int16_t ve2;
+ int16_t iacstep;
+ int16_t cold_adv_deg;
+ int16_t TPSdot;
+ int16_t MAPdot;
+ int16_t dwell;
+ int16_t MAF;
+ uint8_t fuelload;
+ uint8_t pad[128-67];
+ uint16_t fuelPressure;
+ } table7;
+};
+
+}
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index e111a1829f..c5a6e4ec92 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -21,6 +21,9 @@
#include
#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
#include
#include
@@ -195,6 +198,8 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
// @Path: ./SIM_ToneAlarm.cpp
AP_SUBGROUPINFO(tonealarm_sim, "TA_", 57, SITL, ToneAlarm),
+ AP_GROUPINFO("EFI_TYPE", 58, SITL, efi_type, SITL::EFI_TYPE_NONE),
+
AP_GROUPEND
};
@@ -310,3 +315,5 @@ SITL::SITL *sitl()
}
};
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 7552ad33ec..367d6d3860 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -1,5 +1,9 @@
#pragma once
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
#include
#include
#include
@@ -11,6 +15,7 @@
#include "SIM_Precland.h"
#include "SIM_Sprayer.h"
#include "SIM_ToneAlarm.h"
+#include "SIM_EFI_MegaSquirt.h"
namespace SITL {
@@ -180,6 +185,14 @@ public:
AP_Int8 gps_hdg_enabled; // enable the output of a NMEA heading HDT sentence
AP_Int32 loop_delay; // extra delay to add to every loop
+ // EFI type
+ enum EFIType {
+ EFI_TYPE_NONE = 0,
+ EFI_TYPE_MS = 1,
+ };
+
+ AP_Int8 efi_type;
+
// wind control
enum WindType {
WIND_TYPE_SQRT = 0,
@@ -311,6 +324,8 @@ public:
uint8_t num_leds[16];
uint32_t send_counter;
} led;
+
+ EFI_MegaSquirt efi_ms;
};
} // namespace SITL
@@ -319,3 +334,5 @@ public:
namespace AP {
SITL::SITL *sitl();
};
+
+#endif // CONFIG_HAL_BOARD