mirror of https://github.com/ArduPilot/ardupilot
AP_EFI : Hirth Driver Addition
Implementation for Hirth. - Base class - AP_EFI - polynomial functional throttle linearization - AP_EFI_State parameter addition and changes for hirth logging - to fix autotest errors - updated comments - Hirth CI/CD autotest fail fixes - logging - fix CI issues
This commit is contained in:
Pradeep CK
Andrew Tridgell
parent
6bf3debe73
commit
71141080a1
|
|
@ -22,6 +22,7 @@
|
|||
#include "AP_EFI_NWPMU.h"
|
||||
#include "AP_EFI_DroneCAN.h"
|
||||
#include "AP_EFI_Currawong_ECU.h"
|
||||
#include "AP_EFI_Serial_Hirth.h"
|
||||
#include "AP_EFI_Scripting.h"
|
||||
#include "AP_EFI_MAV.h"
|
||||
|
||||
|
|
@ -39,7 +40,7 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = {
|
|||
// @Param: _TYPE
|
||||
// @DisplayName: EFI communication type
|
||||
// @Description: What method of communication is used for EFI #1
|
||||
// @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan,5:DroneCAN,6:Currawong-ECU,7:Scripting,9:MAV
|
||||
// @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan,5:DroneCAN,6:Currawong-ECU,7:Scripting,8:Hirth,9:MAV
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
|
@ -66,6 +67,12 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = {
|
|||
// @User: Advanced
|
||||
AP_GROUPINFO("_FUEL_DENS", 4, AP_EFI, ecu_fuel_density, 0),
|
||||
|
||||
#if AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
// @Group: _THRLIN
|
||||
// @Path: AP_EFI_ThrottleLinearisation.cpp
|
||||
AP_SUBGROUPINFO(throttle_linearisation, "_THRLIN", 5, AP_EFI, AP_EFI_ThrLin),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -118,6 +125,11 @@ void AP_EFI::init(void)
|
|||
backend = new AP_EFI_Scripting(*this);
|
||||
break;
|
||||
#endif
|
||||
#if AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
case Type::Hirth:
|
||||
backend = new AP_EFI_Serial_Hirth(*this);
|
||||
break;
|
||||
#endif
|
||||
#if AP_EFI_MAV_ENABLED
|
||||
case Type::MAV:
|
||||
backend = new AP_EFI_MAV(*this);
|
||||
|
|
@ -232,12 +244,14 @@ void AP_EFI::log_status(void)
|
|||
// @Field: CHT: Cylinder head temperature
|
||||
// @Field: EGT: Exhaust gas temperature
|
||||
// @Field: Lambda: Estimated lambda coefficient (dimensionless ratio)
|
||||
// @Field: CHT2: Cylinder2 head temperature
|
||||
// @Field: EGT2: Cylinder2 Exhaust gas temperature
|
||||
// @Field: IDX: Index of the publishing ECU
|
||||
AP::logger().WriteStreaming("ECYL",
|
||||
"TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,IDX",
|
||||
"s#dsOO--",
|
||||
"F-0C0000",
|
||||
"QBfffffB",
|
||||
"TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,CHT2,EGT2,IDX",
|
||||
"s#dsOO-OO-",
|
||||
"F-0C000000",
|
||||
"QBfffffBff",
|
||||
AP_HAL::micros64(),
|
||||
0,
|
||||
state.cylinder_status.ignition_timing_deg,
|
||||
|
|
@ -245,6 +259,8 @@ void AP_EFI::log_status(void)
|
|||
state.cylinder_status.cylinder_head_temperature,
|
||||
state.cylinder_status.exhaust_gas_temperature,
|
||||
state.cylinder_status.lambda_coefficient,
|
||||
state.cylinder_status.cylinder_head_temperature2,
|
||||
state.cylinder_status.exhaust_gas_temperature2,
|
||||
state.ecu_index);
|
||||
}
|
||||
#endif // LOGGING_ENABLED
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include "AP_EFI_ThrottleLinearisation.h"
|
||||
|
||||
#include "AP_EFI_Backend.h"
|
||||
#include "AP_EFI_State.h"
|
||||
|
|
@ -96,7 +97,9 @@ public:
|
|||
#if AP_EFI_SCRIPTING_ENABLED
|
||||
SCRIPTING = 7,
|
||||
#endif
|
||||
// Hirth = 8 /* Reserved for future implementation */
|
||||
#if AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
Hirth = 8,
|
||||
#endif
|
||||
MAV = 9,
|
||||
};
|
||||
|
||||
|
|
@ -123,6 +126,10 @@ protected:
|
|||
|
||||
EFI_State state;
|
||||
|
||||
#if AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
AP_EFI_ThrLin throttle_linearisation;
|
||||
#endif
|
||||
|
||||
private:
|
||||
// Front End Parameters
|
||||
AP_Enum<Type> type;
|
||||
|
|
|
|||
|
|
@ -42,6 +42,11 @@ float AP_EFI_Backend::get_coef2(void) const
|
|||
return frontend.coef2;
|
||||
}
|
||||
|
||||
void AP_EFI_Backend::set_default_coef1(float coef1)
|
||||
{
|
||||
frontend.coef1.set_default(coef1);
|
||||
}
|
||||
|
||||
HAL_Semaphore &AP_EFI_Backend::get_sem(void)
|
||||
{
|
||||
return frontend.sem;
|
||||
|
|
@ -51,4 +56,15 @@ float AP_EFI_Backend::get_ecu_fuel_density(void) const
|
|||
{
|
||||
return frontend.ecu_fuel_density;
|
||||
}
|
||||
|
||||
#if AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
/*
|
||||
linearise throttle if enabled
|
||||
*/
|
||||
float AP_EFI_Backend::linearise_throttle(float throttle_percent)
|
||||
{
|
||||
return frontend.throttle_linearisation.linearise_throttle(throttle_percent);
|
||||
}
|
||||
#endif // AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
|
||||
#endif // HAL_EFI_ENABLED
|
||||
|
|
|
|||
|
|
@ -49,8 +49,16 @@ protected:
|
|||
int8_t get_dronecan_node_id(void) const;
|
||||
float get_coef1(void) const;
|
||||
float get_coef2(void) const;
|
||||
|
||||
void set_default_coef1(float coef1);
|
||||
|
||||
float get_ecu_fuel_density(void) const;
|
||||
|
||||
/*
|
||||
linearise throttle if enabled
|
||||
*/
|
||||
float linearise_throttle(float throttle_percent);
|
||||
|
||||
HAL_Semaphore &get_sem(void);
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_EFI_DroneCAN.h"
|
||||
#include "AP_EFI_config.h"
|
||||
|
||||
#if AP_EFI_DRONECAN_ENABLED
|
||||
#include "AP_EFI_DroneCAN.h"
|
||||
|
||||
#include <AP_CANManager/AP_CANManager.h>
|
||||
#include <AP_DroneCAN/AP_DroneCAN.h>
|
||||
|
|
|
|||
|
|
@ -1,9 +1,10 @@
|
|||
#pragma once
|
||||
|
||||
#include "AP_EFI.h"
|
||||
#include "AP_EFI_Backend.h"
|
||||
#include "AP_EFI_config.h"
|
||||
|
||||
#if AP_EFI_DRONECAN_ENABLED
|
||||
#include "AP_EFI.h"
|
||||
#include "AP_EFI_Backend.h"
|
||||
#include <AP_DroneCAN/AP_DroneCAN.h>
|
||||
|
||||
class AP_EFI_DroneCAN : public AP_EFI_Backend {
|
||||
|
|
|
|||
|
|
@ -1,7 +1,10 @@
|
|||
#include "AP_EFI_Scripting.h"
|
||||
#include "AP_EFI_config.h"
|
||||
|
||||
#if AP_EFI_SCRIPTING_ENABLED
|
||||
|
||||
#include "AP_EFI_Scripting.h"
|
||||
|
||||
|
||||
// Called from frontend to update with the readings received by handler
|
||||
void AP_EFI_Scripting::update()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -0,0 +1,405 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "AP_EFI_config.h"
|
||||
|
||||
#if AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_EFI/AP_EFI_Serial_Hirth.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include <AP_ICEngine/AP_ICEngine.h>
|
||||
#include <AP_Math/definitions.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#define HIRTH_MAX_PKT_SIZE 100
|
||||
#define HIRTH_MAX_RAW_PKT_SIZE 103
|
||||
|
||||
#define SERIAL_WAIT_TIMEOUT_MS 100
|
||||
|
||||
#define ENGINE_RUNNING 4
|
||||
#define THROTTLE_POSITION_FACTOR 10
|
||||
#define CRANK_SHAFT_SENSOR_OK 0x0F
|
||||
#define INJECTION_TIME_RESOLUTION 0.8
|
||||
#define THROTTLE_POSITION_RESOLUTION 0.1
|
||||
#define VOLTAGE_RESOLUTION 0.0049 /* 5/1024 */
|
||||
#define ADC_CALIBRATION (5.0/1024.0)
|
||||
#define MAP_HPA_PER_VOLT_FACTOR 248.2673
|
||||
#define HPA_TO_KPA 0.1
|
||||
#define TPS_SCALE 0.70
|
||||
|
||||
// request/response status constants
|
||||
#define QUANTITY_REQUEST_STATUS 0x03
|
||||
#define QUANTITY_SET_VALUE 0x17
|
||||
#define CODE_REQUEST_STATUS_1 0x04
|
||||
#define CODE_REQUEST_STATUS_2 0x0B
|
||||
#define CODE_REQUEST_STATUS_3 0x0D
|
||||
#define CODE_SET_VALUE 0xC9
|
||||
#define CHECKSUM_REQUEST_STATUS_1 0xF9
|
||||
#define CHECKSUM_REQUEST_STATUS_2 0xF2
|
||||
#define CHECKSUM_REQUEST_STATUS_3 0xF0
|
||||
#define QUANTITY_RESPONSE_STATUS_1 0x57
|
||||
#define QUANTITY_RESPONSE_STATUS_2 0x65
|
||||
#define QUANTITY_RESPONSE_STATUS_3 0x67
|
||||
#define QUANTITY_ACK_SET_VALUES 0x03
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/**
|
||||
* @brief Constructor with port initialization
|
||||
*
|
||||
* @param _frontend
|
||||
*/
|
||||
AP_EFI_Serial_Hirth::AP_EFI_Serial_Hirth(AP_EFI &_frontend) :
|
||||
AP_EFI_Backend(_frontend)
|
||||
{
|
||||
port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0);
|
||||
set_default_coef1(1.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief checks for response from or makes requests to Hirth ECU periodically
|
||||
*
|
||||
*/
|
||||
void AP_EFI_Serial_Hirth::update()
|
||||
{
|
||||
if (port == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// parse response from Hirth
|
||||
check_response();
|
||||
|
||||
// send request
|
||||
send_request();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if required bytes are available and proceeds with parsing
|
||||
*
|
||||
*/
|
||||
void AP_EFI_Serial_Hirth::check_response()
|
||||
{
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
||||
// waiting for response
|
||||
if (!waiting_response) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t num_bytes = port->available();
|
||||
|
||||
// if already requested
|
||||
if (num_bytes >= expected_bytes) {
|
||||
// read data from buffer
|
||||
uint8_t computed_checksum = 0;
|
||||
computed_checksum += res_data.quantity = port->read();
|
||||
computed_checksum += res_data.code = port->read();
|
||||
|
||||
if (res_data.code == requested_code) {
|
||||
for (int i = 0; i < (res_data.quantity - QUANTITY_REQUEST_STATUS); i++) {
|
||||
computed_checksum += raw_data[i] = port->read();
|
||||
}
|
||||
}
|
||||
|
||||
res_data.checksum = port->read();
|
||||
if (res_data.checksum != (256 - computed_checksum)) {
|
||||
crc_fail_cnt++;
|
||||
port->discard_input();
|
||||
} else {
|
||||
uptime = now - last_packet_ms;
|
||||
last_packet_ms = now;
|
||||
internal_state.last_updated_ms = now;
|
||||
decode_data();
|
||||
copy_to_frontend();
|
||||
port->discard_input();
|
||||
}
|
||||
|
||||
waiting_response = false;
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
log_status();
|
||||
#endif
|
||||
}
|
||||
|
||||
// reset request if no response for SERIAL_WAIT_TIMEOUT_MS
|
||||
if (waiting_response &&
|
||||
now - last_request_ms > SERIAL_WAIT_TIMEOUT_MS) {
|
||||
waiting_response = false;
|
||||
last_request_ms = now;
|
||||
|
||||
port->discard_input();
|
||||
ack_fail_cnt++;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send Throttle and Telemetry requests to Hirth
|
||||
*
|
||||
*/
|
||||
void AP_EFI_Serial_Hirth::send_request()
|
||||
{
|
||||
if (waiting_response) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
bool request_was_sent;
|
||||
|
||||
// get new throttle value
|
||||
const uint16_t new_throttle = (uint16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
||||
|
||||
// check for change or timeout for throttle value
|
||||
if ((new_throttle != last_throttle) || (now - last_req_send_throttle_ms > 500)) {
|
||||
// send new throttle value, only when ARMED
|
||||
bool allow_throttle = hal.util->get_soft_armed();
|
||||
if (!allow_throttle) {
|
||||
#if AP_ICENGINE_ENABLED
|
||||
const auto *ice = AP::ice();
|
||||
if (ice != nullptr) {
|
||||
allow_throttle = ice->allow_throttle_while_disarmed();
|
||||
}
|
||||
#endif // AP_ICENGINE_ENABLED
|
||||
}
|
||||
if (allow_throttle) {
|
||||
request_was_sent = send_target_values(new_throttle);
|
||||
} else {
|
||||
request_was_sent = send_target_values(0);
|
||||
}
|
||||
|
||||
last_throttle = new_throttle;
|
||||
last_req_send_throttle_ms = now;
|
||||
} else {
|
||||
// request Status request at the driver update rate if no throttle commands
|
||||
request_was_sent = send_request_status();
|
||||
}
|
||||
|
||||
if (request_was_sent) {
|
||||
waiting_response = true;
|
||||
last_request_ms = now;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief sends the new throttle command to Hirth ECU
|
||||
*
|
||||
* @param thr - new throttle value given by SRV_Channel::k_throttle
|
||||
* @return true - if success
|
||||
* @return false - currently not implemented
|
||||
*/
|
||||
bool AP_EFI_Serial_Hirth::send_target_values(uint16_t thr)
|
||||
{
|
||||
uint8_t computed_checksum = 0;
|
||||
|
||||
// clear buffer
|
||||
memset(raw_data, 0, ARRAY_SIZE(raw_data));
|
||||
|
||||
#if AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
// linearise throttle input
|
||||
thr = linearise_throttle(thr);
|
||||
#endif
|
||||
|
||||
const uint16_t throttle = thr * THROTTLE_POSITION_FACTOR;
|
||||
|
||||
uint8_t idx = 0;
|
||||
|
||||
// set Quantity + Code + "20 bytes of records to set" + Checksum
|
||||
computed_checksum += raw_data[idx++] = QUANTITY_SET_VALUE;
|
||||
computed_checksum += raw_data[idx++] = requested_code = CODE_SET_VALUE;
|
||||
computed_checksum += raw_data[idx++] = throttle & 0xFF;
|
||||
computed_checksum += raw_data[idx++] = (throttle >> 8) & 0xFF;
|
||||
|
||||
// checksum calculation
|
||||
raw_data[QUANTITY_SET_VALUE - 1] = (256 - computed_checksum);
|
||||
|
||||
expected_bytes = QUANTITY_ACK_SET_VALUES;
|
||||
// write data
|
||||
port->write(raw_data, QUANTITY_SET_VALUE);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief cyclically sends different Status requests to Hirth ECU
|
||||
*
|
||||
* @return true - when successful
|
||||
* @return false - not implemented
|
||||
*/
|
||||
bool AP_EFI_Serial_Hirth::send_request_status() {
|
||||
|
||||
uint8_t requested_quantity;
|
||||
uint8_t requested_checksum;
|
||||
|
||||
switch (requested_code)
|
||||
{
|
||||
case CODE_REQUEST_STATUS_1:
|
||||
requested_quantity = QUANTITY_REQUEST_STATUS;
|
||||
requested_code = CODE_REQUEST_STATUS_2;
|
||||
requested_checksum = CHECKSUM_REQUEST_STATUS_2;
|
||||
expected_bytes = QUANTITY_RESPONSE_STATUS_2;
|
||||
break;
|
||||
case CODE_REQUEST_STATUS_2:
|
||||
requested_quantity = QUANTITY_REQUEST_STATUS;
|
||||
requested_code = CODE_REQUEST_STATUS_3;
|
||||
requested_checksum = CHECKSUM_REQUEST_STATUS_3;
|
||||
expected_bytes = QUANTITY_RESPONSE_STATUS_3;
|
||||
break;
|
||||
case CODE_REQUEST_STATUS_3:
|
||||
requested_quantity = QUANTITY_REQUEST_STATUS;
|
||||
requested_code = CODE_REQUEST_STATUS_1;
|
||||
requested_checksum = CHECKSUM_REQUEST_STATUS_1;
|
||||
expected_bytes = QUANTITY_RESPONSE_STATUS_1;
|
||||
break;
|
||||
default:
|
||||
requested_quantity = QUANTITY_REQUEST_STATUS;
|
||||
requested_code = CODE_REQUEST_STATUS_1;
|
||||
requested_checksum = CHECKSUM_REQUEST_STATUS_1;
|
||||
expected_bytes = QUANTITY_RESPONSE_STATUS_1;
|
||||
break;
|
||||
}
|
||||
raw_data[0] = requested_quantity;
|
||||
raw_data[1] = requested_code;
|
||||
raw_data[2] = requested_checksum;
|
||||
|
||||
port->write(raw_data, QUANTITY_REQUEST_STATUS);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief parses the response from Hirth ECU and updates the internal state instance
|
||||
*
|
||||
*/
|
||||
void AP_EFI_Serial_Hirth::decode_data()
|
||||
{
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
||||
switch (res_data.code) {
|
||||
case CODE_REQUEST_STATUS_1: {
|
||||
struct Record1 *record1 = (Record1*)raw_data;
|
||||
|
||||
internal_state.engine_speed_rpm = record1->rpm;
|
||||
internal_state.throttle_out = record1->throttle;
|
||||
|
||||
// EFI2 log
|
||||
internal_state.engine_state = (Engine_State)record1->engine_status;
|
||||
internal_state.crankshaft_sensor_status = (record1->sensor_ok & CRANK_SHAFT_SENSOR_OK) ? Crankshaft_Sensor_Status::OK : Crankshaft_Sensor_Status::ERROR;
|
||||
|
||||
// ECYL log
|
||||
internal_state.cylinder_status.injection_time_ms = record1->injection_time * INJECTION_TIME_RESOLUTION;
|
||||
internal_state.cylinder_status.ignition_timing_deg = record1->ignition_angle;
|
||||
|
||||
// EFI3 log
|
||||
internal_state.ignition_voltage = record1->battery_voltage * VOLTAGE_RESOLUTION;
|
||||
|
||||
engine_temperature_sensor_status = (record1->sensor_ok & 0x01) != 0;
|
||||
air_temperature_sensor_status = (record1->sensor_ok & 0x02) != 0;
|
||||
air_pressure_sensor_status = (record1->sensor_ok & 0x04) != 0;
|
||||
throttle_sensor_status = (record1->sensor_ok & 0x08) != 0;
|
||||
|
||||
// resusing mavlink variables as required for Hirth
|
||||
// add in ADC voltage of MAP sensor > convert to MAP in kPa
|
||||
internal_state.intake_manifold_pressure_kpa = record1->voltage_int_air_pressure * (ADC_CALIBRATION * MAP_HPA_PER_VOLT_FACTOR * HPA_TO_KPA);
|
||||
internal_state.intake_manifold_temperature = C_TO_KELVIN(record1->air_temperature);
|
||||
break;
|
||||
}
|
||||
|
||||
case CODE_REQUEST_STATUS_2: {
|
||||
struct Record2 *record2 = (Record2*)raw_data;
|
||||
|
||||
// EFI log
|
||||
const float fuel_consumption_rate_lph = record2->fuel_consumption * 0.1;
|
||||
|
||||
internal_state.fuel_consumption_rate_cm3pm = (fuel_consumption_rate_lph * 1000.0 / 60.0) * get_coef1();
|
||||
|
||||
if (last_fuel_integration_ms != 0) {
|
||||
// estimated_consumed_fuel_volume_cm3 is in cm3/pm
|
||||
const float dt_minutes = (now - last_fuel_integration_ms)*(0.001/60);
|
||||
internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * dt_minutes;
|
||||
}
|
||||
last_fuel_integration_ms = now;
|
||||
|
||||
internal_state.throttle_position_percent = record2->throttle_percent_times_10 * 0.1;
|
||||
break;
|
||||
}
|
||||
|
||||
case CODE_REQUEST_STATUS_3: {
|
||||
struct Record3 *record3 = (Record3*)raw_data;
|
||||
|
||||
// EFI3 Log
|
||||
CHT_1_error_excess_temperature_status = (record3->error_excess_temperature_bitfield & 0x0007) != 0;
|
||||
CHT_2_error_excess_temperature_status = (record3->error_excess_temperature_bitfield & 0x0038) != 0;
|
||||
EGT_1_error_excess_temperature_status = (record3->error_excess_temperature_bitfield & 0x01C0) != 0;
|
||||
EGT_2_error_excess_temperature_status = (record3->error_excess_temperature_bitfield & 0x0E00) != 0;
|
||||
|
||||
// ECYL log
|
||||
internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(record3->excess_temperature_1);
|
||||
internal_state.cylinder_status.cylinder_head_temperature2 = C_TO_KELVIN(record3->excess_temperature_2);
|
||||
internal_state.cylinder_status.exhaust_gas_temperature = C_TO_KELVIN(record3->excess_temperature_3);
|
||||
internal_state.cylinder_status.exhaust_gas_temperature2 = C_TO_KELVIN(record3->excess_temperature_4);
|
||||
break;
|
||||
}
|
||||
|
||||
// case CODE_SET_VALUE:
|
||||
// // Do nothing for now
|
||||
// break;
|
||||
}
|
||||
}
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
void AP_EFI_Serial_Hirth::log_status(void)
|
||||
{
|
||||
// @LoggerMessage: EFIS
|
||||
// @Description: Electronic Fuel Injection data - Hirth specific Status information
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: ETS1: Status of EGT1 excess temperature error
|
||||
// @Field: ETS2: Status of EGT2 excess temperature error
|
||||
// @Field: CTS1: Status of CHT1 excess temperature error
|
||||
// @Field: CTS2: Status of CHT2 excess temperature error
|
||||
// @Field: ETSS: Status of Engine temperature sensor
|
||||
// @Field: ATSS: Status of Air temperature sensor
|
||||
// @Field: APSS: Status of Air pressure sensor
|
||||
// @Field: TSS: Status of Temperature sensor
|
||||
// @Field: CRCF: CRC failure count
|
||||
// @Field: AckF: ACK failure count
|
||||
// @Field: Up: Uptime between 2 messages
|
||||
// @Field: ThrO: Throttle output as received by the engine
|
||||
AP::logger().WriteStreaming("EFIS",
|
||||
"TimeUS,ETS1,ETS2,CTS1,CTS2,ETSS,ATSS,APSS,TSS,CRCF,AckF,Up,ThrO",
|
||||
"s------------",
|
||||
"F------------",
|
||||
"QBBBBBBBBIIIf",
|
||||
AP_HAL::micros64(),
|
||||
uint8_t(EGT_1_error_excess_temperature_status),
|
||||
uint8_t(EGT_2_error_excess_temperature_status),
|
||||
uint8_t(CHT_1_error_excess_temperature_status),
|
||||
uint8_t(CHT_2_error_excess_temperature_status),
|
||||
uint8_t(engine_temperature_sensor_status),
|
||||
uint8_t(air_temperature_sensor_status),
|
||||
uint8_t(air_pressure_sensor_status),
|
||||
uint8_t(throttle_sensor_status),
|
||||
uint32_t(crc_fail_cnt),
|
||||
uint32_t(ack_fail_cnt),
|
||||
uint32_t(uptime),
|
||||
float(internal_state.throttle_out));
|
||||
}
|
||||
#endif // HAL_LOGGING_ENABLED
|
||||
|
||||
#endif // AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_EFI_config.h"
|
||||
|
||||
#if AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
#include "AP_EFI.h"
|
||||
#include "AP_EFI_Backend.h"
|
||||
|
||||
/*!
|
||||
* class definition for Hirth 4103 ECU
|
||||
*/
|
||||
class AP_EFI_Serial_Hirth: public AP_EFI_Backend {
|
||||
public:
|
||||
AP_EFI_Serial_Hirth(AP_EFI &_frontend);
|
||||
|
||||
void update() override;
|
||||
|
||||
private:
|
||||
// serial port instance
|
||||
AP_HAL::UARTDriver *port;
|
||||
|
||||
// periodic refresh
|
||||
uint32_t last_request_ms;
|
||||
uint32_t last_packet_ms;
|
||||
uint32_t last_req_send_throttle_ms;
|
||||
|
||||
// raw bytes - max size
|
||||
uint8_t raw_data[256];
|
||||
|
||||
// request and response data
|
||||
uint8_t requested_code;
|
||||
|
||||
// meta-data for a response
|
||||
struct {
|
||||
uint8_t quantity;
|
||||
uint8_t code;
|
||||
uint8_t checksum;
|
||||
} res_data;
|
||||
|
||||
// TRUE - Request is sent; waiting for response
|
||||
// FALSE - Response is already received
|
||||
bool waiting_response;
|
||||
|
||||
// Expected bytes from response
|
||||
uint8_t expected_bytes;
|
||||
|
||||
// Throttle values
|
||||
uint16_t last_throttle;
|
||||
|
||||
uint32_t last_fuel_integration_ms;
|
||||
|
||||
// custom status for Hirth
|
||||
bool engine_temperature_sensor_status;
|
||||
bool air_temperature_sensor_status;
|
||||
bool air_pressure_sensor_status;
|
||||
bool throttle_sensor_status;
|
||||
|
||||
bool CHT_1_error_excess_temperature_status;
|
||||
bool CHT_2_error_excess_temperature_status;
|
||||
bool EGT_1_error_excess_temperature_status;
|
||||
bool EGT_2_error_excess_temperature_status;
|
||||
uint32_t crc_fail_cnt;
|
||||
uint32_t uptime;
|
||||
uint32_t ack_fail_cnt;
|
||||
|
||||
struct PACKED Record1 {
|
||||
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;
|
||||
};
|
||||
static_assert(sizeof(Record1) == 84, "incorrect Record1 length");
|
||||
|
||||
struct PACKED Record2 {
|
||||
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_graphic_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;
|
||||
int16_t throttle_percent_times_10; // percent * 0.1
|
||||
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];
|
||||
};
|
||||
static_assert(sizeof(Record2) == 98, "incorrect Record2 length");
|
||||
|
||||
struct PACKED Record3 {
|
||||
int16_t voltage_excess_temperature_1;
|
||||
int16_t voltage_excess_temperature_2;
|
||||
int16_t voltage_excess_temperature_3;
|
||||
int16_t voltage_excess_temperature_4;
|
||||
int16_t voltage_excess_temperature_5;
|
||||
uint8_t reserved1[6];
|
||||
int16_t excess_temperature_1; // cht1
|
||||
int16_t excess_temperature_2; // cht2
|
||||
int16_t excess_temperature_3; // egt1
|
||||
int16_t excess_temperature_4; // egt2
|
||||
int16_t excess_temperature_5;
|
||||
uint8_t reserved2[6];
|
||||
int16_t enrichment_excess_temperature_cylinder_1;
|
||||
int16_t enrichment_excess_temperature_cylinder_2;
|
||||
int16_t enrichment_excess_temperature_cylinder_3;
|
||||
int16_t enrichment_excess_temperature_cylinder_4;
|
||||
uint8_t reserved3[6];
|
||||
uint16_t error_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;
|
||||
int16_t number_of_injections_oil_pump_2;
|
||||
uint16_t target_rpm;
|
||||
uint16_t FPC;
|
||||
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;
|
||||
int16_t temperature_crankshaft_housing;
|
||||
uint8_t reserved4[14];
|
||||
};
|
||||
static_assert(sizeof(Record3) == 100, "incorrect Record3 length");
|
||||
|
||||
void check_response();
|
||||
void send_request();
|
||||
void decode_data();
|
||||
bool send_request_status();
|
||||
bool send_target_values(uint16_t);
|
||||
void log_status();
|
||||
};
|
||||
|
||||
#endif // AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
|
|
@ -15,6 +15,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <AP_EFI/AP_EFI_config.h>
|
||||
|
||||
#if HAL_EFI_ENABLED
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
|
|
@ -108,11 +112,17 @@ struct Cylinder_Status {
|
|||
// Cylinder head temperature (CHT) (kelvin)
|
||||
float cylinder_head_temperature;
|
||||
|
||||
// 2nd Cylinder head temperature (CHT) (kelvin), 0 if not applicable
|
||||
float cylinder_head_temperature2;
|
||||
|
||||
// Exhaust gas temperature (EGT) (kelvin)
|
||||
// If this cylinder is not equipped with an EGT sensor - will be NaN
|
||||
// If there is a single shared EGT sensor, will be the same value for all cylinders
|
||||
float exhaust_gas_temperature;
|
||||
|
||||
// 2nd cylinder exhaust gas temperature, 0 if not applicable
|
||||
float exhaust_gas_temperature2;
|
||||
|
||||
// Estimated lambda coefficient (dimensionless ratio)
|
||||
// Useful for monitoring and tuning purposes.
|
||||
float lambda_coefficient;
|
||||
|
|
@ -204,3 +214,5 @@ struct EFI_State {
|
|||
// PT compensation
|
||||
float pt_compensation;
|
||||
};
|
||||
|
||||
#endif // HAL_EFI_ENABLED
|
||||
|
|
|
|||
|
|
@ -0,0 +1,74 @@
|
|||
#include "AP_EFI_config.h"
|
||||
|
||||
#if AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
|
||||
#include "AP_EFI.h"
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
// settings for throttle linearisation
|
||||
const AP_Param::GroupInfo AP_EFI_ThrLin::var_info[] = {
|
||||
|
||||
// @Param: _EN
|
||||
// @DisplayName: Enable throttle linearisation
|
||||
// @Description: Enable EFI throttle linearisation
|
||||
// @Values: 0:Disabled, 1:Enabled
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FLAGS("_EN", 1, AP_EFI_ThrLin, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: _COEF1
|
||||
// @DisplayName: Throttle linearisation - First Order
|
||||
// @Description: First Order Polynomial Coefficient. (=1, if throttle is first order polynomial trendline)
|
||||
// @Range: -1 1
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("_COEF1", 2, AP_EFI_ThrLin, coefficient[0], 1),
|
||||
|
||||
// @Param: _COEF2
|
||||
// @DisplayName: Throttle linearisation - Second Order
|
||||
// @Description: Second Order Polynomial Coefficient (=0, if throttle is second order polynomial trendline)
|
||||
// @Range: -1 1
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("_COEF2", 3, AP_EFI_ThrLin, coefficient[1], 0),
|
||||
|
||||
// @Param: _COEF3
|
||||
// @DisplayName: Throttle linearisation - Third Order
|
||||
// @Description: Third Order Polynomial Coefficient. (=0, if throttle is third order polynomial trendline)
|
||||
// @Range: -1 1
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("_COEF3", 4, AP_EFI_ThrLin, coefficient[2], 0),
|
||||
|
||||
// @Param: _OFS
|
||||
// @DisplayName: throttle linearization offset
|
||||
// @Description: Offset for throttle linearization
|
||||
// @Range: 0 100
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("_OFS", 5, AP_EFI_ThrLin, offset, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AP_EFI_ThrLin::AP_EFI_ThrLin(void)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
/*
|
||||
apply throttle linearisation
|
||||
*/
|
||||
float AP_EFI_ThrLin::linearise_throttle(float throttle_percent)
|
||||
{
|
||||
if (!enable) {
|
||||
return throttle_percent;
|
||||
}
|
||||
float ret = coefficient[0] * throttle_percent;
|
||||
ret += coefficient[1] * throttle_percent * throttle_percent;
|
||||
ret += coefficient[2] * throttle_percent * throttle_percent * throttle_percent;
|
||||
ret += offset;
|
||||
return ret;
|
||||
}
|
||||
|
||||
#endif // AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#include "AP_EFI_config.h"
|
||||
|
||||
#if AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
|
||||
/*
|
||||
throttle linearisation support
|
||||
*/
|
||||
class AP_EFI_ThrLin {
|
||||
public:
|
||||
|
||||
AP_EFI_ThrLin();
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
/*
|
||||
apply throttle linearisation, returning value to pass to the
|
||||
engine
|
||||
*/
|
||||
float linearise_throttle(float throttle_pct);
|
||||
|
||||
private:
|
||||
AP_Int8 enable;
|
||||
AP_Float coefficient[3];
|
||||
AP_Float offset;
|
||||
};
|
||||
|
||||
#endif // AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
|
||||
|
|
@ -2,6 +2,7 @@
|
|||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
#include <AP_CANManager/AP_CANSensor.h>
|
||||
#include <AP_Scripting/AP_Scripting_config.h>
|
||||
|
||||
#ifndef HAL_EFI_ENABLED
|
||||
#define HAL_EFI_ENABLED BOARD_FLASH_SIZE > 1024
|
||||
|
|
@ -38,3 +39,11 @@
|
|||
#ifndef AP_EFI_SERIAL_LUTAN_ENABLED
|
||||
#define AP_EFI_SERIAL_LUTAN_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
#define AP_EFI_SERIAL_HIRTH_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
||||
#define AP_EFI_THROTTLE_LINEARISATION_ENABLED AP_EFI_SERIAL_HIRTH_ENABLED
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue