Browse Source

SITL: move simulated megasquirt to SerialDevice framework

gps-1.3.1
Peter Barker 3 years ago committed by Peter Barker
parent
commit
05c6a25e52
  1. 18
      libraries/SITL/SIM_EFI_MegaSquirt.cpp
  2. 18
      libraries/SITL/SIM_EFI_MegaSquirt.h
  3. 2
      libraries/SITL/SITL.h

18
libraries/SITL/SIM_EFI_MegaSquirt.cpp

@ -40,12 +40,6 @@ void EFI_MegaSquirt::update() @@ -40,12 +40,6 @@ void EFI_MegaSquirt::update()
if (!sitl || sitl->efi_type == SIM::EFI_TYPE_NONE) {
return;
}
if (!connected) {
connected = sock.connect("127.0.0.1", 5763);
}
if (!connected) {
return;
}
float rpm = sitl->state.rpm[0];
table7.rpm = rpm;
@ -59,13 +53,9 @@ void EFI_MegaSquirt::update() @@ -59,13 +53,9 @@ void EFI_MegaSquirt::update()
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) {
if (read_from_autopilot((char*)&buf[ofs], 1) != 1) {
break;
}
switch (ofs) {
@ -141,8 +131,8 @@ void EFI_MegaSquirt::send_table(void) @@ -141,8 +131,8 @@ void EFI_MegaSquirt::send_table(void)
outbuf[0] = 0;
swab(table_offset+(const uint8_t *)&table7, &outbuf[1], table_size);
sock.send(&len, sizeof(len));
sock.send(outbuf, sizeof(outbuf));
write_to_autopilot((const char*)&len, sizeof(len));
write_to_autopilot((const char*)outbuf, sizeof(outbuf));
uint32_t crc = htobe32(CRC32_MS(outbuf, sizeof(outbuf)));
sock.send((const uint8_t *)&crc, sizeof(crc));
write_to_autopilot((const char *)&crc, sizeof(crc));
}

18
libraries/SITL/SIM_EFI_MegaSquirt.h

@ -14,25 +14,37 @@ @@ -14,25 +14,37 @@
*/
/*
simulate MegaSquirt EFI system
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:megasquirt --speedup=1
param set SERIAL5_PROTOCOL 24
param set SIM_EFI_TYPE 1
param set EFI_TYPE 1
reboot
status EFI_STATUS
./Tools/autotest/autotest.py --gdb --debug build.Plane test.Plane.MegaSquirt
*/
#pragma once
#include <SITL/SITL.h>
#include <AP_HAL/utility/Socket.h>
#include "SIM_SerialDevice.h"
namespace SITL {
class EFI_MegaSquirt {
class EFI_MegaSquirt : public SerialDevice {
public:
using SerialDevice::SerialDevice;
void update();
private:
void send_table();
SocketAPM sock{false};
uint32_t time_send_ms;
bool connected;
struct PACKED {
uint16_t size;

2
libraries/SITL/SITL.h

@ -439,8 +439,6 @@ public: @@ -439,8 +439,6 @@ public:
uint32_t send_counter;
} led;
EFI_MegaSquirt efi_ms;
AP_Int8 led_layout;
// vicon parameters

Loading…
Cancel
Save