SITL: added MegaSquirt EFI simulation

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulate MegaSquirt EFI system
+*/
+
+#include "SIM_Aircraft.h"
+#include <SITL/SITL.h>
+#include <AP_HAL/utility/sparse-endian.h>
+#include <stdio.h>
+
+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));
+}

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulate MegaSquirt EFI system
+*/
+
+#pragma once
+
+#include <SITL/SITL.h>
+#include <AP_HAL/utility/Socket.h>
+
+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;
+};
+
+}

libraries/SITL/SITL.cpp

@@ -21,6 +21,9 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Logger/AP_Logger.h>
 
@@ -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

libraries/SITL/SITL.h

@@ -1,5 +1,9 @@
 #pragma once
 
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Common/Location.h>
@@ -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