#include "AP_RCProtocol_config.h" #if AP_RCPROTOCOL_DRONECAN_ENABLED #include <AP_CANManager/AP_CANManager.h> #include <AP_DroneCAN/AP_DroneCAN.h> #include <AP_BoardConfig/AP_BoardConfig.h> #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<rcin.num_channels; i++) { rcin.channels[i] = msg.rcin.data[i]; } rcin.last_sample_time_ms = AP_HAL::millis(); } void AP_RCProtocol_DroneCAN::update() { { WITH_SEMAPHORE(rcin.sem); if (rcin.last_sample_time_ms == last_receive_ms) { // no new data return; } last_receive_ms = rcin.last_sample_time_ms; add_input( rcin.num_channels, rcin.channels, rcin.bits.FAILSAFE, rcin.bits.QUALITY_VALID ? rcin.quality : 0, // CHECK ME 0 // link quality ); } } #endif // AP_RCPROTOCOL_DRONECAN_ENABLED