zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_EFI: added DroneCAN EFI driver

apm_2208
Andrew Tridgell 3 years ago
parent
465dbd89f4
commit
9ddd1afb34
8 changed files with 223 additions and 7 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      libraries/AP_EFI/AP_EFI.cpp
  2. 1
      libraries/AP_EFI/AP_EFI.h
  3. 6
      libraries/AP_EFI/AP_EFI_Backend.cpp
  4. 2
      libraries/AP_EFI/AP_EFI_Backend.h
  5. 169
      libraries/AP_EFI/AP_EFI_DroneCAN.cpp
  6. 32
      libraries/AP_EFI/AP_EFI_DroneCAN.h
  7. 4
      libraries/AP_EFI/AP_EFI_NWPMU.cpp
  8. 8
      libraries/AP_EFI/AP_EFI_State.h

8
libraries/AP_EFI/AP_EFI.cpp
Unescape View File

@ -20,6 +20,7 @@ @@ -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[] = { @@ -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) @@ -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
Unescape View File

@ -82,6 +82,7 @@ public: @@ -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
Unescape View File

@ -41,4 +41,10 @@ float AP_EFI_Backend::get_coef2(void) const @@ -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
Unescape View File

@ -42,6 +42,8 @@ protected: @@ -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
Unescape View File

@ -0,0 +1,169 @@ @@ -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
Unescape View File

@ -0,0 +1,32 @@ @@ -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
Unescape View File

@ -36,7 +36,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) @@ -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) @@ -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
Unescape View File

@ -61,10 +61,10 @@ enum class Fuel_Pressure_Status : uint8_t { @@ -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 {

Write Preview
Loading…
Cancel
Save