CoCalc -- AP_EFI_DroneCAN.cpp

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_EFI/AP_EFI_DroneCAN.cpp
Views: ¹⁷⁹⁸
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
AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) :
18
    AP_EFI_Backend(_frontend)
19
{
20
    driver = this;
21
}
22

23
// links the DroneCAN message to this backend
24
bool AP_EFI_DroneCAN::subscribe_msgs(AP_DroneCAN *ap_dronecan)
25
{
26
    const auto driver_index = ap_dronecan->get_driver_index();
27

28
    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
void AP_EFI_DroneCAN::update()
33
{
34
}
35

36
// EFI message handler
37
void AP_EFI_DroneCAN::trampoline_status(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg)
38
{
39
    if (driver == nullptr) {
40
        return;
41
    }
42
    driver->handle_status(msg);
43
}
44

45
/*
46
  handle reciprocating ICE status message from DroneCAN
47
 */
48
void AP_EFI_DroneCAN::handle_status(const uavcan_equipment_ice_reciprocating_Status &pkt)
49
{
50
    auto &istate = internal_state;
51

52
    // state maps 1:1 from Engine_State
53
    istate.engine_state = Engine_State(pkt.state);
54

55
    if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) {
56
        istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::NOT_SUPPORTED;
57
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR) {
58
        istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::ERROR;
59
    } else {
60
        istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::OK;
61
    }
62

63
    if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED)) {
64
        istate.temperature_status = Temperature_Status::NOT_SUPPORTED;
65
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL) {
66
        istate.temperature_status = Temperature_Status::BELOW_NOMINAL;
67
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL) {
68
        istate.temperature_status = Temperature_Status::ABOVE_NOMINAL;
69
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING) {
70
        istate.temperature_status = Temperature_Status::OVERHEATING;
71
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) {
72
        istate.temperature_status = Temperature_Status::EGT_ABOVE_NOMINAL;
73
    } else {
74
        istate.temperature_status = Temperature_Status::OK;
75
    }
76

77
    if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED)) {
78
        istate.fuel_pressure_status = Fuel_Pressure_Status::NOT_SUPPORTED;
79
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL) {
80
        istate.fuel_pressure_status = Fuel_Pressure_Status::BELOW_NOMINAL;
81
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL) {
82
        istate.fuel_pressure_status = Fuel_Pressure_Status::ABOVE_NOMINAL;
83
    } else {
84
        istate.fuel_pressure_status = Fuel_Pressure_Status::OK;
85
    }
86

87
    if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED)) {
88
        istate.oil_pressure_status = Oil_Pressure_Status::NOT_SUPPORTED;
89
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL) {
90
        istate.oil_pressure_status = Oil_Pressure_Status::BELOW_NOMINAL;
91
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL) {
92
        istate.oil_pressure_status = Oil_Pressure_Status::ABOVE_NOMINAL;
93
    } else {
94
        istate.oil_pressure_status = Oil_Pressure_Status::OK;
95
    }
96

97
    if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED)) {
98
        istate.detonation_status = Detonation_Status::NOT_SUPPORTED;
99
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED) {
100
        istate.detonation_status = Detonation_Status::OBSERVED;
101
    } else {
102
        istate.detonation_status = Detonation_Status::NOT_OBSERVED;
103
    }
104

105
    if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED)) {
106
        istate.misfire_status = Misfire_Status::NOT_SUPPORTED;
107
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED) {
108
        istate.misfire_status = Misfire_Status::OBSERVED;
109
    } else {
110
        istate.misfire_status = Misfire_Status::NOT_OBSERVED;
111
    }
112
    
113
    if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED)) {
114
        istate.debris_status = Debris_Status::NOT_SUPPORTED;
115
    } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED) {
116
        istate.debris_status = Debris_Status::DETECTED;
117
    } else {
118
        istate.debris_status = Debris_Status::NOT_DETECTED;
119
    }
120
    
121
    istate.engine_load_percent = pkt.engine_load_percent;
122
    istate.engine_speed_rpm = pkt.engine_speed_rpm;
123
    istate.spark_dwell_time_ms = pkt.spark_dwell_time_ms;
124
    istate.atmospheric_pressure_kpa = pkt.atmospheric_pressure_kpa;
125
    istate.intake_manifold_pressure_kpa = pkt.intake_manifold_pressure_kpa;
126
    istate.intake_manifold_temperature = pkt.intake_manifold_temperature;
127
    istate.coolant_temperature = pkt.coolant_temperature;
128
    istate.oil_pressure = pkt.oil_pressure;
129
    istate.oil_temperature = pkt.oil_temperature;
130
    istate.fuel_pressure = pkt.fuel_pressure;
131
    istate.fuel_consumption_rate_cm3pm = pkt.fuel_consumption_rate_cm3pm;
132
    istate.estimated_consumed_fuel_volume_cm3 = pkt.estimated_consumed_fuel_volume_cm3;
133
    istate.throttle_position_percent = pkt.throttle_position_percent;
134
    istate.ecu_index = pkt.ecu_index;
135

136
    // 1:1 for spark plug usage
137
    istate.spark_plug_usage = Spark_Plug_Usage(pkt.spark_plug_usage);
138

139
    // assume max one cylinder status
140
    if (pkt.cylinder_status.len > 0) {
141
        const auto &cs = pkt.cylinder_status.data[0];
142
        auto &c = istate.cylinder_status;
143
        c.ignition_timing_deg = cs.ignition_timing_deg;
144
        c.injection_time_ms = cs.injection_time_ms;
145
        c.cylinder_head_temperature = cs.cylinder_head_temperature;
146
        c.exhaust_gas_temperature = cs.exhaust_gas_temperature;
147
        c.lambda_coefficient = cs.lambda_coefficient;
148
    }
149

150
    // Required for healthy message
151
    istate.last_updated_ms = AP_HAL::millis();
152

153
    copy_to_frontend();
154
}
155

156
#endif // AP_EFI_DRONECAN_ENABLED
157

158

159
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
