Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : 
       3             : #include "AP_EFI_config.h"
       4             : 
       5             : #if AP_EFI_DRONECAN_ENABLED
       6             : #include "AP_EFI_DroneCAN.h"
       7             : 
       8             : #include <AP_CANManager/AP_CANManager.h>
       9             : #include <AP_DroneCAN/AP_DroneCAN.h>
      10             : #include <AP_BoardConfig/AP_BoardConfig.h>
      11             : 
      12             : extern const AP_HAL::HAL& hal;
      13             : 
      14             : AP_EFI_DroneCAN *AP_EFI_DroneCAN::driver;
      15             : 
      16             : // constructor
      17           0 : AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) :
      18           0 :     AP_EFI_Backend(_frontend)
      19             : {
      20           0 :     driver = this;
      21           0 : }
      22             : 
      23             : // links the DroneCAN message to this backend
      24           5 : bool AP_EFI_DroneCAN::subscribe_msgs(AP_DroneCAN *ap_dronecan)
      25             : {
      26           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      27             : 
      28           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &trampoline_status, driver_index) != nullptr);
      29             : }
      30             : 
      31             : // Called from frontend to update with the readings received by handler
      32           0 : void AP_EFI_DroneCAN::update()
      33             : {
      34           0 : }
      35             : 
      36             : // EFI message handler
      37           0 : void AP_EFI_DroneCAN::trampoline_status(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg)
      38             : {
      39           0 :     if (driver == nullptr) {
      40           0 :         return;
      41             :     }
      42           0 :     driver->handle_status(msg);
      43             : }
      44             : 
      45             : /*
      46             :   handle reciprocating ICE status message from DroneCAN
      47             :  */
      48           0 : void AP_EFI_DroneCAN::handle_status(const uavcan_equipment_ice_reciprocating_Status &pkt)
      49             : {
      50           0 :     auto &istate = internal_state;
      51             : 
      52             :     // state maps 1:1 from Engine_State
      53           0 :     istate.engine_state = Engine_State(pkt.state);
      54             : 
      55           0 :     if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) {
      56           0 :         istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::NOT_SUPPORTED;
      57           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR) {
      58           0 :         istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::ERROR;
      59             :     } else {
      60           0 :         istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::OK;
      61             :     }
      62             : 
      63           0 :     if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED)) {
      64           0 :         istate.temperature_status = Temperature_Status::NOT_SUPPORTED;
      65           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL) {
      66           0 :         istate.temperature_status = Temperature_Status::BELOW_NOMINAL;
      67           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL) {
      68           0 :         istate.temperature_status = Temperature_Status::ABOVE_NOMINAL;
      69           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING) {
      70           0 :         istate.temperature_status = Temperature_Status::OVERHEATING;
      71           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) {
      72           0 :         istate.temperature_status = Temperature_Status::EGT_ABOVE_NOMINAL;
      73             :     } else {
      74           0 :         istate.temperature_status = Temperature_Status::OK;
      75             :     }
      76             : 
      77           0 :     if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED)) {
      78           0 :         istate.fuel_pressure_status = Fuel_Pressure_Status::NOT_SUPPORTED;
      79           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL) {
      80           0 :         istate.fuel_pressure_status = Fuel_Pressure_Status::BELOW_NOMINAL;
      81           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL) {
      82           0 :         istate.fuel_pressure_status = Fuel_Pressure_Status::ABOVE_NOMINAL;
      83             :     } else {
      84           0 :         istate.fuel_pressure_status = Fuel_Pressure_Status::OK;
      85             :     }
      86             : 
      87           0 :     if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED)) {
      88           0 :         istate.oil_pressure_status = Oil_Pressure_Status::NOT_SUPPORTED;
      89           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL) {
      90           0 :         istate.oil_pressure_status = Oil_Pressure_Status::BELOW_NOMINAL;
      91           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL) {
      92           0 :         istate.oil_pressure_status = Oil_Pressure_Status::ABOVE_NOMINAL;
      93             :     } else {
      94           0 :         istate.oil_pressure_status = Oil_Pressure_Status::OK;
      95             :     }
      96             : 
      97           0 :     if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED)) {
      98           0 :         istate.detonation_status = Detonation_Status::NOT_SUPPORTED;
      99           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED) {
     100           0 :         istate.detonation_status = Detonation_Status::OBSERVED;
     101             :     } else {
     102           0 :         istate.detonation_status = Detonation_Status::NOT_OBSERVED;
     103             :     }
     104             : 
     105           0 :     if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED)) {
     106           0 :         istate.misfire_status = Misfire_Status::NOT_SUPPORTED;
     107           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED) {
     108           0 :         istate.misfire_status = Misfire_Status::OBSERVED;
     109             :     } else {
     110           0 :         istate.misfire_status = Misfire_Status::NOT_OBSERVED;
     111             :     }
     112             :     
     113           0 :     if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED)) {
     114           0 :         istate.debris_status = Debris_Status::NOT_SUPPORTED;
     115           0 :     } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED) {
     116           0 :         istate.debris_status = Debris_Status::DETECTED;
     117             :     } else {
     118           0 :         istate.debris_status = Debris_Status::NOT_DETECTED;
     119             :     }
     120             :     
     121           0 :     istate.engine_load_percent = pkt.engine_load_percent;
     122           0 :     istate.engine_speed_rpm = pkt.engine_speed_rpm;
     123           0 :     istate.spark_dwell_time_ms = pkt.spark_dwell_time_ms;
     124           0 :     istate.atmospheric_pressure_kpa = pkt.atmospheric_pressure_kpa;
     125           0 :     istate.intake_manifold_pressure_kpa = pkt.intake_manifold_pressure_kpa;
     126           0 :     istate.intake_manifold_temperature = pkt.intake_manifold_temperature;
     127           0 :     istate.coolant_temperature = pkt.coolant_temperature;
     128           0 :     istate.oil_pressure = pkt.oil_pressure;
     129           0 :     istate.oil_temperature = pkt.oil_temperature;
     130           0 :     istate.fuel_pressure = pkt.fuel_pressure;
     131           0 :     istate.fuel_consumption_rate_cm3pm = pkt.fuel_consumption_rate_cm3pm;
     132           0 :     istate.estimated_consumed_fuel_volume_cm3 = pkt.estimated_consumed_fuel_volume_cm3;
     133           0 :     istate.throttle_position_percent = pkt.throttle_position_percent;
     134           0 :     istate.ecu_index = pkt.ecu_index;
     135             : 
     136             :     // 1:1 for spark plug usage
     137           0 :     istate.spark_plug_usage = Spark_Plug_Usage(pkt.spark_plug_usage);
     138             : 
     139             :     // assume max one cylinder status
     140           0 :     if (pkt.cylinder_status.len > 0) {
     141           0 :         const auto &cs = pkt.cylinder_status.data[0];
     142           0 :         auto &c = istate.cylinder_status;
     143           0 :         c.ignition_timing_deg = cs.ignition_timing_deg;
     144           0 :         c.injection_time_ms = cs.injection_time_ms;
     145           0 :         c.cylinder_head_temperature = cs.cylinder_head_temperature;
     146           0 :         c.exhaust_gas_temperature = cs.exhaust_gas_temperature;
     147           0 :         c.lambda_coefficient = cs.lambda_coefficient;
     148             :     }
     149             : 
     150             :     // Required for healthy message
     151           0 :     istate.last_updated_ms = AP_HAL::millis();
     152             : 
     153           0 :     copy_to_frontend();
     154           0 : }
     155             : 
     156             : #endif // AP_EFI_DRONECAN_ENABLED
     157             :