mirror of https://github.com/ArduPilot/ardupilot
SITL: added MegaSquirt EFI simulation
This commit is contained in:
parent
eb582ac0cc
commit
4d50996780
|
@ -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));
|
||||
}
|
|
@ -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;
|
||||
};
|
||||
|
||||
}
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue