AP_EFI: added DroneCAN EFI driver

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 <AP_Logger/AP_Logger.h>
 
 #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:

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) {

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

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;
 };

libraries/AP_EFI/AP_EFI_DroneCAN.cpp

@@ -0,0 +1,169 @@
+#include <AP_HAL/AP_HAL.h>
+
+#include "AP_EFI_DroneCAN.h"
+
+#if HAL_EFI_DRONECAN_ENABLED
+
+#include <AP_CANManager/AP_CANManager.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+#include <uavcan/equipment/ice/reciprocating/Status.hpp>
+
+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<uavcan::equipment::ice::reciprocating::Status, EFIStatusCb> *status_listener;
+    status_listener = new uavcan::Subscriber<uavcan::equipment::ice::reciprocating::Status, EFIStatusCb>(*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
+

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 <AP_UAVCAN/AP_UAVCAN.h>
+#include <uavcan/equipment/ice/reciprocating/Status.hpp>
+
+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
+

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);

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,
+    NOT_SUPPORTED        = 0,
-    OIL_PRESSURE_OK                   = 1,
+    OK                   = 1,
-    OIL_PRESSURE_BELOW_NOMINAL        = 2,
+    BELOW_NOMINAL        = 2,
-    OIL_PRESSURE_ABOVE_NOMINAL        = 3
+    ABOVE_NOMINAL        = 3
 };
 
 enum class Detonation_Status : uint8_t {