diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index b52756d09f..3cc4d648ff 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -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 }; @@ -117,6 +124,11 @@ void AP_EFI::init(void) case Type::SCRIPTING: 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: @@ -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 diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index 36db32e07b..e2cb728158 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -22,6 +22,7 @@ #include #include #include +#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; diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp index 8eff55e857..c110dcf205 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.cpp +++ b/libraries/AP_EFI/AP_EFI_Backend.cpp @@ -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 diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h index dea30f8c44..77a53888f7 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -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: diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp index c242598cf0..d917cb6599 100644 --- a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp @@ -1,8 +1,9 @@ #include -#include "AP_EFI_DroneCAN.h" +#include "AP_EFI_config.h" #if AP_EFI_DRONECAN_ENABLED +#include "AP_EFI_DroneCAN.h" #include #include diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.h b/libraries/AP_EFI/AP_EFI_DroneCAN.h index c4f9dde22c..0512d8ea1f 100644 --- a/libraries/AP_EFI/AP_EFI_DroneCAN.h +++ b/libraries/AP_EFI/AP_EFI_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 class AP_EFI_DroneCAN : public AP_EFI_Backend { diff --git a/libraries/AP_EFI/AP_EFI_Scripting.cpp b/libraries/AP_EFI/AP_EFI_Scripting.cpp index 8a4b2f73a0..710504710c 100644 --- a/libraries/AP_EFI/AP_EFI_Scripting.cpp +++ b/libraries/AP_EFI/AP_EFI_Scripting.cpp @@ -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() { diff --git a/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp b/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp new file mode 100644 index 0000000000..a7d3978a3e --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp @@ -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 . +*/ + + + +#include "AP_EFI_config.h" + +#if AP_EFI_SERIAL_HIRTH_ENABLED +#include +#include +#include +#include +#include +#include +#include + +#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 diff --git a/libraries/AP_EFI/AP_EFI_Serial_Hirth.h b/libraries/AP_EFI/AP_EFI_Serial_Hirth.h new file mode 100644 index 0000000000..350a3477c4 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.h @@ -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 . +*/ + + +#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 diff --git a/libraries/AP_EFI/AP_EFI_State.h b/libraries/AP_EFI/AP_EFI_State.h index c73416f5a3..522f88f87a 100644 --- a/libraries/AP_EFI/AP_EFI_State.h +++ b/libraries/AP_EFI/AP_EFI_State.h @@ -15,6 +15,10 @@ #pragma once +#include + +#if HAL_EFI_ENABLED + #include #include @@ -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 diff --git a/libraries/AP_EFI/AP_EFI_ThrottleLinearisation.cpp b/libraries/AP_EFI/AP_EFI_ThrottleLinearisation.cpp new file mode 100644 index 0000000000..5b63474672 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_ThrottleLinearisation.cpp @@ -0,0 +1,74 @@ +#include "AP_EFI_config.h" + +#if AP_EFI_THROTTLE_LINEARISATION_ENABLED + +#include "AP_EFI.h" +#include + +// 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 + diff --git a/libraries/AP_EFI/AP_EFI_ThrottleLinearisation.h b/libraries/AP_EFI/AP_EFI_ThrottleLinearisation.h new file mode 100644 index 0000000000..82b1e0fd6a --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_ThrottleLinearisation.h @@ -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 + diff --git a/libraries/AP_EFI/AP_EFI_config.h b/libraries/AP_EFI/AP_EFI_config.h index 00653cc38c..a89307c2bc 100644 --- a/libraries/AP_EFI/AP_EFI_config.h +++ b/libraries/AP_EFI/AP_EFI_config.h @@ -2,6 +2,7 @@ #include #include +#include #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