From 9ddd1afb34ec5a964c72fe2c9880b6b6bff4b212 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Jun 2022 08:09:58 +1000 Subject: [PATCH] AP_EFI: added DroneCAN EFI driver --- libraries/AP_EFI/AP_EFI.cpp | 8 +- libraries/AP_EFI/AP_EFI.h | 1 + libraries/AP_EFI/AP_EFI_Backend.cpp | 6 + libraries/AP_EFI/AP_EFI_Backend.h | 2 + libraries/AP_EFI/AP_EFI_DroneCAN.cpp | 169 +++++++++++++++++++++++++++ libraries/AP_EFI/AP_EFI_DroneCAN.h | 32 +++++ libraries/AP_EFI/AP_EFI_NWPMU.cpp | 4 +- libraries/AP_EFI/AP_EFI_State.h | 8 +- 8 files changed, 223 insertions(+), 7 deletions(-) create mode 100644 libraries/AP_EFI/AP_EFI_DroneCAN.cpp create mode 100644 libraries/AP_EFI/AP_EFI_DroneCAN.h diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index fd2ab2c58b..5c44d5d84b 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -20,6 +20,7 @@ #include "AP_EFI_Serial_MS.h" #include "AP_EFI_Serial_Lutan.h" #include "AP_EFI_NWPMU.h" +#include "AP_EFI_DroneCAN.h" #include #if HAL_MAX_CAN_PROTOCOL_DRIVERS @@ -33,7 +34,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 + // @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan,5:DroneCAN // @User: Advanced // @RebootRequired: True AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE), @@ -83,6 +84,11 @@ void AP_EFI::init(void) case Type::NWPMU: #if HAL_EFI_NWPWU_ENABLED backend = new AP_EFI_NWPMU(*this); +#endif + break; + case Type::DroneCAN: +#if HAL_EFI_DRONECAN_ENABLED + backend = new AP_EFI_DroneCAN(*this); #endif break; default: diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index 36a97a9cbb..8b60b0711a 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -82,6 +82,7 @@ public: NWPMU = 2, Lutan = 3, // LOWEHEISER = 4, + DroneCAN = 5, }; static AP_EFI *get_singleton(void) { diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp index 7348c3f69f..924830e64a 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.cpp +++ b/libraries/AP_EFI/AP_EFI_Backend.cpp @@ -41,4 +41,10 @@ float AP_EFI_Backend::get_coef2(void) const { return frontend.coef2; } + +HAL_Semaphore &AP_EFI_Backend::get_sem(void) +{ + return frontend.sem; +} + #endif // HAL_EFI_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h index 5c70d4908a..0f56205d3b 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -42,6 +42,8 @@ protected: float get_coef1(void) const; float get_coef2(void) const; + HAL_Semaphore &get_sem(void); + private: AP_EFI &frontend; }; diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp new file mode 100644 index 0000000000..9f88650b5b --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp @@ -0,0 +1,169 @@ +#include + +#include "AP_EFI_DroneCAN.h" + +#if HAL_EFI_DRONECAN_ENABLED + +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +AP_EFI_DroneCAN *AP_EFI_DroneCAN::driver; + +// DroneCAN Frontend Registry Binder +UC_REGISTRY_BINDER(EFIStatusCb, uavcan::equipment::ice::reciprocating::Status); + +// constructor +AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) : + AP_EFI_Backend(_frontend) +{ + driver = this; +} + +// links the DroneCAN message to this backend +void AP_EFI_DroneCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) +{ + if (ap_uavcan == nullptr) { + return; + } + auto* node = ap_uavcan->get_node(); + + uavcan::Subscriber *status_listener; + status_listener = new uavcan::Subscriber(*node); + + // Register method to handle incoming status + const int status_listener_res = status_listener->start(EFIStatusCb(ap_uavcan, &trampoline_status)); + if (status_listener_res < 0) { + AP_HAL::panic("DroneCAN EFI subscriber start problem\n\r"); + return; + } +} + +// Called from frontend to update with the readings received by handler +void AP_EFI_DroneCAN::update() +{ +} + +// EFI message handler +void AP_EFI_DroneCAN::trampoline_status(AP_UAVCAN *ap_uavcan, uint8_t node_id, const EFIStatusCb &cb) +{ + if (driver == nullptr) { + return; + } + const uavcan::equipment::ice::reciprocating::Status &pkt = *cb.msg; + driver->handle_status(pkt); +} + +/* + handle reciprocating ICE status message from DroneCAN + */ +void AP_EFI_DroneCAN::handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt) +{ + auto &istate = internal_state; + + // state maps 1:1 from Engine_State + istate.engine_state = Engine_State(pkt.state); + + if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) { + istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::NOT_SUPPORTED; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR) { + istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::ERROR; + } else { + istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::OK; + } + + if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_SUPPORTED)) { + istate.temperature_status = Temperature_Status::NOT_SUPPORTED; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_BELOW_NOMINAL) { + istate.temperature_status = Temperature_Status::BELOW_NOMINAL; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_ABOVE_NOMINAL) { + istate.temperature_status = Temperature_Status::ABOVE_NOMINAL; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_OVERHEATING) { + istate.temperature_status = Temperature_Status::OVERHEATING; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) { + istate.temperature_status = Temperature_Status::EGT_ABOVE_NOMINAL; + } else { + istate.temperature_status = Temperature_Status::OK; + } + + if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_SUPPORTED)) { + istate.fuel_pressure_status = Fuel_Pressure_Status::NOT_SUPPORTED; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_BELOW_NOMINAL) { + istate.fuel_pressure_status = Fuel_Pressure_Status::BELOW_NOMINAL; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_ABOVE_NOMINAL) { + istate.fuel_pressure_status = Fuel_Pressure_Status::ABOVE_NOMINAL; + } else { + istate.fuel_pressure_status = Fuel_Pressure_Status::OK; + } + + if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_SUPPORTED)) { + istate.oil_pressure_status = Oil_Pressure_Status::NOT_SUPPORTED; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_BELOW_NOMINAL) { + istate.oil_pressure_status = Oil_Pressure_Status::BELOW_NOMINAL; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_ABOVE_NOMINAL) { + istate.oil_pressure_status = Oil_Pressure_Status::ABOVE_NOMINAL; + } else { + istate.oil_pressure_status = Oil_Pressure_Status::OK; + } + + if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_SUPPORTED)) { + istate.detonation_status = Detonation_Status::NOT_SUPPORTED; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_OBSERVED) { + istate.detonation_status = Detonation_Status::OBSERVED; + } else { + istate.detonation_status = Detonation_Status::NOT_OBSERVED; + } + + if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_SUPPORTED)) { + istate.misfire_status = Misfire_Status::NOT_SUPPORTED; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_OBSERVED) { + istate.misfire_status = Misfire_Status::OBSERVED; + } else { + istate.misfire_status = Misfire_Status::NOT_OBSERVED; + } + + if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_SUPPORTED)) { + istate.debris_status = Debris_Status::NOT_SUPPORTED; + } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_DETECTED) { + istate.debris_status = Debris_Status::DETECTED; + } else { + istate.debris_status = Debris_Status::NOT_DETECTED; + } + + istate.engine_load_percent = pkt.engine_load_percent; + istate.engine_speed_rpm = pkt.engine_speed_rpm; + istate.spark_dwell_time_ms = pkt.spark_dwell_time_ms; + istate.atmospheric_pressure_kpa = pkt.atmospheric_pressure_kpa; + istate.intake_manifold_pressure_kpa = pkt.intake_manifold_pressure_kpa; + istate.intake_manifold_temperature = pkt.intake_manifold_temperature; + istate.coolant_temperature = pkt.coolant_temperature; + istate.oil_pressure = pkt.oil_pressure; + istate.oil_temperature = pkt.oil_temperature; + istate.fuel_pressure = pkt.fuel_pressure; + istate.fuel_consumption_rate_cm3pm = pkt.fuel_consumption_rate_cm3pm; + istate.estimated_consumed_fuel_volume_cm3 = pkt.estimated_consumed_fuel_volume_cm3; + istate.throttle_position_percent = pkt.throttle_position_percent; + istate.ecu_index = pkt.ecu_index; + + // 1:1 for spark plug usage + istate.spark_plug_usage = Spark_Plug_Usage(pkt.spark_plug_usage); + + // assume max one cylinder status + if (pkt.cylinder_status.size() > 0) { + const auto &cs = pkt.cylinder_status[0]; + auto &c = istate.cylinder_status[0]; + c.ignition_timing_deg = cs.ignition_timing_deg; + c.injection_time_ms = cs.injection_time_ms; + c.cylinder_head_temperature = cs.cylinder_head_temperature; + c.exhaust_gas_temperature = cs.exhaust_gas_temperature; + c.lambda_coefficient = cs.lambda_coefficient; + } + + copy_to_frontend(); +} + +#endif // HAL_EFI_DRONECAN_ENABLED + diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.h b/libraries/AP_EFI/AP_EFI_DroneCAN.h new file mode 100644 index 0000000000..35c02d5e37 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.h @@ -0,0 +1,32 @@ +#pragma once + +#include "AP_EFI.h" +#include "AP_EFI_Backend.h" + +#ifndef HAL_EFI_DRONECAN_ENABLED +#define HAL_EFI_DRONECAN_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && BOARD_FLASH_SIZE > 1024 && HAL_CANMANAGER_ENABLED +#endif + +#if HAL_EFI_DRONECAN_ENABLED +#include +#include + +class EFIStatusCb; + +class AP_EFI_DroneCAN : public AP_EFI_Backend { +public: + AP_EFI_DroneCAN(AP_EFI &_frontend); + + void update() override; + + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static void trampoline_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const EFIStatusCb &cb); + +private: + void handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt); + + // singleton for trampoline + static AP_EFI_DroneCAN *driver; +}; +#endif // HAL_EFI_DRONECAN_ENABLED + diff --git a/libraries/AP_EFI/AP_EFI_NWPMU.cpp b/libraries/AP_EFI/AP_EFI_NWPMU.cpp index be69fea4ea..dfb098579e 100644 --- a/libraries/AP_EFI/AP_EFI_NWPMU.cpp +++ b/libraries/AP_EFI/AP_EFI_NWPMU.cpp @@ -36,7 +36,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) { const uint32_t id = frame.id & AP_HAL::CANFrame::MaskExtID; - WITH_SEMAPHORE(frontend.sem); + WITH_SEMAPHORE(get_sem()); switch ((NWPMU_ID)id) { case NWPMU_ID::ECU_1: { @@ -104,7 +104,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) struct ecu_6 data; memcpy(&data, frame.data, sizeof(data)); if (!_emitted_version && (AP_HAL::millis() > 10000)) { // don't emit a version early in the boot process - gcs().send_text(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d", data.firmware_major, data.firmware_minor, data.firmware_build); diff --git a/libraries/AP_EFI/AP_EFI_State.h b/libraries/AP_EFI/AP_EFI_State.h index ccaf491584..f08daec667 100644 --- a/libraries/AP_EFI/AP_EFI_State.h +++ b/libraries/AP_EFI/AP_EFI_State.h @@ -61,10 +61,10 @@ enum class Fuel_Pressure_Status : uint8_t { }; enum class Oil_Pressure_Status : uint8_t { - OIL_PRESSURE_STATUS_NOT_SUPPORTED = 0, - OIL_PRESSURE_OK = 1, - OIL_PRESSURE_BELOW_NOMINAL = 2, - OIL_PRESSURE_ABOVE_NOMINAL = 3 + NOT_SUPPORTED = 0, + OK = 1, + BELOW_NOMINAL = 2, + ABOVE_NOMINAL = 3 }; enum class Detonation_Status : uint8_t {