mirror of https://github.com/ArduPilot/ardupilot
AP_EFI: Add AP_EFI Library
This commit is contained in:
parent
0751756e91
commit
f8cf388236
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "AP_EFI.h"
|
||||
|
||||
#if EFI_ENABLED
|
||||
|
||||
#include "AP_EFI_Serial_MS.h"
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
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
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#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
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_EFI.h"
|
||||
#include "AP_EFI_State.h"
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
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;
|
||||
};
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_EFI_Serial_MS.h"
|
||||
|
||||
#if EFI_ENABLED
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
|
||||
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
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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
|
||||
};
|
||||
};
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define EFI_MAX_INSTANCES 2
|
||||
#define EFI_MAX_BACKENDS 2
|
||||
#define ENGINE_MAX_CYLINDERS 1
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
// 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];
|
||||
|
||||
};
|
Loading…
Reference in New Issue