From f8cf38823640b0a1f7f5281571f6bc4c0bf6be6c Mon Sep 17 00:00:00 2001 From: Sriram Sami Date: Wed, 28 Mar 2018 16:37:58 -0700 Subject: [PATCH] AP_EFI: Add AP_EFI Library --- libraries/AP_EFI/AP_EFI.cpp | 186 ++++++++++++++++++++ libraries/AP_EFI/AP_EFI.h | 107 ++++++++++++ libraries/AP_EFI/AP_EFI_Backend.cpp | 44 +++++ libraries/AP_EFI/AP_EFI_Backend.h | 50 ++++++ libraries/AP_EFI/AP_EFI_Serial_MS.cpp | 236 ++++++++++++++++++++++++++ libraries/AP_EFI/AP_EFI_Serial_MS.h | 111 ++++++++++++ libraries/AP_EFI/AP_EFI_State.h | 202 ++++++++++++++++++++++ 7 files changed, 936 insertions(+) create mode 100644 libraries/AP_EFI/AP_EFI.cpp create mode 100644 libraries/AP_EFI/AP_EFI.h create mode 100644 libraries/AP_EFI/AP_EFI_Backend.cpp create mode 100644 libraries/AP_EFI/AP_EFI_Backend.h create mode 100644 libraries/AP_EFI/AP_EFI_Serial_MS.cpp create mode 100644 libraries/AP_EFI/AP_EFI_Serial_MS.h create mode 100644 libraries/AP_EFI/AP_EFI_State.h diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp new file mode 100644 index 0000000000..806fc88fd2 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -0,0 +1,186 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_EFI.h" + +#if EFI_ENABLED + +#include "AP_EFI_Serial_MS.h" +#include + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +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 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: _COEF1 + // @DisplayName: EFI Calibration Coefficient 1 + // @Description: Used to calibrate fuel flow for MS protocol (Slope) + // @Range: 0 1 + // @User: Advanced + // @RebootRequired: False + AP_GROUPINFO("_COEF1", 2, AP_EFI, coef1, 0), + + // @Param: _COEF2 + // @DisplayName: EFI Calibration Coefficient 2 + // @Description: Used to calibrate fuel flow for MS protocol (Offset) + // @Range: 0 10 + // @User: Advanced + // @RebootRequired: False + AP_GROUPINFO("_COEF2", 3, AP_EFI, coef2, 0), + + AP_GROUPEND +}; + +AP_EFI *AP_EFI::singleton; + +// Initialize parameters +AP_EFI::AP_EFI() +{ + singleton = this; + AP_Param::setup_object_defaults(this, var_info); +} + +// Initialize backends based on existing params +void AP_EFI::init(void) +{ + if (backend != nullptr) { + // Init called twice, perhaps + return; + } + // Check for MegaSquirt Serial EFI + if (type == EFI_COMMUNICATION_TYPE_SERIAL_MS) { + backend = new AP_EFI_Serial_MS(*this); + } +} + +// Ask all backends to update the frontend +void AP_EFI::update() +{ + if (backend) { + backend->update(); + log_status(); + } +} + +bool AP_EFI::is_healthy(void) const +{ + return (backend && (AP_HAL::millis() - state.last_updated_ms) < HEALTHY_LAST_RECEIVED_MS); +} + +/* + write status to log + */ +void AP_EFI::log_status(void) +{ + AP::logger().Write("EFI", + "TimeUS,LP,Rpm,SDT,ATM,IMP,IMT,ECT,OilP,OilT,FP,FCR,CFV,TPS,IDX", + "s%qsPPOOPOP--%-", + "F00C--00-0-0000", + "QBIffffffffffBB", + AP_HAL::micros64(), + uint8_t(state.engine_load_percent), + uint32_t(state.engine_speed_rpm), + float(state.spark_dwell_time_ms), + float(state.atmospheric_pressure_kpa), + float(state.intake_manifold_pressure_kpa), + float(state.intake_manifold_temperature), + float(state.coolant_temperature), + float(state.oil_pressure), + float(state.oil_temperature), + float(state.fuel_pressure), + float(state.fuel_consumption_rate_cm3pm), + float(state.estimated_consumed_fuel_volume_cm3), + uint8_t(state.throttle_position_percent), + uint8_t(state.ecu_index)); + + AP::logger().Write("EFI2", + "TimeUS,Healthy,ES,GE,CSE,TS,FPS,OPS,DS,MS,DS,SPU,IDX", + "s------------", + "F------------", + "QBBBBBBBBBBBB", + AP_HAL::micros64(), + uint8_t(is_healthy()), + uint8_t(state.engine_state), + uint8_t(state.general_error), + uint8_t(state.crankshaft_sensor_status), + uint8_t(state.temperature_status), + uint8_t(state.fuel_pressure_status), + uint8_t(state.oil_pressure_status), + uint8_t(state.detonation_status), + uint8_t(state.misfire_status), + uint8_t(state.debris_status), + uint8_t(state.spark_plug_usage), + uint8_t(state.ecu_index)); + + for (uint8_t i = 0; i < ENGINE_MAX_CYLINDERS; i++) { + AP::logger().Write("ECYL", + "TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,IDX", + "s#dsOO--", + "F-0C0000", + "QBfffffB", + AP_HAL::micros64(), + i, + state.cylinder_status[i].ignition_timing_deg, + state.cylinder_status[i].injection_time_ms, + state.cylinder_status[i].cylinder_head_temperature, + state.cylinder_status[i].exhaust_gas_temperature, + state.cylinder_status[i].lambda_coefficient, + state.ecu_index); + } +} + +/* + send EFI_STATUS + */ +void AP_EFI::send_mavlink_status(mavlink_channel_t chan) +{ + if (!backend) { + return; + } + mavlink_msg_efi_status_send( + chan, + AP_EFI::is_healthy(), + state.ecu_index, + state.engine_speed_rpm, + state.estimated_consumed_fuel_volume_cm3, + state.fuel_consumption_rate_cm3pm, + state.engine_load_percent, + state.throttle_position_percent, + state.spark_dwell_time_ms, + state.atmospheric_pressure_kpa, + state.intake_manifold_pressure_kpa, + (state.intake_manifold_temperature - 273.0f), + (state.cylinder_status[0].cylinder_head_temperature - 273.0f), + state.cylinder_status[0].ignition_timing_deg, + state.cylinder_status[0].injection_time_ms); +} + +namespace AP { +AP_EFI *EFI() +{ + return AP_EFI::get_singleton(); +} +} + +#endif // EFI_ENABLED + diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h new file mode 100644 index 0000000000..22329768e9 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI.h @@ -0,0 +1,107 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + + + +#include +#include +#include + +#define EFI_ENABLED !HAL_MINIMIZE_FEATURES + +#if EFI_ENABLED +#include "AP_EFI_Backend.h" +#include "AP_EFI_State.h" + + +/* + * This library aims to read data from Electronic Fuel Injection + * or Engine Control units. It is focused around the generic + * internal combustion engine state message provided by the + * UAVCAN protocol due to its comprehensiveness, but is extensible + * to use other forms of data transfer besides UAVCAN. + * + * + * + * Authors: Sriram Sami and David Ingraham + * With direction from Andrew Tridgell, Robert Lefebvre, Francisco Ferreira and + * Pavel Kirienko. + * Thanks to Yonah, SpektreWorks Inc, and HFE International. + */ + +class AP_EFI { +public: + friend class AP_EFI_Backend; + + // For parameter initialization + AP_EFI(); + + // Initializes backend + void init(void); + + // Requests backend to update the frontend. Should be called at 10Hz. + void update(); + + // Returns the RPM + uint32_t get_rpm() const { return state.engine_speed_rpm; } + + // returns enabled state of EFI + bool enabled() const { return type != EFI_COMMUNICATION_TYPE_NONE; } + + bool is_healthy() const; + + // Parameter info + static const struct AP_Param::GroupInfo var_info[]; + + // Backend driver types + enum EFI_Communication_Type { + EFI_COMMUNICATION_TYPE_NONE = 0, + EFI_COMMUNICATION_TYPE_SERIAL_MS = 1 + }; + + static AP_EFI *get_singleton(void) { + return singleton; + } + + // send EFI_STATUS + void send_mavlink_status(mavlink_channel_t chan); + +protected: + + // Back end Parameters + AP_Float coef1; + AP_Float coef2; + + EFI_State state; + +private: + // Front End Parameters + AP_Int8 type; + + // Tracking backends + AP_EFI_Backend *backend; + static AP_EFI *singleton; + + // write to log + void log_status(); +}; + +namespace AP { + AP_EFI *EFI(); +}; + +#endif // EFI_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp new file mode 100644 index 0000000000..072186da3c --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Backend.cpp @@ -0,0 +1,44 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_EFI.h" + +#if EFI_ENABLED + +#include "AP_EFI_Backend.h" + +extern const AP_HAL::HAL &hal; + +AP_EFI_Backend::AP_EFI_Backend(AP_EFI &_frontend) : + frontend(_frontend) +{ +} + +void AP_EFI_Backend::copy_to_frontend() +{ + WITH_SEMAPHORE(sem); + frontend.state = internal_state; +} + +float AP_EFI_Backend::get_coef1(void) const +{ + return frontend.coef1; +} + +float AP_EFI_Backend::get_coef2(void) const +{ + return frontend.coef2; +} +#endif // EFI_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h new file mode 100644 index 0000000000..2f48a4694e --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -0,0 +1,50 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_EFI.h" +#include "AP_EFI_State.h" +#include + +class AP_EFI; //forward declaration + +class AP_EFI_Backend { +public: + // Constructor with initialization + AP_EFI_Backend(AP_EFI &_frontend); + + // Virtual destructor that efi backends can override + virtual ~AP_EFI_Backend(void) {} + + // Update the state structure + virtual void update() = 0; + +protected: + // Copies internal state to the frontend state + void copy_to_frontend(); + + // Semaphore for access to shared frontend data + HAL_Semaphore sem; + + // Internal state for this driver (before copying to frontend) + EFI_State internal_state; + + int8_t get_uavcan_node_id(void) const; + float get_coef1(void) const; + float get_coef2(void) const; + +private: + AP_EFI &frontend; +}; diff --git a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp new file mode 100644 index 0000000000..069cd92523 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp @@ -0,0 +1,236 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include "AP_EFI_Serial_MS.h" + +#if EFI_ENABLED +#include + +extern const AP_HAL::HAL &hal; + +AP_EFI_Serial_MS::AP_EFI_Serial_MS(AP_EFI &_frontend): + AP_EFI_Backend(_frontend) +{ + internal_state.estimated_consumed_fuel_volume_cm3 = 0; // Just to be sure + port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI_MS, 0); +} + + +void AP_EFI_Serial_MS::update() +{ + if (!port) { + return; + } + + uint32_t now = AP_HAL::millis(); + + const uint32_t expected_bytes = 2 + (RT_LAST_OFFSET - RT_FIRST_OFFSET) + 4; + if (port->available() >= expected_bytes && read_incoming_realtime_data()) { + last_response_ms = now; + copy_to_frontend(); + } + + if (port->available() == 0 || now - last_response_ms > 200) { + // clear the input buffer + uint32_t buffered_data_size = port->available(); + for (uint32_t i = 0; i < buffered_data_size; i++) { + port->read(); + } + // Request an update from the realtime table (7). + // The data we need start at offset 6 and ends at 129 + send_request(7, RT_FIRST_OFFSET, RT_LAST_OFFSET); + } +} + +bool AP_EFI_Serial_MS::read_incoming_realtime_data() +{ + // Data is parsed directly from the buffer, otherwise we would need to allocate + // several hundred bytes for the entire realtime data table or request every + // value individiually + uint16_t message_length = 0; + + // reset checksum before reading new data + checksum = 0; + + // Message length field begins the message (16 bits, excluded from CRC calculation) + // Message length value excludes the message length and CRC bytes + message_length = port->read() << 8; + message_length += port->read(); + + if (message_length >= 256) { + // don't process invalid messages + // hal.console->printf("message_length: %u\n", message_length); + return false; + } + + // Response Flag (see "response_codes" enum) + response_flag = read_byte_CRC32(); + if (response_flag != RESPONSE_WRITE_OK) { + // abort read if we did not receive the correct response code; + return false; + } + + // Iterate over the payload bytes + for ( uint8_t offset=RT_FIRST_OFFSET; offset < (RT_FIRST_OFFSET + message_length - 1); offset++) { + uint8_t data = read_byte_CRC32(); + float temp_float; + switch (offset) { + case PW1_MSB: + internal_state.cylinder_status[0].injection_time_ms = (float)((data << 8) + read_byte_CRC32())/1000.0f; + offset++; // increment the counter because we read a byte in the previous line + break; + case RPM_MSB: + // Read 16 bit RPM + internal_state.engine_speed_rpm = (data << 8) + read_byte_CRC32(); + offset++; + break; + case ADVANCE_MSB: + internal_state.cylinder_status[0].ignition_timing_deg = (float)((data << 8) + read_byte_CRC32())/10.0f; + offset++; + break; + case ENGINE_BM: + break; + case BAROMETER_MSB: + internal_state.atmospheric_pressure_kpa = (float)((data << 8) + read_byte_CRC32())/10.0f; + offset++; + break; + case MAP_MSB: + internal_state.intake_manifold_pressure_kpa = (float)((data << 8) + read_byte_CRC32())/10.0f; + offset++; + break; + case MAT_MSB: + temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; + offset++; + internal_state.intake_manifold_temperature = f_to_k(temp_float); + break; + case CHT_MSB: + temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; + offset++; + internal_state.cylinder_status[0].cylinder_head_temperature = f_to_k(temp_float); + break; + case TPS_MSB: + temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; + offset++; + internal_state.throttle_position_percent = roundf(temp_float); + break; + case AFR1_MSB: + temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; + offset++; + internal_state.cylinder_status[0].lambda_coefficient = temp_float; + break; + case DWELL_MSB: + temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; + internal_state.spark_dwell_time_ms = temp_float; + offset++; + break; + case LOAD: + internal_state.engine_load_percent = data; + break; + case FUEL_PRESSURE_MSB: + // MS Fuel Pressure is unitless, store as KPA anyway + temp_float = (float)((data << 8) + read_byte_CRC32()); + internal_state.fuel_pressure = temp_float; + offset++; + break; + + } + } + + // Read the four CRC bytes + uint32_t received_CRC; + received_CRC = port->read() << 24; + received_CRC += port->read() << 16; + received_CRC += port->read() << 8; + received_CRC += port->read(); + + if (received_CRC != checksum) { + // hal.console->printf("EFI CRC: 0x%08x 0x%08x\n", received_CRC, checksum); + return false; + } + + // Calculate Fuel Consumption + // Duty Cycle (Percent, because that's how HFE gives us the calibration coefficients) + float duty_cycle = (internal_state.cylinder_status[0].injection_time_ms * internal_state.engine_speed_rpm)/600.0f; + uint32_t current_time = AP_HAL::millis(); + // Super Simplified integration method - Error Analysis TBD + // This calcualtion gives erroneous results when the engine isn't running + if (internal_state.engine_speed_rpm > RPM_THRESHOLD) { + internal_state.fuel_consumption_rate_cm3pm = duty_cycle*get_coef1() - get_coef2(); + internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (current_time - internal_state.last_updated_ms)/60000.0f; + } else { + internal_state.fuel_consumption_rate_cm3pm = 0; + } + internal_state.last_updated_ms = current_time; + + return true; + +} + +void AP_EFI_Serial_MS::send_request(uint8_t table, uint16_t first_offset, uint16_t last_offset) +{ + uint16_t length = last_offset - first_offset + 1; + // Fixed message size (0x0007) + // Command 'r' (0x72) + // Null CANid (0x00) + const uint8_t data[9] = { + 0x00, + 0x07, + 0x72, + 0x00, + (uint8_t)table, + (uint8_t)(first_offset >> 8), + (uint8_t)(first_offset), + (uint8_t)(length >> 8), + (uint8_t)(length) + }; + + uint32_t crc = 0; + + // Write the request and calc CRC + for (uint8_t i = 0; i != sizeof(data) ; i++) { + // Message size is excluded from CRC + if (i > 1) { + crc = CRC32_compute_byte(crc, data[i]); + } + port->write(data[i]); + } + + // Write the CRC32 + port->write((uint8_t)(crc >> 24)); + port->write((uint8_t)(crc >> 16)); + port->write((uint8_t)(crc >> 8)); + port->write((uint8_t)crc); + +} + +uint8_t AP_EFI_Serial_MS::read_byte_CRC32() +{ + // Read a byte and update the CRC + uint8_t data = port->read(); + checksum = CRC32_compute_byte(checksum, data); + return data; +} + +// CRC32 matching MegaSquirt +uint32_t AP_EFI_Serial_MS::CRC32_compute_byte(uint32_t crc, uint8_t data) +{ + crc ^= ~0U; + crc = crc_crc32(crc, &data, 1); + crc ^= ~0U; + return crc; +} + +#endif // EFI_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Serial_MS.h b/libraries/AP_EFI/AP_EFI_Serial_MS.h new file mode 100644 index 0000000000..b78608a883 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_MS.h @@ -0,0 +1,111 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_EFI.h" +#include "AP_EFI_Backend.h" + +// RPM Threshold for fuel consumption estimator +#define RPM_THRESHOLD 100 + +class AP_EFI_Serial_MS: public AP_EFI_Backend { + +public: + // Constructor with initialization + AP_EFI_Serial_MS(AP_EFI &_frontend); + + // Update the state structure + void update() override; + +private: + AP_HAL::UARTDriver *port; + void parse_realtime_data(); + bool read_incoming_realtime_data(); + void send_request(uint8_t table, uint16_t first_offset, uint16_t last_offset); + uint8_t read_byte_CRC32(); + uint32_t CRC32_compute_byte(uint32_t inCrc32, uint8_t data); + float f_to_k(float temp_f) { return (temp_f + 459.67f) * 0.55556f; }; + + // Serial Protocol Variables + uint32_t checksum; + uint8_t step; + uint8_t response_flag; + uint16_t message_counter; + uint32_t last_response_ms; + + // confirmed that last command was ok + bool last_command_confirmed; + + // Command Response Codes + enum response_codes { + RESPONSE_WRITE_OK =0x00, + RESPONSE_REALTIME_DATA, + RESPONSE_PAGE_DATA, + RESPONSE_CONFIG_ERROR, + RESPONSE_PAGE10_OK, + RESPONSE_CAN_DATA, + // Error Responses + ERR_UNDERRUN = 0X80, + ERR_OVERRUN, + ERR_CRC_FAILURE, + ERR_UNRECOGNIZED_COMMAND, + ERR_OUT_OF_RANGE, + ERR_SERIAL_BUSY, + ERR_FLASH_LOCKED, + ERR_SEQ_FAIL_1, + ERR_SEQ_FAIL_2, + ERR_CAN_QUEUE_FULL, + ERR_CAN_TIMEOUT, + ERR_CAN_FAILURE, + ERR_PARITY, + ERR_FRAMING, + ERR_SERIAL_NOISE, + ERR_TXMODE_RANGE, + ERR_UNKNOWN + }; + + // Realtime Data Table Locations + enum realtime_data { + PW1_MSB = 2, + PW1_LSB, + RPM_MSB = 6, + RPM_LSB, + ADVANCE_MSB, + ADVANCE_LSB, + ENGINE_BM = 11, + BAROMETER_MSB = 16, + BAROMETER_LSB, + MAP_MSB, + MAP_LSB, + MAT_MSB, + MAT_LSB, + CHT_MSB, + CHT_LSB, + TPS_MSB, + TPS_LSB, + AFR1_MSB = 28, + AFR1_LSB, + AFR2_MSB, + AFR2_LSB, + DWELL_MSB = 62, + DWELL_LSB, + LOAD = 66, + FUEL_PRESSURE_MSB = 128, + FUEL_PRESSURE_LSB, + // Helpers used when sending request + RT_FIRST_OFFSET = PW1_MSB, + RT_LAST_OFFSET = FUEL_PRESSURE_LSB + }; +}; diff --git a/libraries/AP_EFI/AP_EFI_State.h b/libraries/AP_EFI/AP_EFI_State.h new file mode 100644 index 0000000000..ccaf491584 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_State.h @@ -0,0 +1,202 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#define EFI_MAX_INSTANCES 2 +#define EFI_MAX_BACKENDS 2 +#define ENGINE_MAX_CYLINDERS 1 + +#include +#include + +// Time in milliseconds before we declare the EFI to be "unhealthy" +#define HEALTHY_LAST_RECEIVED_MS 3000 + +/*************** + * + * Status enums + * + ***************/ + +enum class Engine_State : uint8_t { + STOPPED = 0, + STARTING = 1, + RUNNING = 2, + FAULT = 3 +}; + +enum class Crankshaft_Sensor_Status : uint8_t { + NOT_SUPPORTED = 0, + OK = 1, + ERROR = 2 +}; + +enum class Temperature_Status : uint8_t { + NOT_SUPPORTED = 0, + OK = 1, + BELOW_NOMINAL = 2, + ABOVE_NOMINAL = 3, + OVERHEATING = 4, + EGT_ABOVE_NOMINAL = 5 +}; + +enum class Fuel_Pressure_Status : uint8_t { + NOT_SUPPORTED = 0, + OK = 1, + BELOW_NOMINAL = 2, + ABOVE_NOMINAL = 3 +}; + +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 +}; + +enum class Detonation_Status : uint8_t { + NOT_SUPPORTED = 0, + NOT_OBSERVED = 1, + OBSERVED = 2 +}; + +enum class Misfire_Status : uint8_t { + NOT_SUPPORTED = 0, + NOT_OBSERVED = 1, + OBSERVED = 2 +}; + +enum class Debris_Status : uint8_t { + NOT_SUPPORTED = 0, + NOT_DETECTED = 1, + DETECTED = 2 +}; + +enum class Spark_Plug_Usage : uint8_t { + SINGLE = 0, + FIRST_ACTIVE = 1, + SECOND_ACTIVE = 2, + BOTH_ACTIVE = 3 +}; + + +/*************** + * Status structs. + * EFIs may not provide all data in the message, therefore, the following guidelines should be followed. + * All integer fields are required unless stated otherwise. + * All floating point fields are optional unless stated otherwise; unknown/unapplicable fields will be NaN. + ***************/ + + +// Per-cylinder status struct +struct Cylinder_Status { + // Cylinder ignition timing (angular degrees of the crankshaft) + float ignition_timing_deg; + + // Fuel injection time (millisecond) + float injection_time_ms; + + // Cylinder head temperature (CHT) (kelvin) + float cylinder_head_temperature; + + // Exhaust gas temperature (EGT) (kelvin) + // If this cylinder is not equipped with an EGT sensor - will be NaN + // If there is a single shared EGT sensor, will be the same value for all cylinders + float exhaust_gas_temperature; + + // Estimated lambda coefficient (dimensionless ratio) + // Useful for monitoring and tuning purposes. + float lambda_coefficient; +}; + +// Stores the current state read by the EFI system +// All backends are required to fill in this state structure +struct EFI_State { + // When this structure was last updated (milliseconds) + uint32_t last_updated_ms; + + // Current overall engine state + Engine_State engine_state; + + // If there is an error that does not fit other error types + bool general_error; + + // Error/status fields + Crankshaft_Sensor_Status crankshaft_sensor_status; + Temperature_Status temperature_status; + Fuel_Pressure_Status fuel_pressure_status; + Oil_Pressure_Status oil_pressure_status; + Detonation_Status detonation_status; + Misfire_Status misfire_status; + Debris_Status debris_status; + + // Engine load (percent) + uint8_t engine_load_percent; + + // Engine speed (revolutions per minute) + uint32_t engine_speed_rpm; + + // Spark dwell time (milliseconds) + float spark_dwell_time_ms; + + // Atmospheric (barometric) pressure (kilopascal) + float atmospheric_pressure_kpa; + + // Engine intake manifold pressure (kilopascal) + float intake_manifold_pressure_kpa; + + // Engine intake manifold temperature (kelvin) + float intake_manifold_temperature; + + // Engine coolant temperature (kelvin) + float coolant_temperature; + + // Oil pressure (kilopascal) + float oil_pressure; + + // Oil temperature (kelvin) + float oil_temperature; + + // Fuel pressure (kilopascal) + float fuel_pressure; + + // Instant fuel consumption estimate, which + // should be low-pass filtered in order to prevent aliasing effects. + // (centimeter^3)/minute. + float fuel_consumption_rate_cm3pm; + + // Estimate of the consumed fuel since the start of the engine (centimeter^3) + // This variable is reset when the engine is stopped. + float estimated_consumed_fuel_volume_cm3; + + // Throttle position (percent) + uint8_t throttle_position_percent; + + // The index of the publishing ECU. + uint8_t ecu_index; + + // Spark plug activity report. + // Can be used during pre-flight tests of the spark subsystem. + // Use case is that usually on double spark plug engines, the + // engine switch has the positions OFF-LEFT-RIGHT-BOTH-START. + // Gives pilots the possibility to test both spark plugs on + // ground before takeoff. + Spark_Plug_Usage spark_plug_usage; + + // Status for each cylinder in the engine + Cylinder_Status cylinder_status[ENGINE_MAX_CYLINDERS]; + +};