diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index 476b7e1f9e..a66b644a44 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -17,7 +17,6 @@ #if HAL_EFI_ENABLED -#define HAL_EFI_CURRAWONG_ECU_ENABLED 1 #include "AP_EFI_Serial_MS.h" #include "AP_EFI_Serial_Lutan.h" #include "AP_EFI_NWPMU.h" @@ -55,13 +54,13 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @User: Advanced AP_GROUPINFO("_COEF2", 3, AP_EFI, coef2, 0), - // @Param: ECU_DN + // @Param: _FUEL_DENS // @DisplayName: ECU Fuel Density // @Description: Used to calculate fuel consumption // @Units: kg/m/m/m // @Range: 0 10000 // @User: Advanced - AP_GROUPINFO("_ECU_DN", 4, AP_EFI, ecu_dn, 0.), + AP_GROUPINFO("_FUEL_DENS", 4, AP_EFI, ecu_fuel_density, 0), AP_GROUPEND }; diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index ddae57f481..50e4d38b85 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -99,7 +99,7 @@ protected: AP_Float coef1; AP_Float coef2; - AP_Float ecu_dn; + AP_Float ecu_fuel_density; EFI_State state; diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp index 674f9a3730..8eff55e857 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.cpp +++ b/libraries/AP_EFI/AP_EFI_Backend.cpp @@ -47,8 +47,8 @@ HAL_Semaphore &AP_EFI_Backend::get_sem(void) return frontend.sem; } -float AP_EFI_Backend::get_ecu_dn(void) const +float AP_EFI_Backend::get_ecu_fuel_density(void) const { - return frontend.ecu_dn; + 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 d2d5ab0b15..2e6b7e48c0 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -41,7 +41,7 @@ protected: int8_t get_uavcan_node_id(void) const; float get_coef1(void) const; float get_coef2(void) const; - float get_ecu_dn(void) const; + float get_ecu_fuel_density(void) const; HAL_Semaphore &get_sem(void); diff --git a/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp b/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp index 1cf2482b6f..2d9e957f91 100644 --- a/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp +++ b/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp @@ -16,26 +16,26 @@ /* * AP_EFI_Currawong_ECU.cpp * - * Author: Reilly Callaway + * Author: Reilly Callaway / Currawong Engineering Pty Ltd */ -#include -#include +#include "AP_EFI_Currawong_ECU.h" #if HAL_EFI_CURRAWONG_ECU_ENABLED -extern const AP_HAL::HAL& hal; +#include +#include +#include -AP_EFI_Currawong_ECU* AP_EFI_Currawong_ECU::singleton; +AP_EFI_Currawong_ECU* AP_EFI_Currawong_ECU::_singleton; AP_EFI_Currawong_ECU::AP_EFI_Currawong_ECU(AP_EFI &_frontend) : AP_EFI_Backend(_frontend) { - singleton = this; - - internal_state.oil_pressure_status = Oil_Pressure_Status::OIL_PRESSURE_STATUS_NOT_SUPPORTED; - internal_state.debris_status = Debris_Status::NOT_SUPPORTED; - internal_state.misfire_status = Misfire_Status::NOT_SUPPORTED; + _singleton = this; + // Indicate that temperature and fuel pressure are supported + internal_state.fuel_pressure_status = Fuel_Pressure_Status::OK; + internal_state.temperature_status = Temperature_Status::OK; } void AP_EFI_Currawong_ECU::update() @@ -49,24 +49,61 @@ bool AP_EFI_Currawong_ECU::handle_message(AP_HAL::CANFrame &frame) bool valid = true; // There are differences between Ardupilot EFI_State and types/scaling of Piccolo packets. - // So we first decode to Piccolo structs, and then store the data we need in EFI_State internal_state with any scaling required. + // First decode to Piccolo structs, and then store the data we need in internal_state with any scaling required. // Structs to decode Piccolo messages into - ECU_TelemetryFast_t telemetryFast; - ECU_TelemetrySlow0_t telemetrySlow0; - ECU_TelemetrySlow1_t telemetrySlow1; - ECU_TelemetrySlow2_t telemetrySlow2; - ECU_Errors_t errors; + ECU_TelemetryFast_t telemetry_fast; + ECU_TelemetrySlow0_t telemetry_slow0; + ECU_TelemetrySlow1_t telemetry_slow1; + ECU_TelemetrySlow2_t telemetry_slow2; // Throw the message at the decoding functions + if (decodeECU_TelemetryFastPacketStructure(&frame, &telemetry_fast)) { + internal_state.throttle_position_percent = static_cast(telemetry_fast.throttle); + internal_state.engine_load_percent = static_cast(telemetry_fast.throttle); + internal_state.engine_speed_rpm = static_cast(telemetry_fast.rpm); + if (internal_state.engine_speed_rpm > 0) { + internal_state.engine_state = Engine_State::RUNNING; + } else { + internal_state.engine_state = Engine_State::STOPPED; + } - if (valid) - { + // Prevent div by zero + if (get_ecu_fuel_density() > 0.01) { + internal_state.estimated_consumed_fuel_volume_cm3 = static_cast(telemetry_fast.fuelUsed) / KG_PER_M3_TO_G_PER_CM3(get_ecu_fuel_density()); + } else { + // If no (reasonable) density is provided + internal_state.estimated_consumed_fuel_volume_cm3 = 0.; + } + + internal_state.general_error = telemetry_fast.ecuStatusBits.errorIndicator; + if (!telemetry_fast.ecuStatusBits.enabled) { + internal_state.engine_state = Engine_State::STOPPED; + } + } else if (decodeECU_TelemetrySlow0PacketStructure(&frame, &telemetry_slow0)) { + internal_state.intake_manifold_pressure_kpa = telemetry_slow0.map; + internal_state.atmospheric_pressure_kpa = telemetry_slow0.baro; + internal_state.cylinder_status[0].cylinder_head_temperature = C_TO_KELVIN(telemetry_slow0.cht); + } else if (decodeECU_TelemetrySlow1PacketStructure(&frame, &telemetry_slow1)) { + internal_state.intake_manifold_temperature = C_TO_KELVIN(telemetry_slow1.mat); + internal_state.fuel_pressure = telemetry_slow1.fuelPressure; + } else if (decodeECU_TelemetrySlow2PacketStructure(&frame, &telemetry_slow2)) { + internal_state.cylinder_status[0].ignition_timing_deg = telemetry_slow2.ignAngle1; + if (ENGINE_MAX_CYLINDERS > 1) { + internal_state.cylinder_status[1].ignition_timing_deg = telemetry_slow2.ignAngle2; + } + + internal_state.fuel_consumption_rate_cm3pm = telemetry_slow2.flowRate / KG_PER_M3_TO_G_PER_CM3(get_ecu_fuel_density()); + } else { + valid = false; + } + + if (valid) { internal_state.last_updated_ms = AP_HAL::millis(); } return valid; } -#endif // HAL_EFI_CURRAWONG_ECU_ENABLED \ No newline at end of file +#endif // HAL_EFI_CURRAWONG_ECU_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Currawong_ECU.h b/libraries/AP_EFI/AP_EFI_Currawong_ECU.h index 8692c100c8..89387e9dd4 100644 --- a/libraries/AP_EFI/AP_EFI_Currawong_ECU.h +++ b/libraries/AP_EFI/AP_EFI_Currawong_ECU.h @@ -16,7 +16,7 @@ /* * AP_EFI_Currawong_ECU.h * - * Author: Reilly Callaway + * Author: Reilly Callaway / Currawong Engineering Pty Ltd */ #pragma once @@ -25,13 +25,11 @@ #include "AP_EFI_Backend.h" #ifndef HAL_EFI_CURRAWONG_ECU_ENABLED -#define HAL_EFI_CURRAWONG_ECU_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && BOARD_FLASH_SIZE > 1024 +#define HAL_EFI_CURRAWONG_ECU_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && (BOARD_FLASH_SIZE > 1024) #endif #if HAL_EFI_CURRAWONG_ECU_ENABLED - - class AP_EFI_Currawong_ECU : public AP_EFI_Backend { public: AP_EFI_Currawong_ECU(AP_EFI &_frontend); @@ -40,16 +38,16 @@ public: static AP_EFI_Currawong_ECU* get_instance(void) { - return singleton; + return _singleton; } private: bool handle_message(AP_HAL::CANFrame &frame); - static AP_EFI_Currawong_ECU *singleton; + static AP_EFI_Currawong_ECU* _singleton; friend class AP_PiccoloCAN; }; -#endif // HAL_EFI_NWPWU_ENABLED +#endif // HAL_EFI_CURRAWONG_ECU_ENABLED