Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_EFI: replace libuavcan with libcanard based driver

This commit is contained in:
bugobliterator 2023-01-05 11:21:20 +11:00 committed by Andrew Tridgell
parent 2ede296486
commit 1c2a464be0
3 changed files with 30 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_EFI/AP_EFI_DroneCAN.cpp
View File

@ -6,16 +6,12 @@
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <uavcan/equipment/ice/reciprocating/Status.hpp>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_EFI_DroneCAN *AP_EFI_DroneCAN::driver; AP_EFI_DroneCAN *AP_EFI_DroneCAN::driver;
// DroneCAN Frontend Registry Binder
UC_REGISTRY_BINDER(EFIStatusCb, uavcan::equipment::ice::reciprocating::Status);
// constructor // constructor
AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) : AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) :
AP_EFI_Backend(_frontend) AP_EFI_Backend(_frontend)
@ -29,16 +25,9 @@ void AP_EFI_DroneCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
if (ap_uavcan == nullptr) { if (ap_uavcan == nullptr) {
return; return;
} }
auto* node = ap_uavcan->get_node();
uavcan::Subscriber<uavcan::equipment::ice::reciprocating::Status, EFIStatusCb> *status_listener; if (Canard::allocate_sub_arg_callback(ap_uavcan, &trampoline_status, ap_uavcan->get_driver_index()) == nullptr) {
status_listener = new uavcan::Subscriber<uavcan::equipment::ice::reciprocating::Status, EFIStatusCb>(*node); AP_BoardConfig::allocation_error("status_sub");
// 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;
} }
} }
@ -48,86 +37,85 @@ void AP_EFI_DroneCAN::update()
} }
// EFI message handler // EFI message handler
void AP_EFI_DroneCAN::trampoline_status(AP_UAVCAN *ap_uavcan, uint8_t node_id, const EFIStatusCb &cb) void AP_EFI_DroneCAN::trampoline_status(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg)
{ {
if (driver == nullptr) { if (driver == nullptr) {
return; return;
} }
const uavcan::equipment::ice::reciprocating::Status &pkt = *cb.msg; driver->handle_status(msg);
driver->handle_status(pkt);
} }
/* /*
handle reciprocating ICE status message from DroneCAN handle reciprocating ICE status message from DroneCAN
*/ */
void AP_EFI_DroneCAN::handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt) void AP_EFI_DroneCAN::handle_status(const uavcan_equipment_ice_reciprocating_Status &pkt)
{ {
auto &istate = internal_state; auto &istate = internal_state;
// state maps 1:1 from Engine_State // state maps 1:1 from Engine_State
istate.engine_state = Engine_State(pkt.state); istate.engine_state = Engine_State(pkt.state);
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) { if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) {
istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::NOT_SUPPORTED; istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR) {
istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::ERROR; istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::ERROR;
} else { } else {
istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::OK; istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::OK;
} }
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_SUPPORTED)) { if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED)) {
istate.temperature_status = Temperature_Status::NOT_SUPPORTED; istate.temperature_status = Temperature_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_BELOW_NOMINAL) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL) {
istate.temperature_status = Temperature_Status::BELOW_NOMINAL; istate.temperature_status = Temperature_Status::BELOW_NOMINAL;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_ABOVE_NOMINAL) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL) {
istate.temperature_status = Temperature_Status::ABOVE_NOMINAL; istate.temperature_status = Temperature_Status::ABOVE_NOMINAL;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_OVERHEATING) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING) {
istate.temperature_status = Temperature_Status::OVERHEATING; istate.temperature_status = Temperature_Status::OVERHEATING;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) {
istate.temperature_status = Temperature_Status::EGT_ABOVE_NOMINAL; istate.temperature_status = Temperature_Status::EGT_ABOVE_NOMINAL;
} else { } else {
istate.temperature_status = Temperature_Status::OK; istate.temperature_status = Temperature_Status::OK;
} }
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_SUPPORTED)) { if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED)) {
istate.fuel_pressure_status = Fuel_Pressure_Status::NOT_SUPPORTED; istate.fuel_pressure_status = Fuel_Pressure_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_BELOW_NOMINAL) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL) {
istate.fuel_pressure_status = Fuel_Pressure_Status::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) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL) {
istate.fuel_pressure_status = Fuel_Pressure_Status::ABOVE_NOMINAL; istate.fuel_pressure_status = Fuel_Pressure_Status::ABOVE_NOMINAL;
} else { } else {
istate.fuel_pressure_status = Fuel_Pressure_Status::OK; istate.fuel_pressure_status = Fuel_Pressure_Status::OK;
} }
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_SUPPORTED)) { if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED)) {
istate.oil_pressure_status = Oil_Pressure_Status::NOT_SUPPORTED; istate.oil_pressure_status = Oil_Pressure_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_BELOW_NOMINAL) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL) {
istate.oil_pressure_status = Oil_Pressure_Status::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) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL) {
istate.oil_pressure_status = Oil_Pressure_Status::ABOVE_NOMINAL; istate.oil_pressure_status = Oil_Pressure_Status::ABOVE_NOMINAL;
} else { } else {
istate.oil_pressure_status = Oil_Pressure_Status::OK; istate.oil_pressure_status = Oil_Pressure_Status::OK;
} }
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_SUPPORTED)) { if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED)) {
istate.detonation_status = Detonation_Status::NOT_SUPPORTED; istate.detonation_status = Detonation_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_OBSERVED) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED) {
istate.detonation_status = Detonation_Status::OBSERVED; istate.detonation_status = Detonation_Status::OBSERVED;
} else { } else {
istate.detonation_status = Detonation_Status::NOT_OBSERVED; istate.detonation_status = Detonation_Status::NOT_OBSERVED;
} }
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_SUPPORTED)) { if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED)) {
istate.misfire_status = Misfire_Status::NOT_SUPPORTED; istate.misfire_status = Misfire_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_OBSERVED) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED) {
istate.misfire_status = Misfire_Status::OBSERVED; istate.misfire_status = Misfire_Status::OBSERVED;
} else { } else {
istate.misfire_status = Misfire_Status::NOT_OBSERVED; istate.misfire_status = Misfire_Status::NOT_OBSERVED;
} }
if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_SUPPORTED)) { if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED)) {
istate.debris_status = Debris_Status::NOT_SUPPORTED; istate.debris_status = Debris_Status::NOT_SUPPORTED;
} else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_DETECTED) { } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED) {
istate.debris_status = Debris_Status::DETECTED; istate.debris_status = Debris_Status::DETECTED;
} else { } else {
istate.debris_status = Debris_Status::NOT_DETECTED; istate.debris_status = Debris_Status::NOT_DETECTED;
@ -152,8 +140,8 @@ void AP_EFI_DroneCAN::handle_status(const uavcan::equipment::ice::reciprocating:
istate.spark_plug_usage = Spark_Plug_Usage(pkt.spark_plug_usage); istate.spark_plug_usage = Spark_Plug_Usage(pkt.spark_plug_usage);
// assume max one cylinder status // assume max one cylinder status
if (pkt.cylinder_status.size() > 0) { if (pkt.cylinder_status.len > 0) {
const auto &cs = pkt.cylinder_status[0]; const auto &cs = pkt.cylinder_status.data[0];
auto &c = istate.cylinder_status; auto &c = istate.cylinder_status;
c.ignition_timing_deg = cs.ignition_timing_deg; c.ignition_timing_deg = cs.ignition_timing_deg;
c.injection_time_ms = cs.injection_time_ms; c.injection_time_ms = cs.injection_time_ms;

7
libraries/AP_EFI/AP_EFI_DroneCAN.h
View File

@ -5,9 +5,6 @@
#if AP_EFI_DRONECAN_ENABLED #if AP_EFI_DRONECAN_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/equipment/ice/reciprocating/Status.hpp>
class EFIStatusCb;
class AP_EFI_DroneCAN : public AP_EFI_Backend { class AP_EFI_DroneCAN : public AP_EFI_Backend {
public: public:
@ -16,10 +13,10 @@ public:
void update() override; void update() override;
static void subscribe_msgs(AP_UAVCAN* ap_uavcan); static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static void trampoline_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const EFIStatusCb &cb); static void trampoline_status(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg);
private: private:
void handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt); void handle_status(const uavcan_equipment_ice_reciprocating_Status &pkt);
// singleton for trampoline // singleton for trampoline
static AP_EFI_DroneCAN *driver; static AP_EFI_DroneCAN *driver;

2
libraries/AP_EFI/AP_EFI_config.h
View File

@ -16,7 +16,7 @@
#endif #endif
#ifndef AP_EFI_DRONECAN_ENABLED #ifndef AP_EFI_DRONECAN_ENABLED
#define AP_EFI_DRONECAN_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED && HAL_MAX_CAN_PROTOCOL_DRIVERS && HAL_CANMANAGER_ENABLED #define AP_EFI_DRONECAN_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_LIBUAVCAN_DRIVERS
#endif #endif
#ifndef AP_EFI_NWPWU_ENABLED #ifndef AP_EFI_NWPWU_ENABLED