diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 4b02171695..9c8ef679a1 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -32,6 +32,7 @@ #include "AP_RCProtocol_ST24.h" #include "AP_RCProtocol_FPort.h" #include "AP_RCProtocol_FPort2.h" +#include "AP_RCProtocol_DroneCAN.h" #include #include @@ -78,6 +79,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_FPORT_ENABLED backend[AP_RCProtocol::FPORT] = new AP_RCProtocol_FPort(*this, true); #endif +#if AP_RCPROTOCOL_DRONECAN_ENABLED + backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -359,9 +363,6 @@ void AP_RCProtocol::update() bool AP_RCProtocol::new_input() { - bool ret = _new_input; - _new_input = false; - // if we have an extra UART from a SERIALn_PROTOCOL then check it for data check_added_uart(); @@ -371,6 +372,25 @@ bool AP_RCProtocol::new_input() backend[i]->update(); } } + +#if AP_RCPROTOCOL_DRONECAN_ENABLED + uint32_t now = AP_HAL::millis(); + if (should_search(now)) { + if (backend[AP_RCProtocol::DRONECAN] != nullptr && + backend[AP_RCProtocol::DRONECAN]->new_input()) { + _detected_protocol = AP_RCProtocol::DRONECAN; + _last_input_ms = now; + } + } else if (_detected_protocol == AP_RCProtocol::DRONECAN) { + _new_input = backend[AP_RCProtocol::DRONECAN]->new_input(); + if (_new_input) { + _last_input_ms = now; + } + } +#endif + + bool ret = _new_input; + _new_input = false; return ret; } @@ -480,6 +500,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_FPORT2_ENABLED case FPORT2: return "FPORT2"; +#endif +#if AP_RCPROTOCOL_DRONECAN_ENABLED + case DRONECAN: + return "DroneCAN"; #endif case NONE: break; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index ca219f5d85..eeacb6fa0a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -66,6 +66,9 @@ public: #endif #if AP_RCPROTOCOL_FASTSBUS_ENABLED FASTSBUS = 12, +#endif +#if AP_RCPROTOCOL_DRONECAN_ENABLED + DRONECAN = 13, #endif NONE //last enum always is None }; @@ -142,6 +145,9 @@ public: #endif #if AP_RCPROTOCOL_ST24_ENABLED case ST24: +#endif +#if AP_RCPROTOCOL_DRONECAN_ENABLED + case DRONECAN: #endif case NONE: return false; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp new file mode 100644 index 0000000000..879344e323 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp @@ -0,0 +1,103 @@ +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_DRONECAN_ENABLED + +#include +#include +#include +#include "AP_RCProtocol_DroneCAN.h" + +extern const AP_HAL::HAL& hal; + +#define LOG_TAG "RCInput" + +AP_RCProtocol_DroneCAN::Registry AP_RCProtocol_DroneCAN::registry; +AP_RCProtocol_DroneCAN *AP_RCProtocol_DroneCAN::_singleton; + +void AP_RCProtocol_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +{ + if (ap_dronecan == nullptr) { + return; + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rcinput, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("rc_sub"); + } +} + +AP_RCProtocol_DroneCAN* AP_RCProtocol_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id) +{ + if (_singleton == nullptr) { + return nullptr; + } + + if (ap_dronecan == nullptr) { + return nullptr; + } + + for (auto &device : registry.detected_devices) { + if (device.driver == nullptr) { + continue; + } + if (device.ap_dronecan != ap_dronecan) { + continue; + } + if (device.node_id != node_id ) { + continue; + } + return device.driver; + } + + // not found in registry; add it if possible. + for (auto &device : registry.detected_devices) { + if (device.ap_dronecan == nullptr) { + device.ap_dronecan = ap_dronecan; + device.node_id = node_id; + device.driver = _singleton; + return device.driver; + } + } + + return nullptr; +} + +void AP_RCProtocol_DroneCAN::handle_rcinput(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_rc_RCInput &msg) +{ + AP_RCProtocol_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); + if (driver == nullptr) { + return; + } + + auto &rcin = driver->rcin; + WITH_SEMAPHORE(rcin.sem); + rcin.quality = msg.quality; + rcin.status = msg.status; + rcin.num_channels = MIN(msg.rcin.len, ARRAY_SIZE(rcin.channels)); + for (auto i=0; i + +#include + +class AP_RCProtocol_DroneCAN : public AP_RCProtocol_Backend { +public: + + AP_RCProtocol_DroneCAN(AP_RCProtocol &_frontend) : + AP_RCProtocol_Backend(_frontend) { + _singleton = this; + } + + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + + void update() override; + +private: + + static class AP_RCProtocol_DroneCAN *_singleton; + + static void handle_rcinput(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_rc_RCInput &msg); + + static AP_RCProtocol_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id); + + struct { + uint8_t quality; + union { + uint16_t status; + struct { + uint8_t QUALITY_VALID : 1; + uint8_t FAILSAFE : 1; + } bits; + }; + uint8_t num_channels; + uint16_t channels[MAX_RCIN_CHANNELS]; + + uint32_t last_sample_time_ms; + HAL_Semaphore sem; + } rcin; + + // Module Detection Registry + static struct Registry { + struct DetectedDevice { + AP_DroneCAN* ap_dronecan; + uint8_t node_id; + AP_RCProtocol_DroneCAN *driver; + } detected_devices[1]; + HAL_Semaphore sem; + } registry; + + uint32_t last_receive_ms; +}; + + +#endif // AP_RCPROTOCOL_DRONECAN_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 959dae61d5..26a225ad21 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -15,6 +15,10 @@ #define AP_RCPROTOCOL_CRSF_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_RCPROTOCOL_DRONECAN_ENABLED +#define AP_RCPROTOCOL_DRONECAN_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS +#endif + #ifndef AP_RCPROTOCOL_FPORT_ENABLED #define AP_RCPROTOCOL_FPORT_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_FRSKY_SPORT_TELEM_ENABLED #endif