diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp index 0df6d05484..18332295e0 100644 --- a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp @@ -6,16 +6,12 @@ #include #include - -#include +#include 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) @@ -29,16 +25,9 @@ void AP_EFI_DroneCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) if (ap_uavcan == nullptr) { return; } - auto* node = ap_uavcan->get_node(); - uavcan::Subscriber *status_listener; - status_listener = new uavcan::Subscriber(*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; + if (Canard::allocate_sub_arg_callback(ap_uavcan, &trampoline_status, ap_uavcan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); } } @@ -48,86 +37,85 @@ 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) +void AP_EFI_DroneCAN::trampoline_status(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg) { if (driver == nullptr) { return; } - const uavcan::equipment::ice::reciprocating::Status &pkt = *cb.msg; - driver->handle_status(pkt); + driver->handle_status(msg); } /* 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; // 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)) { + 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) { + } 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)) { + 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) { + } 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) { + } 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) { + } 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) { + } 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)) { + 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) { + } 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) { + } 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)) { + 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) { + } 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) { + } 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)) { + 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) { + } 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)) { + 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) { + } 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)) { + 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) { + } 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; @@ -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); // assume max one cylinder status - if (pkt.cylinder_status.size() > 0) { - const auto &cs = pkt.cylinder_status[0]; + if (pkt.cylinder_status.len > 0) { + const auto &cs = pkt.cylinder_status.data[0]; auto &c = istate.cylinder_status; c.ignition_timing_deg = cs.ignition_timing_deg; c.injection_time_ms = cs.injection_time_ms; diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.h b/libraries/AP_EFI/AP_EFI_DroneCAN.h index b84e4be1b0..fc695962be 100644 --- a/libraries/AP_EFI/AP_EFI_DroneCAN.h +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.h @@ -5,9 +5,6 @@ #if AP_EFI_DRONECAN_ENABLED #include -#include - -class EFIStatusCb; class AP_EFI_DroneCAN : public AP_EFI_Backend { public: @@ -16,10 +13,10 @@ public: 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); + static void trampoline_status(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg); private: - void handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt); + void handle_status(const uavcan_equipment_ice_reciprocating_Status &pkt); // singleton for trampoline static AP_EFI_DroneCAN *driver; diff --git a/libraries/AP_EFI/AP_EFI_config.h b/libraries/AP_EFI/AP_EFI_config.h index 29f07638f7..0552d781a3 100644 --- a/libraries/AP_EFI/AP_EFI_config.h +++ b/libraries/AP_EFI/AP_EFI_config.h @@ -16,7 +16,7 @@ #endif #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 #ifndef AP_EFI_NWPWU_ENABLED