aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_EFI: added DroneCAN EFI driver

Browse Source
This commit is contained in:
Andrew Tridgell 2022-06-04 08:09:58 +10:00
parent 465dbd89f4
commit 9ddd1afb34
8 changed files with 223 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_EFI/AP_EFI.cpp
View File

@ -20,6 +20,7 @@
#include "AP_EFI_Serial_MS.h"
#include "AP_EFI_Serial_Lutan.h"
#include "AP_EFI_NWPMU.h"
#include "AP_EFI_DroneCAN.h"
#include <AP_Logger/AP_Logger.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
@ -33,7 +34,7 @@ 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,2:NWPMU,3:Serial-Lutan
// @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan,5:DroneCAN
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE),
@ -83,6 +84,11 @@ void AP_EFI::init(void)
case Type::NWPMU:
#if HAL_EFI_NWPWU_ENABLED
backend = new AP_EFI_NWPMU(*this);
#endif
break;
case Type::DroneCAN:
#if HAL_EFI_DRONECAN_ENABLED
backend = new AP_EFI_DroneCAN(*this);
#endif
break;
default:

1
libraries/AP_EFI/AP_EFI.h
View File

@ -82,6 +82,7 @@ public:
NWPMU = 2,
Lutan = 3,
// LOWEHEISER = 4,
DroneCAN = 5,
};
static AP_EFI *get_singleton(void) {

6
libraries/AP_EFI/AP_EFI_Backend.cpp
View File

@ -41,4 +41,10 @@ float AP_EFI_Backend::get_coef2(void) const
{
return frontend.coef2;
}
HAL_Semaphore &AP_EFI_Backend::get_sem(void)
{
return frontend.sem;
}
#endif // HAL_EFI_ENABLED

2
libraries/AP_EFI/AP_EFI_Backend.h
View File

@ -42,6 +42,8 @@ protected:
float get_coef1(void) const;
float get_coef2(void) const;
HAL_Semaphore &get_sem(void);
private:
AP_EFI &frontend;
};

169
libraries/AP_EFI/AP_EFI_DroneCAN.cpp Normal file
View File

