mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AP_EFI: Add AP_EFI Library
This commit is contained in:
parent
0751756e91
commit
f8cf388236
186
libraries/AP_EFI/AP_EFI.cpp
Normal file
186
libraries/AP_EFI/AP_EFI.cpp
Normal file
@ -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
|
||||||
|
|
107
libraries/AP_EFI/AP_EFI.h
Normal file
107
libraries/AP_EFI/AP_EFI.h
Normal file
@ -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
|
44
libraries/AP_EFI/AP_EFI_Backend.cpp
Normal file
44
libraries/AP_EFI/AP_EFI_Backend.cpp
Normal file
@ -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
|
50
libraries/AP_EFI/AP_EFI_Backend.h
Normal file
50
libraries/AP_EFI/AP_EFI_Backend.h
Normal file
@ -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;
|
||||||
|
};
|
236
libraries/AP_EFI/AP_EFI_Serial_MS.cpp
Normal file
236
libraries/AP_EFI/AP_EFI_Serial_MS.cpp
Normal file
@ -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
|
111
libraries/AP_EFI/AP_EFI_Serial_MS.h
Normal file
111
libraries/AP_EFI/AP_EFI_Serial_MS.h
Normal file
@ -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
|
||||||
|
};
|
||||||
|
};
|
202
libraries/AP_EFI/AP_EFI_State.h
Normal file
202
libraries/AP_EFI/AP_EFI_State.h
Normal file
@ -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
Block a user