mirror of https://github.com/ArduPilot/ardupilot
104 lines
2.7 KiB
C++
104 lines
2.7 KiB
C++
#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
|