@ -0,0 +1,169 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_EFI_DroneCAN.h"
#if HAL_EFI_DRONECAN_ENABLED
#include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/equipment/ice/reciprocating/Status.hpp>
extern const AP_HAL::HAL& hal;
AP_EFI_DroneCAN *AP_EFI_DroneCAN::driver;
// DroneCAN Frontend Registry Binder
UC_REGISTRY_BINDER(EFIStatusCb, uavcan::equipment::ice::reciprocating::Status);
// constructor
AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) :
AP_EFI_Backend(_frontend)
{
driver = this;
}
// links the DroneCAN message to this backend
void AP_EFI_DroneCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
{
if (ap_uavcan == nullptr) {
return;
}
auto* node = ap_uavcan->get_node();
uavcan::Subscriber<uavcan::equipment::ice::reciprocating::Status, EFIStatusCb> *status_listener;
status_listener = new uavcan::Subscriber<uavcan::equipment::ice::reciprocating::Status, EFIStatusCb>(*node);
// Register method to handle incoming status
const int status_listener_res = status_listener->start(EFIStatusCb(ap_uavcan, &trampoline_status));
if (status_listener_res < 0) {
AP_HAL::panic("DroneCAN EFI subscriber start problem\n\r");
return;
}
}
// Called from frontend to update with the readings received by handler
void AP_EFI_DroneCAN::update()
{
}
// EFI message handler
void AP_EFI_DroneCAN::trampoline_status(AP_UAVCAN *ap_uavcan, uint8_t node_id, const EFIStatusCb &cb)
{
if (driver == nullptr) {
return;
}
const uavcan::equipment::ice::reciprocating::Status &pkt = *cb.msg;
driver->handle_status(pkt);
}
/*
handle reciprocating ICE status message from DroneCAN
*/
void AP_EFI_DroneCAN::handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt)
{
auto &istate = internal_state;
// state maps 1:1 from Engine_State
istate.engine_state = Engine_State(pkt.state);
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) {
istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR) {
istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::ERROR;
} else {
istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::OK;
}
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_SUPPORTED)) {
istate.temperature_status = Temperature_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_BELOW_NOMINAL) {
istate.temperature_status = Temperature_Status::BELOW_NOMINAL;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_ABOVE_NOMINAL) {
istate.temperature_status = Temperature_Status::ABOVE_NOMINAL;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_OVERHEATING) {
istate.temperature_status = Temperature_Status::OVERHEATING;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) {
istate.temperature_status = Temperature_Status::EGT_ABOVE_NOMINAL;
} else {
istate.temperature_status = Temperature_Status::OK;
}
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_SUPPORTED)) {
istate.fuel_pressure_status = Fuel_Pressure_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_BELOW_NOMINAL) {
istate.fuel_pressure_status = Fuel_Pressure_Status::BELOW_NOMINAL;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_ABOVE_NOMINAL) {
istate.fuel_pressure_status = Fuel_Pressure_Status::ABOVE_NOMINAL;
} else {
istate.fuel_pressure_status = Fuel_Pressure_Status::OK;
}
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_SUPPORTED)) {
istate.oil_pressure_status = Oil_Pressure_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_BELOW_NOMINAL) {
istate.oil_pressure_status = Oil_Pressure_Status::BELOW_NOMINAL;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_ABOVE_NOMINAL) {
istate.oil_pressure_status = Oil_Pressure_Status::ABOVE_NOMINAL;
} else {
istate.oil_pressure_status = Oil_Pressure_Status::OK;
}
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_SUPPORTED)) {
istate.detonation_status = Detonation_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_OBSERVED) {
istate.detonation_status = Detonation_Status::OBSERVED;
} else {
istate.detonation_status = Detonation_Status::NOT_OBSERVED;
}
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_SUPPORTED)) {
istate.misfire_status = Misfire_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_OBSERVED) {
istate.misfire_status = Misfire_Status::OBSERVED;
} else {
istate.misfire_status = Misfire_Status::NOT_OBSERVED;
}
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_SUPPORTED)) {
istate.debris_status = Debris_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_DETECTED) {
istate.debris_status = Debris_Status::DETECTED;
} else {
istate.debris_status = Debris_Status::NOT_DETECTED;
}
istate.engine_load_percent = pkt.engine_load_percent;
istate.engine_speed_rpm = pkt.engine_speed_rpm;
istate.spark_dwell_time_ms = pkt.spark_dwell_time_ms;
istate.atmospheric_pressure_kpa = pkt.atmospheric_pressure_kpa;
istate.intake_manifold_pressure_kpa = pkt.intake_manifold_pressure_kpa;
istate.intake_manifold_temperature = pkt.intake_manifold_temperature;
istate.coolant_temperature = pkt.coolant_temperature;
istate.oil_pressure = pkt.oil_pressure;
istate.oil_temperature = pkt.oil_temperature;
istate.fuel_pressure = pkt.fuel_pressure;
istate.fuel_consumption_rate_cm3pm = pkt.fuel_consumption_rate_cm3pm;
istate.estimated_consumed_fuel_volume_cm3 = pkt.estimated_consumed_fuel_volume_cm3;
istate.throttle_position_percent = pkt.throttle_position_percent;
istate.ecu_index = pkt.ecu_index;
// 1:1 for spark plug usage
istate.spark_plug_usage = Spark_Plug_Usage(pkt.spark_plug_usage);
// assume max one cylinder status
if (pkt.cylinder_status.size() > 0) {
const auto &cs = pkt.cylinder_status[0];
auto &c = istate.cylinder_status[0];
c.ignition_timing_deg = cs.ignition_timing_deg;
c.injection_time_ms = cs.injection_time_ms;
c.cylinder_head_temperature = cs.cylinder_head_temperature;
c.exhaust_gas_temperature = cs.exhaust_gas_temperature;
c.lambda_coefficient = cs.lambda_coefficient;
}
copy_to_frontend();
}
#endif // HAL_EFI_DRONECAN_ENABLED

32
libraries/AP_EFI/AP_EFI_DroneCAN.h Normal file
View File

@ -0,0 +1,32 @@
#pragma once
#include "AP_EFI.h"
#include "AP_EFI_Backend.h"
#ifndef HAL_EFI_DRONECAN_ENABLED
#define HAL_EFI_DRONECAN_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && BOARD_FLASH_SIZE > 1024 && HAL_CANMANAGER_ENABLED
#endif
#if HAL_EFI_DRONECAN_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/equipment/ice/reciprocating/Status.hpp>
class EFIStatusCb;
class AP_EFI_DroneCAN : public AP_EFI_Backend {
public:
AP_EFI_DroneCAN(AP_EFI &_frontend);
void update() override;
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static void trampoline_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const EFIStatusCb &cb);
private:
void handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt);
// singleton for trampoline
static AP_EFI_DroneCAN *driver;
};
#endif // HAL_EFI_DRONECAN_ENABLED

4
libraries/AP_EFI/AP_EFI_NWPMU.cpp
View File

@ -36,7 +36,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame)
{
const uint32_t id = frame.id & AP_HAL::CANFrame::MaskExtID;
WITH_SEMAPHORE(frontend.sem);
WITH_SEMAPHORE(get_sem());
switch ((NWPMU_ID)id) {
case NWPMU_ID::ECU_1: {
@ -104,7 +104,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame)
struct ecu_6 data;
memcpy(&data, frame.data, sizeof(data));
if (!_emitted_version && (AP_HAL::millis() > 10000)) { // don't emit a version early in the boot process
gcs().send_text(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d",
data.firmware_major,
data.firmware_minor,
data.firmware_build);

8
libraries/AP_EFI/AP_EFI_State.h
View File

@ -61,10 +61,10 @@ enum class Fuel_Pressure_Status : uint8_t {
};
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
NOT_SUPPORTED = 0,
OK = 1,
BELOW_NOMINAL = 2,
ABOVE_NOMINAL = 3
};
enum class Detonation_Status : uint8_t {