mirror of https://github.com/ArduPilot/ardupilot
SITL: add simulator Hirth EFI
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
f134a1df34
commit
ca16e924db
|
|
@ -0,0 +1,248 @@
|
|||
/*
|
||||
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 Hirth EFI system
|
||||
*/
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
#include <SITL/SITL.h>
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <stdio.h>
|
||||
#include "SIM_EFI_Hirth.h"
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
// assume SERVO3 is throttle
|
||||
#define HIRTH_RPM_INDEX 2
|
||||
|
||||
void EFI_Hirth::update_receive()
|
||||
{
|
||||
const ssize_t num_bytes_read = read_from_autopilot((char*)&receive_buf[receive_buf_ofs], ARRAY_SIZE(receive_buf) - receive_buf_ofs);
|
||||
if (num_bytes_read < 0) {
|
||||
return;
|
||||
}
|
||||
receive_buf_ofs += num_bytes_read;
|
||||
|
||||
if (receive_buf_ofs < 1) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint8_t expected_bytes_in_message = receive_buf[0];
|
||||
|
||||
if (expected_bytes_in_message == 0) {
|
||||
AP_HAL::panic("zero bytes expected is unexpected");
|
||||
}
|
||||
|
||||
if (expected_bytes_in_message > ARRAY_SIZE(receive_buf)) {
|
||||
AP_HAL::panic("Unexpectedly large byte count");
|
||||
}
|
||||
|
||||
if (receive_buf_ofs < expected_bytes_in_message) {
|
||||
return;
|
||||
}
|
||||
|
||||
// checksum is sum of all bytes except the received checksum:
|
||||
const uint8_t expected_checksum = 256U - crc_sum_of_bytes(receive_buf, expected_bytes_in_message-1);
|
||||
const uint8_t received_checksum = receive_buf[expected_bytes_in_message-1];
|
||||
if (expected_checksum == received_checksum) {
|
||||
PacketCode received_packet_code = PacketCode(receive_buf[1]);
|
||||
if (received_packet_code == PacketCode::SetValues) {
|
||||
// do this synchronously for now
|
||||
handle_set_values();
|
||||
} else {
|
||||
assert_receive_size(3);
|
||||
if (requested_data_record.time_ms != 0) {
|
||||
AP_HAL::panic("Requesting too fast?");
|
||||
}
|
||||
requested_data_record.code = received_packet_code;
|
||||
requested_data_record.time_ms = AP_HAL::millis();
|
||||
}
|
||||
} else {
|
||||
AP_HAL::panic("checksum failed");
|
||||
// simply throw these bytes away. What the actual device does in the
|
||||
// face of weird data is unknown.
|
||||
}
|
||||
memmove(&receive_buf[0], &receive_buf[expected_bytes_in_message], receive_buf_ofs - expected_bytes_in_message);
|
||||
receive_buf_ofs -= expected_bytes_in_message;
|
||||
}
|
||||
|
||||
void EFI_Hirth::assert_receive_size(uint8_t receive_size)
|
||||
{
|
||||
if (receive_buf[0] != receive_size) {
|
||||
AP_HAL::panic("Expected %u message size, got %u message size", receive_size, receive_buf[0]);
|
||||
}
|
||||
}
|
||||
|
||||
void EFI_Hirth::handle_set_values()
|
||||
{
|
||||
assert_receive_size(23);
|
||||
static_assert(sizeof(settings) == 20, "correct number of bytes in settings");
|
||||
memcpy((void*)&settings, &receive_buf[2], sizeof(settings));
|
||||
}
|
||||
|
||||
void EFI_Hirth::update_send()
|
||||
{
|
||||
if (requested_data_record.time_ms == 0) {
|
||||
// no outstanding request
|
||||
return;
|
||||
}
|
||||
if (AP_HAL::millis() - requested_data_record.time_ms < 20) {
|
||||
// 20ms to service a request
|
||||
return;
|
||||
}
|
||||
requested_data_record.time_ms = 0;
|
||||
|
||||
switch (requested_data_record.code) {
|
||||
case PacketCode::DataRecord1:
|
||||
send_record1();
|
||||
break;
|
||||
case PacketCode::DataRecord2:
|
||||
send_record2();
|
||||
break;
|
||||
case PacketCode::DataRecord3:
|
||||
send_record3();
|
||||
break;
|
||||
default:
|
||||
AP_HAL::panic("Unknown data record (%u) requested", (unsigned)requested_data_record.code);
|
||||
}
|
||||
}
|
||||
|
||||
void EFI_Hirth::update_engine_model()
|
||||
{
|
||||
auto sitl = AP::sitl();
|
||||
|
||||
// FIXME: this should come from simulation, not baro. baro gets
|
||||
// warmed by the simulated electronics!
|
||||
const float ambient = AP::baro().get_temperature();
|
||||
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
|
||||
const float delta_t = (now_ms - engine.last_update_ms) * 1e-6;
|
||||
engine.last_update_ms = now_ms;
|
||||
|
||||
// lose heat to environment (air-cooling due to airspeed and prop
|
||||
// airflow could be taken into account here)
|
||||
const float ENV_LOSS_FACTOR = 25;
|
||||
engine.cht1_temperature -= (engine.cht1_temperature - ambient) * delta_t * ENV_LOSS_FACTOR;
|
||||
engine.cht2_temperature -= (engine.cht2_temperature - ambient) * delta_t * ENV_LOSS_FACTOR;
|
||||
|
||||
const float rpm = sitl->state.rpm[HIRTH_RPM_INDEX];
|
||||
const float RPM_GAIN_FACTOR_CHT1 = 10;
|
||||
const float RPM_GAIN_FACTOR_CHT2 = 8;
|
||||
engine.cht1_temperature += rpm * delta_t * RPM_GAIN_FACTOR_CHT1;
|
||||
engine.cht2_temperature += rpm * delta_t * RPM_GAIN_FACTOR_CHT2;
|
||||
}
|
||||
|
||||
void EFI_Hirth::init()
|
||||
{
|
||||
// auto sitl = AP::sitl();
|
||||
|
||||
if (is_zero(AP::baro().get_temperature())) {
|
||||
// defer until the baro has had a chance to update....
|
||||
return;
|
||||
}
|
||||
|
||||
engine.cht1_temperature = AP::baro().get_temperature();
|
||||
engine.cht2_temperature = AP::baro().get_temperature();
|
||||
|
||||
init_done = true;
|
||||
}
|
||||
|
||||
void EFI_Hirth::update()
|
||||
{
|
||||
const auto *sitl = AP::sitl();
|
||||
if (!sitl || sitl->efi_type != SIM::EFI_TYPE_HIRTH) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!init_done) {
|
||||
init();
|
||||
}
|
||||
|
||||
// update throttle; interim thing to make life a little more interesting
|
||||
throttle = 0.9 * throttle + 0.1 * settings.throttle/10;
|
||||
|
||||
update_engine_model();
|
||||
|
||||
update_receive();
|
||||
update_send();
|
||||
}
|
||||
|
||||
uint16_t EFI_Hirth::engine_status_field_value() const
|
||||
{
|
||||
return (
|
||||
0U << 0 | // engine temperature sensor
|
||||
1U << 1 | // air temperature sensor
|
||||
1U << 2 | // air pressure sensor
|
||||
1U << 3 // throttle sensor OK
|
||||
);
|
||||
}
|
||||
|
||||
void SITL::EFI_Hirth::send_record1()
|
||||
{
|
||||
const auto *sitl = AP::sitl();
|
||||
|
||||
// notionally the field updates should happen in the update()
|
||||
// method, but here to save CPU for now:
|
||||
auto &r = packed_record1.record;
|
||||
r.engine_status = engine_status_field_value();
|
||||
r.rpm = sitl->state.rpm[HIRTH_RPM_INDEX];
|
||||
r.air_temperature = AP::baro().get_temperature();
|
||||
r.throttle = settings.throttle / 10; // just echo this back
|
||||
|
||||
packed_record1.update_checksum();
|
||||
|
||||
write_to_autopilot((char*)&packed_record1, sizeof(packed_record1));
|
||||
|
||||
assert_storage_size<Record1, 84> _assert_storage_size_Record1;
|
||||
(void)_assert_storage_size_Record1;
|
||||
}
|
||||
|
||||
void SITL::EFI_Hirth::send_record2()
|
||||
{
|
||||
const auto *sitl = AP::sitl();
|
||||
|
||||
// notionally the field updates should happen in the update()
|
||||
// method, but here to save CPU for now:
|
||||
auto &r = packed_record2.record;
|
||||
r.throttle_percent_times_10 = throttle * 10.0;
|
||||
r.fuel_consumption = ((MAX(sitl->state.rpm[HIRTH_RPM_INDEX] - 1500.0, 0)) /2200.0) * 10; // from log, very rough
|
||||
|
||||
packed_record2.update_checksum();
|
||||
|
||||
write_to_autopilot((char*)&packed_record2, sizeof(packed_record2));
|
||||
|
||||
assert_storage_size<Record2, 98> _assert_storage_size_Record2;
|
||||
(void)_assert_storage_size_Record2;
|
||||
}
|
||||
|
||||
|
||||
void SITL::EFI_Hirth::send_record3()
|
||||
{
|
||||
// notionally the field updates should happen in the update()
|
||||
// method, but here to save CPU for now:
|
||||
auto &r = packed_record3.record;
|
||||
r.excess_temperature_1 = engine.cht1_temperature; // cht1
|
||||
r.excess_temperature_2 = engine.cht2_temperature; // cht2
|
||||
r.excess_temperature_3 = 39; // egt1
|
||||
r.excess_temperature_4 = 41; // egt2
|
||||
|
||||
packed_record3.update_checksum();
|
||||
|
||||
write_to_autopilot((char*)&packed_record3, sizeof(packed_record3));
|
||||
|
||||
assert_storage_size<Record3, 100> _assert_storage_size_Record3;
|
||||
(void)_assert_storage_size_Record3;
|
||||
}
|
||||
|
|
@ -0,0 +1,240 @@
|
|||
/*
|
||||
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 Hirth EFI system
|
||||
|
||||
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:hirth --speedup=1
|
||||
param set SERIAL5_PROTOCOL 24
|
||||
param set SIM_EFI_TYPE 6
|
||||
param set EFI_TYPE 6
|
||||
reboot
|
||||
status EFI_STATUS
|
||||
|
||||
./Tools/autotest/autotest.py --gdb --debug build.Plane test.Plane.Hirth
|
||||
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <SITL/SITL.h>
|
||||
#include <AP_HAL/utility/Socket.h>
|
||||
#include "SIM_SerialDevice.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class EFI_Hirth : public SerialDevice {
|
||||
public:
|
||||
|
||||
using SerialDevice::SerialDevice;
|
||||
|
||||
void update();
|
||||
|
||||
private:
|
||||
|
||||
void update_receive();
|
||||
void update_send();
|
||||
|
||||
void assert_receive_size(uint8_t receive_size);
|
||||
|
||||
void handle_set_values();
|
||||
|
||||
// maps from an on-wire number to a record number:
|
||||
enum class PacketCode : uint8_t {
|
||||
DataRecord1 = 4,
|
||||
DataRecord2 = 11,
|
||||
DataRecord3 = 13,
|
||||
SetValues = 201,
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class PACKED PackedRecord {
|
||||
public:
|
||||
PackedRecord(PacketCode _code, T _record) :
|
||||
code(uint8_t(_code)),
|
||||
record(_record)
|
||||
{ }
|
||||
const uint8_t length { sizeof(T) + 3 }; // 1 each of length, code and checksum
|
||||
const uint8_t code;
|
||||
T record;
|
||||
uint8_t checksum;
|
||||
|
||||
void update() {
|
||||
record.update();
|
||||
update_checksum();
|
||||
}
|
||||
|
||||
void update_checksum() {
|
||||
checksum = 256U - crc_sum_of_bytes((uint8_t*)this, length-1);
|
||||
}
|
||||
};
|
||||
|
||||
void send_record1();
|
||||
void send_record2();
|
||||
void send_record3();
|
||||
|
||||
class PACKED Record1 {
|
||||
public:
|
||||
uint8_t reserved1[2];
|
||||
uint16_t save_in_flash; // "1 = data are saved in flash automatically"
|
||||
uint8_t reserved2[4];
|
||||
uint16_t engine_status;
|
||||
uint16_t rpm;
|
||||
uint8_t reserved3[12];
|
||||
uint16_t number_of_interfering_pulses;
|
||||
uint16_t reserved4[2];
|
||||
uint16_t number_of_speed_errors;
|
||||
uint16_t injection_time;
|
||||
uint16_t ignition_angle;
|
||||
uint16_t reserved5;
|
||||
uint16_t voltage_throttle;
|
||||
uint16_t reserved6;
|
||||
uint8_t reserved7[2];
|
||||
uint16_t voltage_engine_temperature;
|
||||
uint16_t voltage_air_temperature;
|
||||
uint8_t reserved8[2];
|
||||
uint16_t voltage_int_air_pressure;
|
||||
uint8_t reserved9[20];
|
||||
int16_t throttle;
|
||||
int16_t engine_temperature;
|
||||
int16_t battery_voltage;
|
||||
int16_t air_temperature;
|
||||
int16_t reserved10;
|
||||
uint16_t sensor_ok;
|
||||
|
||||
void update();
|
||||
};
|
||||
|
||||
class PACKED Record2 {
|
||||
public:
|
||||
uint8_t reserved1[12];
|
||||
int16_t injection_rate_from_basic_graphic_map;
|
||||
int16_t reserved2;
|
||||
int16_t basic_injection_rate;
|
||||
int16_t injection_rate_from_air_correction;
|
||||
int16_t reserved3;
|
||||
int16_t injection_rate_from_warming_up_characteristic_curve;
|
||||
int16_t injection_rate_from_acceleration_enrichment;
|
||||
int16_t turn_on_time_of_intake_valves;
|
||||
int16_t injection_rate_from_race_switch;
|
||||
int16_t reserved4;
|
||||
int16_t injection_angle_from_ignition_angle_map;
|
||||
int16_t injection_angle_from_air_temperature_characteristic_curve;
|
||||
int16_t injection_angle_from_air_pressure_characteristic_curve;
|
||||
int16_t ignition_angle_from_engine_temperature_characteristic_curve;
|
||||
int16_t ignition_angle_from_acceleration;
|
||||
int16_t ignition_angle_from_race_switch;
|
||||
uint32_t total_time_in_26ms;
|
||||
uint32_t total_number_of_rotations;
|
||||
uint16_t fuel_consumption;
|
||||
uint16_t number_of_errors_in_error_memory;
|
||||
int16_t voltage_input1_throttle_target;
|
||||
int16_t reserved5;
|
||||
int16_t position_throttle_target;
|
||||
uint16_t throttle_percent_times_10; // percent * 10
|
||||
int16_t reserved6[3];
|
||||
uint16_t time_of_injector_opening_percent_times_10;
|
||||
uint8_t reserved7[10];
|
||||
uint32_t no_of_logged_data;
|
||||
uint8_t reserved8[12];
|
||||
};
|
||||
|
||||
class PACKED Record3 {
|
||||
public:
|
||||
uint16_t voltage_excess_temperature_1;
|
||||
uint16_t voltage_excess_temperature_2;
|
||||
uint16_t voltage_excess_temperature_3;
|
||||
uint16_t voltage_excess_temperature_4;
|
||||
uint16_t voltage_excess_temperature_5;
|
||||
uint8_t reserved1[6];
|
||||
uint16_t excess_temperature_1; // cht1
|
||||
uint16_t excess_temperature_2; // cht2
|
||||
uint16_t excess_temperature_3; // egt1
|
||||
uint16_t excess_temperature_4; // egt2
|
||||
uint16_t excess_temperature_5;
|
||||
uint8_t reserved2[6];
|
||||
uint16_t enrichment_excess_temperature_cylinder_1;
|
||||
uint16_t enrichment_excess_temperature_cylinder_2;
|
||||
uint16_t enrichment_excess_temperature_cylinder_3;
|
||||
uint16_t enrichment_excess_temperature_cylinder_4;
|
||||
uint8_t reserved3[6];
|
||||
uint16_t enrichment_excess_temperature_bitfield;
|
||||
uint16_t mixing_ratio_oil_pump1;
|
||||
uint16_t mixing_ratio_oil_pump2;
|
||||
uint16_t ouput_value_water_pump;
|
||||
uint16_t ouput_value_fuel_pump;
|
||||
uint16_t ouput_value_exhaust_valve;
|
||||
uint16_t ouput_value_air_vane;
|
||||
uint16_t ouput_value_e_throttle;
|
||||
uint16_t number_of_injections_oil_pump_1;
|
||||
uint32_t system_time_in_ms;
|
||||
uint16_t number_of_injections_oil_pump_2;
|
||||
uint16_t target_rpm;
|
||||
uint16_t FPC;
|
||||
// these appear to be duplicates of the above; one is probably
|
||||
// voltage?
|
||||
uint16_t xenrichment_excess_temperature_cylinder_1;
|
||||
uint16_t xenrichment_excess_temperature_cylinder_2;
|
||||
uint16_t xenrichment_excess_temperature_cylinder_3;
|
||||
uint16_t xenrichment_excess_temperature_cylinder_4;
|
||||
uint16_t voltage_input_temperature_crankshaft_housing;
|
||||
uint16_t temperature_crankshaft_housing;
|
||||
uint8_t reserved4[14];
|
||||
};
|
||||
|
||||
class PACKED SetValues {
|
||||
public:
|
||||
int16_t throttle; // percent * 10
|
||||
int16_t rpm;;
|
||||
int8_t reserved1[16];
|
||||
};
|
||||
|
||||
// these records are just used for initial values of the fields;
|
||||
// they aren't used past that.
|
||||
Record1 record1;
|
||||
Record2 record2;
|
||||
Record3 record3;
|
||||
|
||||
|
||||
SetValues settings;
|
||||
|
||||
PackedRecord<Record1> packed_record1{PacketCode::DataRecord1, record1};
|
||||
PackedRecord<Record2> packed_record2{PacketCode::DataRecord2, record2};
|
||||
PackedRecord<Record3> packed_record3{PacketCode::DataRecord3, record3};
|
||||
|
||||
struct {
|
||||
PacketCode code; // code which was requested by driver
|
||||
uint32_t time_ms; // time that code was requested by driver
|
||||
} requested_data_record;
|
||||
|
||||
uint8_t receive_buf[32];
|
||||
uint8_t receive_buf_ofs;
|
||||
|
||||
float throttle;
|
||||
|
||||
uint16_t engine_status_field_value() const;
|
||||
|
||||
void init();
|
||||
bool init_done = false;
|
||||
|
||||
// engine model:
|
||||
void update_engine_model();
|
||||
struct {
|
||||
float cht1_temperature; // engine reports in deg-C
|
||||
float cht2_temperature;
|
||||
uint32_t last_update_ms;
|
||||
} engine;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -296,6 +296,7 @@ public:
|
|||
enum EFIType {
|
||||
EFI_TYPE_NONE = 0,
|
||||
EFI_TYPE_MS = 1,
|
||||
EFI_TYPE_HIRTH = 8,
|
||||
};
|
||||
|
||||
AP_Int8 efi_type;
|
||||
|
|
|
|||
Loading…
Reference in New Issue