Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_EFI.h"
17 :
18 : #if HAL_EFI_ENABLED
19 :
20 : #include "AP_EFI_Serial_MS.h"
21 : #include "AP_EFI_Serial_Lutan.h"
22 : #include "AP_EFI_NWPMU.h"
23 : #include "AP_EFI_DroneCAN.h"
24 : #include "AP_EFI_Currawong_ECU.h"
25 : #include "AP_EFI_Serial_Hirth.h"
26 : #include "AP_EFI_Loweheiser.h"
27 : #include "AP_EFI_Scripting.h"
28 : #include "AP_EFI_MAV.h"
29 :
30 : #include <AP_Logger/AP_Logger.h>
31 : #include <GCS_MAVLink/GCS.h>
32 : #include <AP_Math/AP_Math.h>
33 :
34 : #if HAL_MAX_CAN_PROTOCOL_DRIVERS
35 : #include <AP_CANManager/AP_CANManager.h>
36 : #endif
37 :
38 : extern const AP_HAL::HAL& hal;
39 :
40 : // table of user settable parameters
41 : const AP_Param::GroupInfo AP_EFI::var_info[] = {
42 : // @Param: _TYPE
43 : // @DisplayName: EFI communication type
44 : // @Description: What method of communication is used for EFI #1
45 : // @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan,4:Loweheiser,5:DroneCAN,6:Currawong-ECU,7:Scripting,8:Hirth,9:MAVLink
46 : // @User: Advanced
47 : // @RebootRequired: True
48 : AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE),
49 :
50 : // @Param: _COEF1
51 : // @DisplayName: EFI Calibration Coefficient 1
52 : // @Description: Used to calibrate fuel flow for MS protocol (Slope). This should be calculated from a log at constant fuel usage rate. Plot (ECYL[0].InjT*EFI.Rpm)/600.0 to get the duty_cycle. Measure actual fuel usage in cm^3/min, and set EFI_COEF1 = fuel_usage_cm3permin / duty_cycle
53 : // @Range: 0 1
54 : // @User: Advanced
55 : AP_GROUPINFO("_COEF1", 2, AP_EFI, coef1, 0),
56 :
57 : // @Param: _COEF2
58 : // @DisplayName: EFI Calibration Coefficient 2
59 : // @Description: Used to calibrate fuel flow for MS protocol (Offset). This can be used to correct for a non-zero offset in the fuel consumption calculation of EFI_COEF1
60 : // @Range: 0 10
61 : // @User: Advanced
62 : AP_GROUPINFO("_COEF2", 3, AP_EFI, coef2, 0),
63 :
64 : // @Param: _FUEL_DENS
65 : // @DisplayName: ECU Fuel Density
66 : // @Description: Used to calculate fuel consumption
67 : // @Units: kg/m/m/m
68 : // @Range: 0 10000
69 : // @User: Advanced
70 : AP_GROUPINFO("_FUEL_DENS", 4, AP_EFI, ecu_fuel_density, 0),
71 :
72 : #if AP_EFI_THROTTLE_LINEARISATION_ENABLED
73 : // @Group: _THRLIN
74 : // @Path: AP_EFI_ThrottleLinearisation.cpp
75 : AP_SUBGROUPINFO(throttle_linearisation, "_THRLIN", 5, AP_EFI, AP_EFI_ThrLin),
76 : #endif
77 :
78 : AP_GROUPEND
79 : };
80 :
81 : AP_EFI *AP_EFI::singleton;
82 :
83 : // Initialize parameters
84 2098 : AP_EFI::AP_EFI()
85 : {
86 2098 : singleton = this;
87 2098 : AP_Param::setup_object_defaults(this, var_info);
88 2098 : }
89 :
90 : // Initialize backends based on existing params
91 2086 : void AP_EFI::init(void)
92 : {
93 2086 : if (backend != nullptr) {
94 : // Init called twice, perhaps
95 0 : return;
96 : }
97 2086 : switch ((Type)type.get()) {
98 2074 : case Type::NONE:
99 2074 : break;
100 : #if AP_EFI_SERIAL_MS_ENABLED
101 1 : case Type::MegaSquirt:
102 1 : backend = NEW_NOTHROW AP_EFI_Serial_MS(*this);
103 1 : break;
104 : #endif
105 : #if AP_EFI_SERIAL_LUTAN_ENABLED
106 0 : case Type::Lutan:
107 0 : backend = NEW_NOTHROW AP_EFI_Serial_Lutan(*this);
108 0 : break;
109 : #endif
110 : #if AP_EFI_LOWEHEISER_ENABLED
111 10 : case Type::LOWEHEISER:
112 10 : backend = NEW_NOTHROW AP_EFI_Loweheiser(*this);
113 10 : break;
114 : #endif
115 : #if AP_EFI_NWPWU_ENABLED
116 0 : case Type::NWPMU:
117 0 : backend = NEW_NOTHROW AP_EFI_NWPMU(*this);
118 0 : break;
119 : #endif
120 : #if AP_EFI_DRONECAN_ENABLED
121 0 : case Type::DroneCAN:
122 0 : backend = NEW_NOTHROW AP_EFI_DroneCAN(*this);
123 0 : break;
124 : #endif
125 : #if AP_EFI_CURRAWONG_ECU_ENABLED
126 0 : case Type::CurrawongECU:
127 0 : backend = NEW_NOTHROW AP_EFI_Currawong_ECU(*this);
128 0 : break;
129 : #endif
130 : #if AP_EFI_SCRIPTING_ENABLED
131 0 : case Type::SCRIPTING:
132 0 : backend = NEW_NOTHROW AP_EFI_Scripting(*this);
133 0 : break;
134 : #endif
135 : #if AP_EFI_SERIAL_HIRTH_ENABLED
136 1 : case Type::Hirth:
137 1 : backend = NEW_NOTHROW AP_EFI_Serial_Hirth(*this);
138 1 : break;
139 : #endif
140 : #if AP_EFI_MAV_ENABLED
141 0 : case Type::MAV:
142 0 : backend = NEW_NOTHROW AP_EFI_MAV(*this);
143 0 : break;
144 : #endif
145 0 : default:
146 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unknown EFI type");
147 0 : break;
148 : }
149 : }
150 :
151 : // Ask all backends to update the frontend
152 7525129 : void AP_EFI::update()
153 : {
154 7525129 : if (backend) {
155 39522 : backend->update();
156 : #if HAL_LOGGING_ENABLED
157 39522 : log_status();
158 : #endif
159 : }
160 7525129 : }
161 :
162 44922 : bool AP_EFI::is_healthy(void) const
163 : {
164 44922 : if (backend == nullptr) {
165 0 : return false;
166 : }
167 44922 : return backend->healthy();
168 : }
169 :
170 : #if HAL_LOGGING_ENABLED
171 : /*
172 : write status to log
173 : */
174 39522 : void AP_EFI::log_status(void)
175 : {
176 : // @LoggerMessage: EFI
177 : // @Description: Electronic Fuel Injection system data
178 : // @Field: TimeUS: Time since system startup
179 : // @Field: LP: Reported engine load
180 : // @Field: Rpm: Reported engine RPM
181 : // @Field: SDT: Spark Dwell Time
182 : // @Field: ATM: Atmospheric pressure
183 : // @Field: IMP: Intake manifold pressure
184 : // @Field: IMT: Intake manifold temperature
185 : // @Field: ECT: Engine Coolant Temperature
186 : // @Field: OilP: Oil Pressure
187 : // @Field: OilT: Oil temperature
188 : // @Field: FP: Fuel Pressure
189 : // @Field: FCR: Fuel Consumption Rate
190 : // @Field: CFV: Consumed fuel volume
191 : // @Field: TPS: Throttle Position
192 : // @Field: IDX: Index of the publishing ECU
193 79044 : AP::logger().WriteStreaming("EFI",
194 : "TimeUS,LP,Rpm,SDT,ATM,IMP,IMT,ECT,OilP,OilT,FP,FCR,CFV,TPS,IDX",
195 : "s%qsPPOOPOP--%-",
196 : "F00C--00-0-0000",
197 : "QBIffffffffffBB",
198 : AP_HAL::micros64(),
199 39522 : state.engine_load_percent,
200 : state.engine_speed_rpm,
201 39522 : state.spark_dwell_time_ms,
202 39522 : state.atmospheric_pressure_kpa,
203 39522 : state.intake_manifold_pressure_kpa,
204 39522 : KELVIN_TO_C(state.intake_manifold_temperature),
205 39522 : KELVIN_TO_C(state.coolant_temperature),
206 39522 : state.oil_pressure,
207 39522 : KELVIN_TO_C(state.oil_temperature),
208 39522 : state.fuel_pressure,
209 39522 : state.fuel_consumption_rate_cm3pm,
210 39522 : state.estimated_consumed_fuel_volume_cm3,
211 39522 : state.throttle_position_percent,
212 39522 : state.ecu_index);
213 :
214 : // @LoggerMessage: EFI2
215 : // @Description: Electronic Fuel Injection system data - redux
216 : // @Field: TimeUS: Time since system startup
217 : // @Field: Healthy: True if EFI is healthy
218 : // @Field: ES: Engine state
219 : // @Field: GE: General error
220 : // @Field: CSE: Crankshaft sensor status
221 : // @Field: TS: Temperature status
222 : // @Field: FPS: Fuel pressure status
223 : // @Field: OPS: Oil pressure status
224 : // @Field: DS: Detonation status
225 : // @Field: MS: Misfire status
226 : // @Field: DebS: Debris status
227 : // @Field: SPU: Spark plug usage
228 : // @Field: IDX: Index of the publishing ECU
229 79044 : AP::logger().WriteStreaming("EFI2",
230 : "TimeUS,Healthy,ES,GE,CSE,TS,FPS,OPS,DS,MS,DebS,SPU,IDX",
231 : "s------------",
232 : "F------------",
233 : "QBBBBBBBBBBBB",
234 : AP_HAL::micros64(),
235 39522 : uint8_t(is_healthy()),
236 39522 : uint8_t(state.engine_state),
237 39522 : uint8_t(state.general_error),
238 39522 : uint8_t(state.crankshaft_sensor_status),
239 39522 : uint8_t(state.temperature_status),
240 39522 : uint8_t(state.fuel_pressure_status),
241 39522 : uint8_t(state.oil_pressure_status),
242 39522 : uint8_t(state.detonation_status),
243 39522 : uint8_t(state.misfire_status),
244 39522 : uint8_t(state.debris_status),
245 39522 : uint8_t(state.spark_plug_usage),
246 39522 : uint8_t(state.ecu_index));
247 :
248 : // @LoggerMessage: ECYL
249 : // @Description: EFI per-cylinder information
250 : // @Field: TimeUS: Time since system startup
251 : // @Field: Inst: Cylinder this data belongs to
252 : // @Field: IgnT: Ignition timing
253 : // @Field: InjT: Injection time
254 : // @Field: CHT: Cylinder head temperature
255 : // @Field: EGT: Exhaust gas temperature
256 : // @Field: Lambda: Estimated lambda coefficient (dimensionless ratio)
257 : // @Field: CHT2: Cylinder2 head temperature
258 : // @Field: EGT2: Cylinder2 Exhaust gas temperature
259 : // @Field: IDX: Index of the publishing ECU
260 79044 : AP::logger().WriteStreaming("ECYL",
261 : "TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,CHT2,EGT2,IDX",
262 : "s#dsOO-OO-",
263 : "F-0C000000",
264 : "QBffffffff",
265 : AP_HAL::micros64(),
266 : 0,
267 39522 : state.cylinder_status.ignition_timing_deg,
268 39522 : state.cylinder_status.injection_time_ms,
269 39522 : KELVIN_TO_C(state.cylinder_status.cylinder_head_temperature),
270 39522 : KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature),
271 39522 : state.cylinder_status.lambda_coefficient,
272 39522 : KELVIN_TO_C(state.cylinder_status.cylinder_head_temperature2),
273 39522 : KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature2),
274 39522 : state.ecu_index);
275 39522 : }
276 : #endif // HAL_LOGGING_ENABLED
277 :
278 : /*
279 : send EFI_STATUS
280 : */
281 1122204 : void AP_EFI::send_mavlink_status(mavlink_channel_t chan)
282 : {
283 1122204 : if (!backend) {
284 1116804 : return;
285 : }
286 :
287 : float ignition_voltage;
288 10002 : if (isnan(state.ignition_voltage) ||
289 4602 : is_equal(state.ignition_voltage, -1.0f)) {
290 : // zero means "unknown" in mavlink, 0.0001 means 0 volts
291 1043 : ignition_voltage = 0;
292 4357 : } else if (is_zero(state.ignition_voltage)) {
293 : // zero means "unknown" in mavlink, 0.0001 means 0 volts
294 243 : ignition_voltage = 0.0001f;
295 : } else {
296 4114 : ignition_voltage = state.ignition_voltage;
297 : };
298 :
299 : // If fuel pressure is supported, but is exactly zero, shift it to 0.0001
300 : // to indicate that it is supported.
301 5400 : float fuel_pressure = state.fuel_pressure;
302 5400 : if (is_zero(fuel_pressure) && state.fuel_pressure_status != Fuel_Pressure_Status::NOT_SUPPORTED) {
303 0 : fuel_pressure = 0.0001;
304 : }
305 :
306 5400 : mavlink_msg_efi_status_send(
307 : chan,
308 5400 : AP_EFI::is_healthy(),
309 5400 : state.ecu_index,
310 5400 : state.engine_speed_rpm,
311 : state.estimated_consumed_fuel_volume_cm3,
312 : state.fuel_consumption_rate_cm3pm,
313 5400 : state.engine_load_percent,
314 5400 : state.throttle_position_percent,
315 : state.spark_dwell_time_ms,
316 : state.atmospheric_pressure_kpa,
317 : state.intake_manifold_pressure_kpa,
318 5400 : KELVIN_TO_C(state.intake_manifold_temperature),
319 5400 : KELVIN_TO_C(state.cylinder_status.cylinder_head_temperature),
320 : state.cylinder_status.ignition_timing_deg,
321 : state.cylinder_status.injection_time_ms,
322 5400 : KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature),
323 : state.throttle_out,
324 : state.pt_compensation,
325 : ignition_voltage,
326 : fuel_pressure
327 : );
328 : }
329 :
330 : // get a copy of state structure
331 0 : void AP_EFI::get_state(EFI_State &_state)
332 : {
333 0 : WITH_SEMAPHORE(sem);
334 0 : _state = state;
335 0 : }
336 :
337 0 : void AP_EFI::handle_EFI_message(const mavlink_message_t &msg) {
338 0 : if (backend != nullptr) {
339 0 : backend->handle_EFI_message(msg);
340 : }
341 0 : }
342 :
343 : namespace AP {
344 1146774 : AP_EFI *EFI()
345 : {
346 1146774 : return AP_EFI::get_singleton();
347 : }
348 : }
349 :
350 : #endif // HAL_EFI_ENABLED
351 :
|
