Subscriptions get_subscriptions() {
return {
make_handler(&RCIOPlugin::handle_rc_channels_raw),
make_handler(&RCIOPlugin::handle_rc_channels),
make_handler(&RCIOPlugin::handle_servo_output_raw),
};
}
/**
* This function returns message subscriptions.
*
* Each subscription made by PluginBase::make_handler() template.
* Two variations:
* - With automatic decoding and framing error filtering (see handle_heartbeat)
* - Raw message with framig status (see handle_systemtext)
*/
Subscriptions get_subscriptions() {
return {
/* automatic message deduction by second argument */
make_handler(&DummyPlugin::handle_heartbeat),
make_handler(&DummyPlugin::handle_sys_status),
/* handle raw message, check framing! */
make_handler(mavlink::common::msg::STATUSTEXT::MSG_ID, &DummyPlugin::handle_statustext_raw),
make_handler(&DummyPlugin::handle_statustext),
};
}
int KSGDevice::create_handler(KSGDeviceNode*node,ACE_HANDLE* handler)
{
int ret;
ret = make_handler(node,handler);
if(ret == 0)
{
//ret = find_handler(node,&oldhandler);
//if(ret == 0)
//{
// close_handler(node,oldhandler);
//}
save_handler(node,*handler);
return 0;
}
else if(ret > 0)
{
if(node->GetParent() && node->GetParent()->GetDevice())
ret = node->GetParent()->GetDevice()->create_handler(node->GetParent(),handler);
return ret;
}
else
{
return -1;
}
}
Subscriptions get_subscriptions() {
return {
make_handler(&IMUPlugin::handle_attitude),
make_handler(&IMUPlugin::handle_attitude_quaternion),
make_handler(&IMUPlugin::handle_highres_imu),
make_handler(&IMUPlugin::handle_raw_imu),
make_handler(&IMUPlugin::handle_scaled_imu),
make_handler(&IMUPlugin::handle_scaled_pressure),
};
}
void call_abort()
{
auto ptr = shared_from_this();
dispatch(m_io_context, make_handler([ptr] { ptr->abort(); }
, m_abort_handler_storage, *this));
}
int KSGDevice::find_handler(KSGDeviceNode* node,ACE_HANDLE* handler)
{
// 申请资源锁
int ret;
int retries = 3;
int need_wait;
ACE_HANDLE new_handle = ACE_INVALID_HANDLE;
again:
//ACE_DEBUG((LM_DEBUG,"申请连接资源"));
{
ACE_GUARD_RETURN(ACE_Recursive_Thread_Mutex,mon,_handle_mutex,1);
ret = -1;
need_wait = 0;
// 从连接表中查找
DeviceHandleMap::iterator i = _handlers.find(node);
if(i != _handlers.end())
{
// 如果有连接可用, 将连接从[连接表]中移到[已使用连接表]中
*handler = i->second;
_handlers.erase(i);
_used_handlers.insert(DeviceHandleMap::value_type(node,*handler));
//ACE_DEBUG((LM_DEBUG,"获取连接资源"));
return 0;
}
else
{
i = _used_handlers.find(node);
// 如果在[已使用连接表]中找到连接
if(i != _used_handlers.end())
{
need_wait = 1;
}
//ACE_DEBUG((LM_ERROR,"无法找到 CCU 连接"));
}
}
if(need_wait)
{
ACE_DEBUG((LM_DEBUG,"等待连接资源..."));
// 等待其它线程释放资源
if(wait_for_handler(node,handler)==0)
{
ACE_DEBUG((LM_DEBUG,"重新申请连接资源"));
if(--retries > 0)
goto again;
return -1;
}
else
{
// 等待失败
return 1;
}
}
ret = make_handler(node,&new_handle);
if(ret>0)
{
// 本设备节点不能创建连接,查找父设备
//ACE_DEBUG((LM_DEBUG,"查找父设备连接"));
if(node->GetParent() && node->GetParent()->GetDevice())
{
ret = node->GetParent()->GetDevice()->find_handler(node->GetParent(),handler);
}
else
ret = -1;
}
else if(ret == 0)
{
// 创建新连接成功,重新获取连接
ACE_DEBUG((LM_DEBUG,"建立新连接..."));
save_handler(node,new_handle);
node->connect_once(1);
goto again;
}
else
{
// 创建连接失败
ACE_DEBUG((LM_ERROR,"无法连接到设备!!![%d]",node->GetDevId()));
node->connect_once(0);
ret = -1;
}
return ret;
}
Subscriptions get_subscriptions()
{
return {
make_handler(&TaskStatusChangePlugin::handle_mavros_msg),
};
}
Subscriptions get_subscriptions()
{
return {
make_handler(&FixedTargetPositionPlugin::handle_mavros_msg),
};
}
