diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index b52756d09f18ea..3e8f3d717341e9 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), @@ -65,6 +66,70 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Range: 0 10000 // @User: Advanced AP_GROUPINFO("_FUEL_DENS", 4, AP_EFI, ecu_fuel_density, 0), + + // @Param: _THTL_FOP + // @DisplayName: Throttle value - First Order + // @Description: First Order Polynomial with 0.0001 Resolution. (=1, if throttle is first order polynomial trendline) + // @Range: -1 1 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_FOP", 5, AP_EFI, throttle_firstorder, 1), + + // @Param: _THTL_SOP + // @DisplayName: Throttle value - Second Order + // @Description: Second Order Polynomial with 0.0001 Resolution. (=0, if throttle is first order polynomial trendline) + // @Range: -1 1 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_SOP", 6, AP_EFI, throttle_secondorder, 0), + + // @Param: _THTL_TOP + // @DisplayName: Throttle value - First Order + // @Description: Third Order Polynomial with 0.0001 Resolution. (=0, if throttle is first order polynomial trendline) + // @Range: -1 1 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_TOP", 7, AP_EFI, throttle_thirdorder, 0), + + // @Param: _THTL_OFF + // @DisplayName: EFI throttle linearization offset + // @Description: Offset for throttle linearization + // @Range: 0 100 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_OFF", 8, AP_EFI, throttle_offset, 10), + + // @Param: _EFCR_SLP + // @DisplayName: ECU Fuel Consumption Rate factor + // @Description: ECU FCR gradient/factor. Must be used along with _EFCR_OFT + // @Range: 0 1000 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_EFCR_SLP", 9, AP_EFI, ecu_fcr_slope, 1), + + // @Param: _EFCR_OFT + // @DisplayName: ECU Fuel Consumption Rate Offset + // @Description: ECU FCR intercept/offset. Must be used along with _EFCR_SLP + // @Range: 0 1000 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_EFCR_OFT", 10, AP_EFI, ecu_fcr_offset, 0), + + // @Param: _EFCR_AVG + // @DisplayName: ECU Fuel Consumption Rate Average count + // @Description: Averages _EFCR_AVG consecutive reading + // @Range: 0 100 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_EFCR_AVG", 11, AP_EFI, ecu_fcr_average_count, 1), + + // @Param: _FUEL_VOL + // @DisplayName: Full Fuel Volume / Capacity + // @Description: Full fuel volume in ml + // @Range: 0 65535 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_FUEL_VOL", 12, AP_EFI, fuel_volume_in_ml, 1), AP_GROUPEND }; @@ -117,6 +182,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: @@ -151,23 +221,23 @@ bool AP_EFI::is_healthy(void) const */ void AP_EFI::log_status(void) { -// @LoggerMessage: EFI -// @Description: Electronic Fuel Injection system data -// @Field: TimeUS: Time since system startup -// @Field: LP: Reported engine load -// @Field: Rpm: Reported engine RPM -// @Field: SDT: Spark Dwell Time -// @Field: ATM: Atmospheric pressure -// @Field: IMP: Intake manifold pressure -// @Field: IMT: Intake manifold temperature -// @Field: ECT: Engine Coolant Temperature -// @Field: OilP: Oil Pressure -// @Field: OilT: Oil temperature -// @Field: FP: Fuel Pressure -// @Field: FCR: Fuel Consumption Rate -// @Field: CFV: Consumed fueld volume -// @Field: TPS: Throttle Position -// @Field: IDX: Index of the publishing ECU + // @LoggerMessage: EFI + // @Description: Electronic Fuel Injection system data + // @Field: TimeUS: Time since system startup + // @Field: LP: Reported engine load + // @Field: Rpm: Reported engine RPM + // @Field: SDT: Spark Dwell Time + // @Field: ATM: Atmospheric pressure + // @Field: IMP: Intake manifold pressure + // @Field: IMT: Intake manifold temperature + // @Field: ECT: Engine Coolant Temperature + // @Field: OilP: Oil Pressure + // @Field: OilT: Oil temperature + // @Field: FP: Fuel Pressure + // @Field: FCR: Fuel Consumption Rate + // @Field: CFV: Consumed fueld volume + // @Field: TPS: Throttle Position + // @Field: IDX: Index of the publishing ECU AP::logger().WriteStreaming("EFI", "TimeUS,LP,Rpm,SDT,ATM,IMP,IMT,ECT,OilP,OilT,FP,FCR,CFV,TPS,IDX", "s%qsPPOOPOP--%-", @@ -189,50 +259,50 @@ void AP_EFI::log_status(void) uint8_t(state.throttle_position_percent), uint8_t(state.ecu_index)); -// @LoggerMessage: EFI2 -// @Description: Electronic Fuel Injection system data - redux -// @Field: TimeUS: Time since system startup -// @Field: Healthy: True if EFI is healthy -// @Field: ES: Engine state -// @Field: GE: General error -// @Field: CSE: Crankshaft sensor status -// @Field: TS: Temperature status -// @Field: FPS: Fuel pressure status -// @Field: OPS: Oil pressure status -// @Field: DS: Detonation status -// @Field: MS: Misfire status -// @Field: DebS: Debris status -// @Field: SPU: Spark plug usage -// @Field: IDX: Index of the publishing ECU + // @LoggerMessage: EFI2 + // @Description: Electronic Fuel Injection system data - redux + // @Field: TimeUS: Time since system startup + // @Field: Healthy: True if EFI is healthy + // @Field: ES: Engine state + // @Field: GE: General error + // @Field: CSE: Crankshaft sensor status + // @Field: TS: Temperature status + // @Field: FPS: Fuel pressure status + // @Field: OPS: Oil pressure status + // @Field: DS: Detonation status + // @Field: MS: Misfire status + // @Field: DebS: Debris status + // @Field: SPU: Spark plug usage + // @Field: IDX: Index of the publishing ECU AP::logger().WriteStreaming("EFI2", - "TimeUS,Healthy,ES,GE,CSE,TS,FPS,OPS,DS,MS,DebS,SPU,IDX", - "s------------", - "F------------", - "QBBBBBBBBBBBB", - AP_HAL::micros64(), - uint8_t(is_healthy()), - uint8_t(state.engine_state), - uint8_t(state.general_error), - uint8_t(state.crankshaft_sensor_status), - uint8_t(state.temperature_status), - uint8_t(state.fuel_pressure_status), - uint8_t(state.oil_pressure_status), - uint8_t(state.detonation_status), - uint8_t(state.misfire_status), - uint8_t(state.debris_status), - uint8_t(state.spark_plug_usage), - uint8_t(state.ecu_index)); + "TimeUS,Healthy,ES,GE,CSE,TS,FPS,OPS,DS,MS,DebS,SPU,IDX", + "s------------", + "F------------", + "QBBBBBBBBBBBB", + AP_HAL::micros64(), + uint8_t(is_healthy()), + uint8_t(state.engine_state), + uint8_t(state.general_error), + uint8_t(state.crankshaft_sensor_status), + uint8_t(state.temperature_status), + uint8_t(state.fuel_pressure_status), + uint8_t(state.oil_pressure_status), + uint8_t(state.detonation_status), + uint8_t(state.misfire_status), + uint8_t(state.debris_status), + uint8_t(state.spark_plug_usage), + uint8_t(state.ecu_index)); -// @LoggerMessage: ECYL -// @Description: EFI per-cylinder information -// @Field: TimeUS: Time since system startup -// @Field: Inst: Cylinder this data belongs to -// @Field: IgnT: Ignition timing -// @Field: InjT: Injection time -// @Field: CHT: Cylinder head temperature -// @Field: EGT: Exhaust gas temperature -// @Field: Lambda: Estimated lambda coefficient (dimensionless ratio) -// @Field: IDX: Index of the publishing ECU + // @LoggerMessage: ECYL + // @Description: EFI per-cylinder information + // @Field: TimeUS: Time since system startup + // @Field: Inst: Cylinder this data belongs to + // @Field: IgnT: Ignition timing + // @Field: InjT: Injection time + // @Field: CHT: Cylinder head temperature + // @Field: EGT: Exhaust gas temperature + // @Field: Lambda: Estimated lambda coefficient (dimensionless ratio) + // @Field: IDX: Index of the publishing ECU AP::logger().WriteStreaming("ECYL", "TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,IDX", "s#dsOO--", @@ -246,6 +316,22 @@ void AP_EFI::log_status(void) state.cylinder_status.exhaust_gas_temperature, state.cylinder_status.lambda_coefficient, state.ecu_index); + +#if AP_EFI_SERIAL_HIRTH_ENABLED + AP::logger().WriteStreaming("ECYL", + "TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,IDX", + "s#dsOO--", + "F-0C0000", + "QBfffffB", + AP_HAL::micros64(), + 1, + state.cylinder_status.ignition_timing_deg, + state.cylinder_status.injection_time_ms, + state.cylinder_status2.cylinder_head_temperature, + state.cylinder_status2.exhaust_gas_temperature, + state.ecu_index); +#endif // AP_EFI_SERIAL_HIRTH_ENABLED + state.cylinder_status.lambda_coefficient, } #endif // LOGGING_ENABLED @@ -289,16 +375,15 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan) state.spark_dwell_time_ms, state.atmospheric_pressure_kpa, state.intake_manifold_pressure_kpa, - KELVIN_TO_C(state.intake_manifold_temperature), - KELVIN_TO_C(state.cylinder_status.cylinder_head_temperature), + state.intake_manifold_temperature, + state.cylinder_status.cylinder_head_temperature, state.cylinder_status.ignition_timing_deg, state.cylinder_status.injection_time_ms, - KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature), + state.cylinder_status.exhaust_gas_temperature, state.throttle_out, - state.pt_compensation, + float(state.engine_state), //pt_compensation ignition_voltage, - fuel_pressure - ); + fuel_pressure); } // get a copy of state structure diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index 36db32e07b5e83..ae946a89f9e77e 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -23,6 +23,10 @@ #include #include +#ifndef HAL_EFI_ENABLED +#define HAL_EFI_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024 +#endif + #include "AP_EFI_Backend.h" #include "AP_EFI_State.h" @@ -96,7 +100,9 @@ class AP_EFI { #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, }; @@ -113,6 +119,58 @@ class AP_EFI { void handle_EFI_message(const mavlink_message_t &msg); +#if !AP_EFI_SERIAL_HIRTH_ENABLED + //get engine sensor status hirth values for lua + bool get_sensor_status(uint8_t &engine_temperature_sensor_status, uint8_t &air_temperature_sensor_status,uint8_t &air_pressure_sensor_status,uint8_t &throttle_sensor_status,uint8_t &CHT_1_error_excess_temperature_status,uint8_t &CHT_2_error_excess_temperature_status,uint8_t &EGT_1_error_excess_temperature_status,uint8_t &EGT_2_error_excess_temperature_status) const { + engine_temperature_sensor_status = state.engine_temperature_sensor_status; + air_temperature_sensor_status = state.air_temperature_sensor_status; + air_pressure_sensor_status = state.air_pressure_sensor_status; + throttle_sensor_status = state.throttle_sensor_status; + CHT_1_error_excess_temperature_status = state.CHT_1_error_excess_temperature_status; + CHT_2_error_excess_temperature_status = state.CHT_2_error_excess_temperature_status; + EGT_1_error_excess_temperature_status = state.EGT_1_error_excess_temperature_status; + EGT_2_error_excess_temperature_status = state.EGT_2_error_excess_temperature_status; + return true; + } + + //get engine temperature value hirth values for lua + bool get_temp(float &cht1_temp, float &cht2_temp, float &egt1_temp, float &egt2_temp, float &air_temp, float &eng_temp) const { + cht1_temp = state.cht1_temp; + cht2_temp = state.cht2_temp; + egt1_temp = state.egt1_temp; + egt2_temp = state.egt2_temp; + air_temp = state.air_temp; + eng_temp = state.eng_temp; + return true; + } + //get engine load perct values for lua + uint8_t get_eng_load_prct() const { + return uint8_t(state.engine_load_percent); + } + //get engine state specific to Hirth + uint8_t get_state() const { + return uint8_t(state.engine_state); + } + //get crc fail packet number + uint32_t get_crc_fail_ct() const { + return uint8_t(state.crc_fail_cnt); + } + + // get pressure from hirth + bool get_pressure(float &atmospheric_pressure_kpa, float &intake_manifold_pressure_kpa) const { + atmospheric_pressure_kpa = state.atmospheric_pressure_kpa; + intake_manifold_pressure_kpa = state.intake_manifold_pressure_kpa; + return true; + } + + //get Thr + bool get_thr_pos(float &k_throttle, float &thr_pos) const { + k_throttle = state.k_throttle; + thr_pos = state.thr_pos; + return true; + } +#endif // AP_EFI_SERIAL_HIRTH_ENABLED + protected: // Back end Parameters @@ -121,6 +179,20 @@ class AP_EFI { AP_Float ecu_fuel_density; +#if AP_EFI_SERIAL_HIRTH_ENABLED + // polynomial config variables for throttle linearization + AP_Float throttle_firstorder; + AP_Float throttle_secondorder; + AP_Float throttle_thirdorder; + AP_Float throttle_offset; + + // ECU fuel consumption calculation + AP_Float ecu_fcr_slope; + AP_Float ecu_fcr_offset; + AP_Int16 ecu_fcr_average_count; + AP_Int16 fuel_volume_in_ml; +#endif //AP_EFI_SERIAL_HIRTH_ENABLED + EFI_State state; private: diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp index 8eff55e857aa05..1594ebcb99c1dc 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.cpp +++ b/libraries/AP_EFI/AP_EFI_Backend.cpp @@ -42,6 +42,49 @@ float AP_EFI_Backend::get_coef2(void) const return frontend.coef2; } +float AP_EFI_Backend::get_throttle_firstorder(void) const +{ + return frontend.throttle_firstorder; +} + +float AP_EFI_Backend::get_throttle_secondorder(void) const +{ + return frontend.throttle_secondorder; +} + +float AP_EFI_Backend::get_throttle_thirdorder(void) const +{ + return frontend.throttle_thirdorder; +} + +float AP_EFI_Backend::get_throttle_offset(void) const +{ + return frontend.throttle_offset; +} + +float AP_EFI_Backend::get_ecu_fcr_slope(void) const +{ + return frontend.ecu_fcr_slope; +} + +float AP_EFI_Backend::get_ecu_fcr_offset(void) const +{ + return frontend.ecu_fcr_offset; +} + +int16_t AP_EFI_Backend::get_ecu_fcr_average_count(void) const +{ + if (frontend.ecu_fcr_average_count >= 255){ + return 255; + } + return frontend.ecu_fcr_average_count; +} + +int16_t AP_EFI_Backend::get_fuel_volume_in_ml(void) const +{ + return frontend.fuel_volume_in_ml; +} + HAL_Semaphore &AP_EFI_Backend::get_sem(void) { return frontend.sem; @@ -51,4 +94,5 @@ float AP_EFI_Backend::get_ecu_fuel_density(void) const { return frontend.ecu_fuel_density; } + #endif // HAL_EFI_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h index dea30f8c44fce5..502dd099c87230 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -49,6 +49,15 @@ class AP_EFI_Backend { int8_t get_dronecan_node_id(void) const; float get_coef1(void) const; float get_coef2(void) const; + float get_throttle_firstorder(void) const; + float get_throttle_secondorder(void) const; + float get_throttle_thirdorder(void) const; + float get_throttle_offset(void) const; + float get_ecu_fcr_slope(void) const; + float get_ecu_fcr_offset(void) const; + int16_t get_ecu_fcr_average_count(void) const; + int16_t get_fuel_volume_in_ml(void) const; + float get_ecu_fuel_density(void) const; HAL_Semaphore &get_sem(void); 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 00000000000000..a0c78d2506c98b --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp @@ -0,0 +1,483 @@ +/* + 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 + +#include + +#if AP_EFI_SERIAL_HIRTH_ENABLED +#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 FUEL_CONSUMPTION_RESOLUTION 10.0 +#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 +#define LPH_TO_CCMPM 16.67 + +extern const AP_HAL::HAL& hal; + +// custom parameters CX +uint8_t engine_temperature_sensor_status; +uint8_t air_temperature_sensor_status; +uint8_t air_pressure_sensor_status; +uint8_t throttle_sensor_status; +uint8_t CHT_1_error_excess_temperature_status; +uint8_t CHT_2_error_excess_temperature_status; +uint8_t EGT_1_error_excess_temperature_status; +uint8_t EGT_2_error_excess_temperature_status; +float k_throttle; +float battery_voltage; +uint32_t crc_fail_cnt; +uint32_t uptime; +uint32_t ack_fail_cnt; +float fuel_consumption_rate_average; +float fuel_consumption_rate_raw; +float total_fuel_consumed; + +/** + * @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); + get_throttle_polynomial(); + fuel_avg_config = get_ecu_fcr_average_count(); + fuel_avg_count = 0; +} + +/** + * @brief initializes the polynomials + * + */ +void AP_EFI_Serial_Hirth::get_throttle_polynomial() { + throttle_fop = get_throttle_firstorder(); + throttle_sop = get_throttle_secondorder(); + throttle_top = get_throttle_thirdorder(); + throttle_offset = get_throttle_offset(); +} + +/** + * @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(); + +#if HAL_LOGGING_ENABLED + log_status(); +#endif // HAL_LOGGING_ENABLED +} + +/** + * @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) { + + // reset request if no response for SERIAL_WAIT_TIMEOUT_MS + if (now - last_response_ms > SERIAL_WAIT_TIMEOUT_MS) { + waiting_response = false; + last_response_ms = now; + + port->discard_input(); + + ack_fail_cnt++; + } else { + + 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 == req_data.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(); + computed_checksum = computed_checksum % BYTE_RANGE_MAX; + if (res_data.checksum != (BYTE_RANGE_MAX - computed_checksum)) { + crc_fail_cnt++; + port->discard_input(); + } else { + uptime = now - last_uptime; + last_uptime = now; + internal_state.last_updated_ms = now; + decode_data(); + copy_to_frontend(); + } + + waiting_response = false; + } + } + } +} + +/** + * @brief Send Throttle and Telemetry requests to Hirth + * + */ +void AP_EFI_Serial_Hirth::send_request() { + bool status = false; + const uint32_t now = AP_HAL::millis(); + + // sending cmd + if (!waiting_response) { + + // 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 != old_throttle) || (now - last_req_send_throttle_ms > 500)) { + // if new throttle value, send throttle request + // also send new throttle value, only when ARMED + bool allow_throttle = hal.util->get_soft_armed(); + if (!allow_throttle) { + const auto *ice = AP::ice(); + if (ice != nullptr) { + allow_throttle = ice->allow_throttle_while_disarmed(); + } + } + if (allow_throttle) { + status = send_target_values(new_throttle); + } else { + status = send_target_values(0); + } + + old_throttle = new_throttle; + k_throttle = new_throttle; + last_req_send_throttle_ms = now; + } else { + // request Status request, if no throttle commands + status = send_request_status(); + } + + if (status == true) { + waiting_response = true; + last_response_ms = now; + } + } +} + + +/** + * @brief updates the current quantity that will be expected + * + */ +void AP_EFI_Serial_Hirth::get_quantity() { + switch (req_data.code) { + case CODE_REQUEST_STATUS_1: + expected_bytes = QUANTITY_RESPONSE_STATUS_1; + break; + case CODE_REQUEST_STATUS_2: + expected_bytes = QUANTITY_RESPONSE_STATUS_2; + break; + case CODE_REQUEST_STATUS_3: + expected_bytes = QUANTITY_RESPONSE_STATUS_3; + break; + case CODE_SET_VALUE: + expected_bytes = QUANTITY_ACK_SET_VALUES; + break; + } +} + + +/** + * @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)); + + // throttle value is computed as a polynomial defined by the config parameters + // to disable linearization, set throttle_top=0, throttle_sop=0, throttle_fop=1, throttle_offset=0 + uint16_t throttle = ((throttle_top * thr * thr * thr) + + (throttle_sop * thr * thr) + + (throttle_fop * thr) + + throttle_offset) * THROTTLE_POSITION_FACTOR; + + uint8_t idx = 0; + + // set Quantity + Code + "20 bytes of records to set" + Checksum + computed_checksum += raw_data[idx++] = req_data.quantity = QUANTITY_SET_VALUE; + computed_checksum += raw_data[idx++] = req_data.code = CODE_SET_VALUE; + computed_checksum += raw_data[idx++] = throttle & 0xFF; + computed_checksum += raw_data[idx++] = (throttle >> 0x08) & 0xFF; + + for (; idx < QUANTITY_SET_VALUE - 2; idx++) { + // 0 has no impact on checksum + raw_data[idx] = 0; + } + // checksum calculation + computed_checksum = computed_checksum % BYTE_RANGE_MAX; + raw_data[QUANTITY_SET_VALUE - 1] = (BYTE_RANGE_MAX - 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() { + + bool status = false; + + switch (req_data.code) + { + case CODE_REQUEST_STATUS_1: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_2; + req_data.checksum = CHECKSUM_REQUEST_STATUS_2; + expected_bytes = QUANTITY_RESPONSE_STATUS_2; + break; + case CODE_REQUEST_STATUS_2: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_3; + req_data.checksum = CHECKSUM_REQUEST_STATUS_3; + expected_bytes = QUANTITY_RESPONSE_STATUS_3; + break; + case CODE_REQUEST_STATUS_3: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_1; + req_data.checksum = CHECKSUM_REQUEST_STATUS_1; + expected_bytes = QUANTITY_RESPONSE_STATUS_1; + break; + default: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_1; + req_data.checksum = CHECKSUM_REQUEST_STATUS_1; + expected_bytes = QUANTITY_RESPONSE_STATUS_1; + break; + } + raw_data[0] = req_data.quantity; + raw_data[1] = req_data.code; + raw_data[2] = req_data.checksum; + + port->write(raw_data, QUANTITY_REQUEST_STATUS); + + status = true; + + return status; +} + + +/** + * @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(); + + int engine_status = 0; + + int excess_temp_error = 0; + + switch (res_data.code) { + case CODE_REQUEST_STATUS_1: + // EFI log + internal_state.engine_speed_rpm = (raw_data[10] | raw_data[11] << 0x08); + internal_state.throttle_out = (raw_data[72] | raw_data[73] << 0x08); + + // EFI2 log + engine_status = (raw_data[8] | raw_data[9] << 0x08); + internal_state.engine_state = (Engine_State)engine_status; + internal_state.crankshaft_sensor_status = ((raw_data[82] & CRANK_SHAFT_SENSOR_OK) == CRANK_SHAFT_SENSOR_OK) ? Crankshaft_Sensor_Status::OK : Crankshaft_Sensor_Status::ERROR; + + // ECYL log + internal_state.cylinder_status.injection_time_ms = ((raw_data[32] | raw_data[33] << 0x08)) * INJECTION_TIME_RESOLUTION; + internal_state.cylinder_status.ignition_timing_deg = (raw_data[34] | raw_data[35] << 0x08); + + // EFI3 log + battery_voltage = (raw_data[76] | raw_data[77] << 0x08) * VOLTAGE_RESOLUTION; + + engine_temperature_sensor_status = raw_data[82] & 0x01; + air_temperature_sensor_status = raw_data[82] & 0x02; + air_pressure_sensor_status = raw_data[82] & 0x04; + throttle_sensor_status = raw_data[82] & 0x08; + + // 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 = (raw_data[50] | raw_data[51] << 0x08) * ADC_CALIBRATION * MAP_HPA_PER_VOLT_FACTOR * HPA_TO_KPA; + internal_state.intake_manifold_temperature = (raw_data[78] | raw_data[79] << 0x08); + break; + + case CODE_REQUEST_STATUS_2: + // EFI log + // fuel_consumption_rate_raw value is in l/h from hirth + fuel_consumption_rate_raw = (raw_data[52] | raw_data[53] << 0x08) / FUEL_CONSUMPTION_RESOLUTION; + fuel_consumption_rate_raw = get_avg_fuel_consumption_rate(fuel_consumption_rate_raw); + // fuel_consumption_rate_cm3pm is in cm3/pm + internal_state.fuel_consumption_rate_cm3pm = ((fuel_consumption_rate_raw * get_ecu_fcr_slope())) * LPH_TO_CCMPM; + + if (last_fuel_integration_ms != 0) { + // estimated_consumed_fuel_volume_cm3 is in cm3/pm + internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (now - last_fuel_integration_ms)/60000.0f; + } + last_fuel_integration_ms = now; + + // total_fuel_consumed value is logged in litres + total_fuel_consumed = total_fuel_consumed + (fuel_consumption_rate_raw * get_ecu_fcr_slope()); + + internal_state.throttle_position_percent = (raw_data[62] | raw_data[63] << 0x08) / 10; + + break; + + case CODE_REQUEST_STATUS_3: // TBD - internal state addition + excess_temp_error = (raw_data[46] | raw_data[47] << 0x08); + + // EFI3 Log + CHT_1_error_excess_temperature_status = (excess_temp_error & 0x01) || (excess_temp_error & 0x02) || (excess_temp_error & 0x04); + CHT_2_error_excess_temperature_status = (excess_temp_error & 0x08) || (excess_temp_error & 0x10) || (excess_temp_error & 0x20); + EGT_1_error_excess_temperature_status = (excess_temp_error & 0x40) || (excess_temp_error & 0x80) || (excess_temp_error & 0x100); + EGT_2_error_excess_temperature_status = (excess_temp_error & 0x200) || (excess_temp_error & 0x400) || (excess_temp_error & 0x800); + + // ECYL log + internal_state.cylinder_status.cylinder_head_temperature = (raw_data[16] | raw_data[17] << 0x08); + internal_state.cylinder_status2.cylinder_head_temperature = (raw_data[18] | raw_data[19] << 0x08); + internal_state.cylinder_status.exhaust_gas_temperature = (raw_data[20] | raw_data[21] << 0x08); + internal_state.cylinder_status2.exhaust_gas_temperature = (raw_data[22] | raw_data[23] << 0x08); + break; + + // case CODE_SET_VALUE: + // // Do nothing for now + // break; + } +} + +float AP_EFI_Serial_Hirth::get_avg_fuel_consumption_rate(float fuel_consumed) { + fuel_avg_count = (fuel_avg_count) % fuel_avg_config; + float avg_fuel_consumed = 0; + + instance_fuel_reading[fuel_avg_count++] = fuel_consumed; + for (int i = 0; i < fuel_avg_config; i++) { + avg_fuel_consumed += instance_fuel_reading[i]; + } + + return (avg_fuel_consumed / fuel_avg_config); +} + +void AP_EFI_Serial_Hirth::log_status(void) { + // @LoggerMessage: EFIH + // @Description: Electronic Fuel Injection data - Hirth specific information + // @Field: TimeUS: Time since system startup + // @Field: Up: Uptime between 2 messages + // @Field: ThrO: Throttle output as received by the engine + // @Field: BVol: Battery Voltage + // @Field: Fuel: Total fuel consumed + // @Field: C1_T: Cylinder head 1 temperature + // @Field: C2_T: Cylinder head 2 temperature + // @Field: E1_T: Exhaust gas 1 temperature + // @Field: E2_T: Exhaust gas 2 temperature + // @Field: IDX: Index of the publishing ECU + AP::logger().WriteStreaming("EFIH", + "TimeUS,Up,ThrO,BVol,Fuel,C1_T,C2_T,E1_T,E2_T,IDX", + "ss-vlOOOO-", + "FF--------", + "QIfffffffB", + AP_HAL::micros64(), + uint32_t(uptime), + float(internal_state.throttle_out), + float(battery_voltage), + float(total_fuel_consumed), + float(internal_state.cylinder_status.cylinder_head_temperature), + float(internal_state.cylinder_status2.cylinder_head_temperature), + float(internal_state.cylinder_status.exhaust_gas_temperature), + float(internal_state.cylinder_status2.exhaust_gas_temperature), + uint8_t(internal_state.ecu_index)); + + // @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: IDX: Index of the publishing ECU + AP::logger().WriteStreaming("EFIS", + "TimeUS,ETS1,ETS2,CTS1,CTS2,ETSS,ATSS,APSS,TSS,CRCF,AckF,IDX", + "s-----------", + "F-----------", + "QBBBBBBBBIIB", + 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), + uint8_t(internal_state.ecu_index)); +} + +#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 00000000000000..ce13199eba48d4 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.h @@ -0,0 +1,128 @@ +/* + 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.h" + +#if AP_EFI_SERIAL_HIRTH_ENABLED + +#include "AP_EFI_Serial_Hirth.h" + +#define BYTE_RANGE_MAX 256 + +const uint8_t QUANTITY_REQUEST_STATUS = 0x03; +const uint8_t QUANTITY_SET_VALUE = 0x17; + +const uint8_t CODE_REQUEST_STATUS_1 = 0x04; +const uint8_t CODE_REQUEST_STATUS_2 = 0x0B; +const uint8_t CODE_REQUEST_STATUS_3 = 0x0D; +const uint8_t CODE_SET_VALUE = 0xC9; + +const uint8_t CHECKSUM_REQUEST_STATUS_1 = 0xF9; +const uint8_t CHECKSUM_REQUEST_STATUS_2 = 0xF2; +const uint8_t CHECKSUM_REQUEST_STATUS_3 = 0xF0; +const uint8_t CHECKSUM_SET_VALUE = 0x34; + +const uint8_t QUANTITY_RESPONSE_STATUS_1 = 0x57; +const uint8_t QUANTITY_RESPONSE_STATUS_2 = 0x65; +const uint8_t QUANTITY_RESPONSE_STATUS_3 = 0x67; +const uint8_t QUANTITY_ACK_SET_VALUES = 0x03; + + + +/*! + * Data mapping between rawBytes and Telemetry packets + */ +typedef struct { + uint8_t quantity; + uint8_t code; + uint8_t checksum; +} data_set_t; + + +/*! + * 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; + + + void check_response(); + + void send_request(); + + void decode_data(); + + bool send_request_status(); + + bool send_target_values(uint16_t); + + void get_quantity(); + + float get_avg_fuel_consumption_rate(float fuel_consumed); + + void get_throttle_polynomial(); + + void log_status(); + +private: + // serial port instance + AP_HAL::UARTDriver *port; + + // periodic refresh + uint32_t last_response_ms; + uint32_t last_loop_ms; + uint32_t last_uptime; + uint32_t last_req_send_throttle_ms; + + // raw bytes - max size + uint8_t raw_data[BYTE_RANGE_MAX]; + + // request and response data + data_set_t req_data; + data_set_t 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 old_throttle; + + float fuel_consumption_rate_average; + float fuel_consumption_rate_raw; + float fuel_consumption_rate; + float total_fuel_consumed; + uint32_t last_fuel_integration_ms; + float instance_fuel_reading[BYTE_RANGE_MAX]; + uint8_t fuel_avg_count; + uint8_t fuel_avg_config; + + // These polynomial variables are used to linearize the throttle response curve + float throttle_fop; // First order Polynomial + float throttle_sop; // Second order Polynomial + float throttle_top; // Third order Polynomial + float throttle_offset; // Offset to modify the throttle range +}; + +#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 97acbecd6b2209..c5d40353cab2f3 100644 --- a/libraries/AP_EFI/AP_EFI_State.h +++ b/libraries/AP_EFI/AP_EFI_State.h @@ -15,11 +15,18 @@ #pragma once +#include +#include +#include + #define EFI_MAX_INSTANCES 2 #define EFI_MAX_BACKENDS 2 -#include -#include +#if AP_EFI_SERIAL_HIRTH_ENABLED +#define ENGINE_MAX_CYLINDERS 2 +#else +#define ENGINE_MAX_CYLINDERS 1 +#endif // AP_EFI_SERIAL_HIRTH_ENABLED // Time in milliseconds before we declare the EFI to be "unhealthy" #define HEALTHY_LAST_RECEIVED_MS 3000 @@ -31,10 +38,16 @@ ***************/ enum class Engine_State : uint8_t { - STOPPED = 0, + STOPPED = 0, STARTING = 1, - RUNNING = 2, - FAULT = 3 +#if AP_EFI_SERIAL_HIRTH_ENABLED + FAULT = 2, + STARTING_WARM_UP = 3, + RUNNING = 4 +#else + RUNNING = 2, + FAULT = 3 +#endif }; enum class Crankshaft_Sensor_Status : uint8_t { @@ -197,6 +210,10 @@ struct EFI_State { // Status for each cylinder in the engine Cylinder_Status cylinder_status; + +#if AP_EFI_SERIAL_HIRTH_ENABLED + Cylinder_Status cylinder_status2; +#endif // AP_EFI_SERIAL_HIRTH_ENABLED // ignition voltage in Volts float ignition_voltage = -1; // -1 is "unknown"; diff --git a/libraries/AP_EFI/AP_EFI_config.h b/libraries/AP_EFI/AP_EFI_config.h index 00653cc38c54a6..2d293bb3a5150b 100644 --- a/libraries/AP_EFI/AP_EFI_config.h +++ b/libraries/AP_EFI/AP_EFI_config.h @@ -38,3 +38,8 @@ #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 +#define AP_ICENGINE_ENABLED 1 +#endif