894 lines
30 KiB
C++
894 lines
30 KiB
C++
#include "profinet.hpp"
|
||
|
||
#include "profinet_cb_state_ind.hpp"
|
||
#include "profinet_cb_connect_ind.hpp"
|
||
#include "profinet_cb_release_ind.hpp"
|
||
#include "profinet_cb_dcontrol_ind.hpp"
|
||
#include "profinet_cb_ccontrol_ind.hpp"
|
||
#include "profinet_cb_read_ind.hpp"
|
||
#include "profinet_cb_write_ind.hpp"
|
||
#include "profinet_cb_exp_module_ind.hpp"
|
||
#include "profinet_cb_exp_submodule_ind.hpp"
|
||
#include "profinet_cb_new_data_status_ind.hpp"
|
||
#include "profinet_cb_alarm_ind.hpp"
|
||
#include "profinet_cb_alarm_cnf.hpp"
|
||
#include "profinet_cb_alarm_ack_cnf.hpp"
|
||
#include "profinet_cb_reset_ind.hpp"
|
||
#include "profinet_cb_signal_led_ind.hpp"
|
||
|
||
#include "utils.h"
|
||
#include <cstring>
|
||
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
#include <iostream>
|
||
|
||
#include <chrono>
|
||
#include <thread>
|
||
|
||
|
||
using namespace std;
|
||
|
||
|
||
#define GET_HIGH_BYTE(id) ((id >> 8) & 0xFF)
|
||
#define GET_LOW_BYTE(id) (id & 0xFF)
|
||
|
||
#define APP_MAIN_SLEEPTIME_US 5000 * 1000
|
||
#define APP_SNMP_THREAD_PRIORITY 1
|
||
#define APP_SNMP_THREAD_STACKSIZE 256 * 1024 /* bytes */
|
||
#define APP_ETH_THREAD_PRIORITY 10
|
||
#define APP_ETH_THREAD_STACKSIZE 4096 /* bytes */
|
||
#define APP_BG_WORKER_THREAD_PRIORITY 5
|
||
#define APP_BG_WORKER_THREAD_STACKSIZE 4096 /* bytes */
|
||
|
||
static void pnetConfigure(pnet_cfg_t& cfg, ProfinetSettings& Settings, ProfinetDeviceSettings& DevSettings)
|
||
{
|
||
cfg.tick_us = Settings.ticks_us;
|
||
cfg.im_0_data.im_vendor_id_hi = GET_HIGH_BYTE(DevSettings.im_0.vendor_id);
|
||
cfg.im_0_data.im_vendor_id_lo = GET_LOW_BYTE (DevSettings.im_0.vendor_id);
|
||
cfg.im_0_data.im_hardware_revision = DevSettings.im_0.hw_revision;
|
||
cfg.im_0_data.im_sw_revision_prefix = DevSettings.im_0.sw_revision.prefix;
|
||
cfg.im_0_data.im_sw_revision_functional_enhancement = DevSettings.im_0.sw_revision.functional_enhancement;
|
||
cfg.im_0_data.im_sw_revision_bug_fix = DevSettings.im_0.sw_revision.bug_fix;
|
||
cfg.im_0_data.im_sw_revision_internal_change = DevSettings.im_0.sw_revision.internal_change;
|
||
cfg.im_0_data.im_revision_counter = DevSettings.im_0.revision_counter;
|
||
cfg.im_0_data.im_profile_id = DevSettings.im_0.profile_id;
|
||
cfg.im_0_data.im_profile_specific_type = DevSettings.im_0.profile_specific_type;
|
||
cfg.im_0_data.im_version_major = 1;
|
||
cfg.im_0_data.im_version_minor = 1;
|
||
cfg.im_0_data.im_supported = DevSettings.im_0.supported;
|
||
|
||
std::snprintf (
|
||
cfg.im_0_data.im_order_id,
|
||
sizeof (cfg.im_0_data.im_order_id),
|
||
"%s",
|
||
DevSettings.im_0.order_id.c_str());
|
||
std::snprintf (
|
||
cfg.im_0_data.im_serial_number,
|
||
sizeof (cfg.im_0_data.im_serial_number),
|
||
"%s",
|
||
DevSettings.im_0.serial_number.c_str());
|
||
std::snprintf (
|
||
cfg.im_1_data.im_tag_function,
|
||
sizeof (cfg.im_1_data.im_tag_function),
|
||
"%s",
|
||
DevSettings.im_1.tag_function.c_str());
|
||
std::snprintf (
|
||
cfg.im_1_data.im_tag_location,
|
||
sizeof (cfg.im_1_data.im_tag_location),
|
||
"%s",
|
||
DevSettings.im_1.tag_location.c_str());
|
||
std::snprintf (
|
||
cfg.im_2_data.im_date,
|
||
sizeof (cfg.im_2_data.im_date),
|
||
"%s",
|
||
DevSettings.im_2.date.c_str());
|
||
std::snprintf (
|
||
cfg.im_3_data.im_descriptor,
|
||
sizeof (cfg.im_3_data.im_descriptor),
|
||
"%s",
|
||
DevSettings.im_3.descriptor.c_str());
|
||
std::snprintf (
|
||
cfg.im_4_data.im_signature,
|
||
sizeof (cfg.im_4_data.im_signature),
|
||
"%s",
|
||
DevSettings.im_4.signature.c_str());
|
||
|
||
cfg.state_cb = profinet_cb_state_ind;
|
||
cfg.connect_cb = profinet_cb_connect_ind;
|
||
cfg.release_cb = profinet_cb_release_ind;
|
||
cfg.dcontrol_cb = profinet_cb_dcontrol_ind;
|
||
cfg.ccontrol_cb = profinet_cb_ccontrol_ind;
|
||
cfg.read_cb = profinet_cb_read_ind;
|
||
cfg.write_cb = profinet_cb_write_ind;
|
||
cfg.exp_module_cb = profinet_cb_exp_module_ind;
|
||
cfg.exp_submodule_cb = profinet_cb_exp_submodule_ind;
|
||
cfg.new_data_status_cb = profinet_cb_new_data_status_ind;
|
||
cfg.alarm_ind_cb = profinet_cb_alarm_ind;
|
||
cfg.alarm_cnf_cb = profinet_cb_alarm_cnf;
|
||
cfg.alarm_ack_cnf_cb = profinet_cb_alarm_ack_cnf;
|
||
cfg.reset_cb = profinet_cb_reset_ind;
|
||
cfg.signal_led_cb = profinet_cb_signal_led_ind;
|
||
|
||
cfg.device_id.vendor_id_hi = GET_HIGH_BYTE (DevSettings.im_0.vendor_id);
|
||
cfg.device_id.vendor_id_lo = GET_LOW_BYTE (DevSettings.im_0.vendor_id);
|
||
cfg.device_id.device_id_hi = GET_HIGH_BYTE (DevSettings.device_id);
|
||
cfg.device_id.device_id_lo = GET_LOW_BYTE (DevSettings.device_id);
|
||
cfg.oem_device_id.vendor_id_hi = GET_HIGH_BYTE (DevSettings.oem_vendor_id);
|
||
cfg.oem_device_id.vendor_id_lo = GET_LOW_BYTE (DevSettings.oem_vendor_id);
|
||
cfg.oem_device_id.device_id_hi = GET_HIGH_BYTE (DevSettings.oem_device_id);
|
||
cfg.oem_device_id.device_id_lo = GET_LOW_BYTE (DevSettings.oem_device_id);
|
||
|
||
|
||
snprintf (
|
||
cfg.product_name,
|
||
sizeof (cfg.product_name),
|
||
"%s",
|
||
DevSettings.product_name.c_str());
|
||
|
||
cfg.send_hello = true;
|
||
|
||
/* Timing */
|
||
cfg.min_device_interval = DevSettings.min_device_interval;
|
||
|
||
/* Should be set by application as part of network configuration. */
|
||
cfg.num_physical_ports = 1;
|
||
|
||
snprintf (
|
||
cfg.station_name,
|
||
sizeof (cfg.station_name),
|
||
"%s",
|
||
DevSettings.station_name.c_str());
|
||
|
||
/* Diagnosis mechanism */
|
||
/* We prefer using "Extended channel diagnosis" instead of
|
||
* "Qualified channel diagnosis" format on the wire,
|
||
* as this is better supported by Wireshark.
|
||
*/
|
||
cfg.use_qualified_diagnosis = false;
|
||
}
|
||
|
||
Profinet::Profinet() : m_pnet_data({nullptr, 0, UINT32_MAX, 0}),
|
||
m_cyclic_io_cnt{0},
|
||
m_periodic_stop{false}
|
||
{
|
||
}
|
||
|
||
bool Profinet::Config(ProfinetSettings& Settings, ProfinetDeviceSettings& DevSettings)
|
||
{
|
||
bool ret = true;
|
||
pnet_cfg_t pnet_cfg = {0};
|
||
utils_netif_namelist_t netif_name_list;
|
||
uint16_t number_of_ports = 1;
|
||
pnet_if_cfg_t netif_cfg = {0};
|
||
|
||
std::memset(&pnet_cfg, 0, sizeof(pnet_cfg));
|
||
|
||
pnetConfigure(pnet_cfg, Settings, DevSettings);
|
||
|
||
m_run_period_us = Settings.ticks_us;
|
||
m_cyclic_io_period_ms = Settings.cyclic_ms;
|
||
|
||
|
||
|
||
///TODO Определить что передавать в cb_cfg
|
||
pnet_cfg.cb_arg = static_cast<void*>(this);
|
||
|
||
utils_pnet_cfg_init_netifs(Settings.EthIface.c_str(),
|
||
&netif_name_list,
|
||
&number_of_ports,
|
||
&netif_cfg,
|
||
0x10 /* Copper 100 Mbit/s Full duplex */
|
||
);
|
||
|
||
pnet_cfg.if_cfg = netif_cfg;
|
||
pnet_cfg.num_physical_ports = number_of_ports;
|
||
|
||
pnet_cfg.pnal_cfg.snmp_thread.prio = APP_SNMP_THREAD_PRIORITY;
|
||
pnet_cfg.pnal_cfg.snmp_thread.stack_size = APP_SNMP_THREAD_STACKSIZE;
|
||
pnet_cfg.pnal_cfg.eth_recv_thread.prio = APP_ETH_THREAD_PRIORITY;
|
||
pnet_cfg.pnal_cfg.eth_recv_thread.stack_size = APP_ETH_THREAD_STACKSIZE;
|
||
pnet_cfg.pnal_cfg.bg_worker_thread.prio = APP_BG_WORKER_THREAD_PRIORITY;
|
||
pnet_cfg.pnal_cfg.bg_worker_thread.stack_size = APP_BG_WORKER_THREAD_STACKSIZE;
|
||
|
||
/// Инициализация библиотеки pnet
|
||
m_pnet_data.pnet_ptr = pnet_init (&pnet_cfg);
|
||
|
||
if (m_pnet_data.pnet_ptr == nullptr)
|
||
return false;
|
||
|
||
///
|
||
pnet_data_cfg_t dap_data_cfg = { PNET_DIR_NO_IO, 0, 0 };
|
||
/// Создаем обязательный модуль DAP
|
||
auto dap_module = make_shared<ProfinetModule>(PNET_MOD_DAP_IDENT, "DAP 1");
|
||
/// Создаем обязательный подмодуль DAP Identity 1
|
||
auto dap_submodule_indent_1 = make_shared<ProfinetSubmodule>(PNET_SUBMOD_DAP_IDENT, "DAP Identity 1", dap_data_cfg, nullptr, nullptr);
|
||
/// Создаем обязательный подмодуль DAP IFACE1 IDENT
|
||
auto dap_submodule_iface_1_indent_1 = make_shared<ProfinetSubmodule>(PNET_SUBMOD_DAP_INTERFACE_1_IDENT, "DAP IFACE1 IDENT", dap_data_cfg, nullptr, nullptr);
|
||
/// Создаем обязательный подмодуль DAP Port 1
|
||
auto dap_subslot_iface_1_port1_ident = make_shared<ProfinetSubmodule>(PNET_SUBMOD_DAP_INTERFACE_1_PORT_1_IDENT, "DAP Port 1", dap_data_cfg, nullptr, nullptr);
|
||
/// Добавляем подмодули к модулю
|
||
ret&= dap_module->addSubmodule(dap_submodule_indent_1);
|
||
ret&= dap_module->addSubmodule(dap_submodule_iface_1_indent_1);
|
||
ret&= dap_module->addSubmodule(dap_subslot_iface_1_port1_ident);
|
||
|
||
/// Создаем слот для модуля DAP и доключаем к нему модуль dap_module
|
||
ret&= addSlotAndPlugModule(PNET_SLOT_DAP_IDENT, dap_module);
|
||
/// Добавляем подслоты к слоту PNET_SLOT_DAP_IDENT и подключаем к подслотам подмодули
|
||
/// 1. Подмодуль "DAP Identity 1"
|
||
ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_IDENT, dap_submodule_indent_1);
|
||
/// 2. Подмодуль "DAP IFACE1 IDENT"
|
||
ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_INTERFACE_1_IDENT, dap_submodule_iface_1_indent_1);
|
||
/// 3. Подмодуль "DAP Port 1"
|
||
ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_INTERFACE_1_PORT_1_IDENT, dap_subslot_iface_1_port1_ident);
|
||
|
||
return ret;
|
||
}
|
||
|
||
bool Profinet::addSlot(std::shared_ptr<ProfinetSlot>& slot_ptr)
|
||
{
|
||
if (slot_ptr == nullptr)
|
||
return false;
|
||
|
||
m_slots[slot_ptr->m_slot_nbr] = slot_ptr;
|
||
|
||
return true;
|
||
}
|
||
|
||
std::shared_ptr<ProfinetSlot> Profinet::addSlot(uint16_t slot_nbr)
|
||
{
|
||
std::shared_ptr<ProfinetSlot> slot_ptr(nullptr);
|
||
|
||
if (m_slots.count(slot_nbr))
|
||
{
|
||
slot_ptr = m_slots[slot_nbr];
|
||
}
|
||
else
|
||
{
|
||
slot_ptr = make_shared<ProfinetSlot>(slot_nbr);
|
||
}
|
||
|
||
if (slot_ptr != nullptr)
|
||
{
|
||
m_slots[slot_nbr] = slot_ptr;
|
||
}
|
||
|
||
return slot_ptr;
|
||
}
|
||
|
||
bool Profinet::addModule(std::shared_ptr<ProfinetModule>& module_ptr)
|
||
{
|
||
if (module_ptr == nullptr)
|
||
return false;
|
||
|
||
m_modules[module_ptr->m_id] = module_ptr;
|
||
|
||
return true;
|
||
}
|
||
|
||
std::shared_ptr<ProfinetModule> Profinet::getModule(uint32_t module_id)
|
||
{
|
||
if (!m_modules.count(module_id))
|
||
{
|
||
return nullptr;
|
||
}
|
||
|
||
return m_modules[module_id];
|
||
}
|
||
|
||
|
||
bool Profinet::addSlotAndPlugModule(uint16_t slot_nbr, std::shared_ptr<ProfinetModule>& module_ptr)
|
||
{
|
||
auto slot = addSlot(slot_nbr);
|
||
|
||
if (slot == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return slot->plugModule(m_pnet_data, module_ptr);
|
||
}
|
||
|
||
bool Profinet::addSlotAndPlugModule(uint16_t slot_nbr, uint32_t module_id)
|
||
{
|
||
/// 1. Проверяем, что module_id поддерживается
|
||
auto module_ptr = getModule(module_id);
|
||
|
||
if (module_ptr == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
///2. Добавляет слот
|
||
auto slot = addSlot(slot_nbr);
|
||
|
||
if (slot == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
/// 3. Подключаем модуль к слоту
|
||
return slot->plugModule(m_pnet_data, module_ptr);
|
||
}
|
||
|
||
|
||
bool Profinet::addSubslotAndPlugSubmodule(uint16_t slot_nbr, uint16_t subslot_nbr, std::shared_ptr<ProfinetSubmodule>& submodule_ptr)
|
||
{
|
||
auto slot_ptr = getSlotPtr(slot_nbr);
|
||
|
||
if (slot_ptr == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
auto subslot_ptr = slot_ptr->addSubslot(subslot_nbr);
|
||
|
||
if (subslot_ptr == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return subslot_ptr->plugSubmodule(m_pnet_data, submodule_ptr);
|
||
}
|
||
|
||
bool Profinet::addSubslotAndPlugSubmodule(uint16_t slot_nbr,
|
||
uint16_t subslot_nbr,
|
||
uint32_t module_id,
|
||
uint32_t submodule_id,
|
||
const pnet_data_cfg_t * p_exp_data)
|
||
{
|
||
///1. Проверяем наличие слота
|
||
auto slot_ptr = getSlotPtr(slot_nbr);
|
||
|
||
if (slot_ptr == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
///2. Проверяем, что к слоту подключен модуль с идентификатором module_id
|
||
auto module_ptr = slot_ptr->getModulePtr();
|
||
if (module_ptr == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (module_ptr->m_id != module_id)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
///3. Проверяем, что в модуль входит подмодуль с идентфикатором submodule_id
|
||
auto submodule_ptr = module_ptr->getSubmodulePtr(submodule_id);
|
||
if (submodule_ptr == nullptr)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
///4. Проверяем, что конфигурация данных совпадает
|
||
if ((submodule_ptr->m_data_cfg.data_dir != p_exp_data->data_dir) ||
|
||
(submodule_ptr->m_data_cfg.insize != p_exp_data->insize) ||
|
||
(submodule_ptr->m_data_cfg.outsize != p_exp_data->outsize))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return addSubslotAndPlugSubmodule(slot_nbr, subslot_nbr, submodule_ptr);
|
||
}
|
||
|
||
|
||
std::shared_ptr<ProfinetSlot> Profinet::getSlotPtr(uint16_t slot_nbr)
|
||
{
|
||
/// 1. Проверить, что слот slot добавлен в конфигурацию
|
||
if (!m_slots.count(slot_nbr))
|
||
{
|
||
return nullptr;
|
||
}
|
||
|
||
return m_slots[slot_nbr];
|
||
}
|
||
|
||
std::shared_ptr<ProfinetSubslot> Profinet::getSubslotPtr(uint16_t slot_nbr, uint16_t subslot_nbr)
|
||
{
|
||
auto slot_ptr = getSlotPtr(slot_nbr);
|
||
|
||
if (slot_ptr == nullptr)
|
||
{
|
||
return nullptr;
|
||
}
|
||
|
||
return slot_ptr->getSubslotPtr(subslot_nbr);
|
||
}
|
||
|
||
std::shared_ptr<ProfinetParameter> Profinet::getSubmoduleParameter(uint16_t slot_nbr, uint16_t subslot_nbr, uint32_t param_index)
|
||
{
|
||
///1. Проверяем что существует подслот \a subslot_nbr входящий к \a slot_nbr
|
||
auto subslot_ptr = getSubslotPtr(slot_nbr, subslot_nbr);
|
||
|
||
if (subslot_ptr == nullptr)
|
||
{
|
||
return nullptr;
|
||
}
|
||
|
||
///2. Проверяем что к подслоту \a subslot_nbr подключен подмодуль
|
||
auto submodule_ptr = subslot_ptr->getSubmodulePtr();
|
||
|
||
if (submodule_ptr == nullptr)
|
||
{
|
||
return nullptr;
|
||
}
|
||
///3. Проверяем, что у подмодуля есть параметр с индексом idx
|
||
auto param_ptr = submodule_ptr->getParameterPtr(param_index);
|
||
|
||
if (param_ptr == nullptr)
|
||
{
|
||
return nullptr;
|
||
}
|
||
|
||
return param_ptr;
|
||
}
|
||
|
||
void Profinet::initDataAndIoxs()
|
||
{
|
||
uint8_t indata_iops;
|
||
/**
|
||
* Итерируемся по всем слотам.
|
||
* Если слоты есть, то и модули к ним уже подключены, т.к. это делает контроллер.
|
||
*/
|
||
for (auto slot : m_slots)
|
||
{
|
||
shared_ptr<ProfinetSlot>& slot_ptr = slot.second;
|
||
|
||
auto& subslots = slot_ptr->getSubslotsPtr();
|
||
/// Итерируемся по всем подслотам
|
||
for (auto subslot : subslots)
|
||
{
|
||
shared_ptr<ProfinetSubslot>& subslot_ptr = subslot.second;
|
||
|
||
auto submodule_ptr = subslot_ptr->getSubmodulePtr();
|
||
|
||
/// На всякий случай. Вообще если подслоты есть, то и подмодули уже к ним подключены.
|
||
if (submodule_ptr == nullptr)
|
||
continue;
|
||
|
||
indata_iops = PNET_IOXS_BAD;
|
||
/**
|
||
* @brief Если для подмодуля существуют входные данные (данные от устройства к контроллеру) или
|
||
* циклических данных нет(DAP), то нужно установить статус IOPS.
|
||
* submodule_ptr->m_data_cfg.data_dir == PNET_DIR_NO_IO - вот это условие нужно для установки статуса для DAP
|
||
*/
|
||
if ( submodule_ptr->m_data_cfg.insize > 0 ||
|
||
submodule_ptr->m_data_cfg.data_dir == PNET_DIR_NO_IO )
|
||
{
|
||
/// Для данных DAP статус всегда ОК
|
||
if (slot_ptr->m_slot_nbr == PNET_SLOT_DAP_IDENT)
|
||
{
|
||
/// Для данных DAP статус ОК
|
||
indata_iops = PNET_IOXS_GOOD;
|
||
}
|
||
else if (submodule_ptr->m_data_cfg.insize > 0)
|
||
{
|
||
/// Проверка, что данные есть
|
||
if (submodule_ptr->m_inp_data_ptr != nullptr)
|
||
{
|
||
indata_iops = PNET_IOXS_GOOD;
|
||
}
|
||
}
|
||
|
||
int ret = pnet_input_set_data_and_iops ( m_pnet_data.pnet_ptr,
|
||
m_pnet_data.api,
|
||
slot_ptr->m_slot_nbr,
|
||
subslot_ptr->m_subslot_nbr,
|
||
(uint8_t*)submodule_ptr->m_inp_data_ptr, /// Указатель на данные
|
||
submodule_ptr->m_data_cfg.insize, /// Размер данных
|
||
indata_iops);
|
||
|
||
/*
|
||
* If a submodule is still plugged but not used in current AR,
|
||
* setting the data and IOPS will fail.
|
||
* This is not a problem.
|
||
* Log message below will only be printed for active submodules.
|
||
*/
|
||
if (ret == 0)
|
||
{
|
||
/// Успешно
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief Если данные подмодуля включаются в себя выходные данные(данные от контроллера к устройству)
|
||
* То нужно установить для них статус IOCS в PNET_IOXS_GOOD.
|
||
*
|
||
*/
|
||
if (submodule_ptr->m_data_cfg.outsize > 0)
|
||
{
|
||
int ret = pnet_output_set_iocs ( m_pnet_data.pnet_ptr,
|
||
m_pnet_data.api,
|
||
slot_ptr->m_slot_nbr,
|
||
subslot_ptr->m_subslot_nbr,
|
||
PNET_IOXS_GOOD);
|
||
|
||
if (ret == 0)
|
||
{
|
||
/// Успешно
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void Profinet::PeriodicOperations()
|
||
{
|
||
while(!m_periodic_stop.load(std::memory_order_relaxed))
|
||
{
|
||
/// Ждем время в мкс: ticks_period_us, по умолчанию 1мс
|
||
std::this_thread::sleep_for(std::chrono::microseconds(m_run_period_us));
|
||
|
||
/// Вызов cyclic io
|
||
if (++m_cyclic_io_cnt >= m_cyclic_io_period_ms)
|
||
{
|
||
m_cyclic_io_cnt = 0;
|
||
cyclicIoData();
|
||
}
|
||
|
||
/// запускаем pnet
|
||
pnet_handle_periodic (m_pnet_data.pnet_ptr);
|
||
}
|
||
}
|
||
|
||
void Profinet::cyclicIoData()
|
||
{
|
||
for (auto slot : m_slots)
|
||
{
|
||
shared_ptr<ProfinetSlot>& slot_ptr = slot.second;
|
||
|
||
/// DAP слот игнорируем при циклическом обмене
|
||
if (slot_ptr->m_slot_nbr == PNET_SLOT_DAP_IDENT)
|
||
continue;
|
||
|
||
auto& subslots = slot_ptr->getSubslotsPtr();
|
||
/// Итерируемся по всем подслотам
|
||
for (auto subslot : subslots)
|
||
{
|
||
shared_ptr<ProfinetSubslot>& subslot_ptr = subslot.second;
|
||
|
||
auto submodule_ptr = subslot_ptr->getSubmodulePtr();
|
||
|
||
/// На всякий случай. Вообще если подслоты есть, то и подмодули уже к ним подключены.
|
||
if (submodule_ptr == nullptr)
|
||
continue;
|
||
|
||
if (submodule_ptr->m_data_cfg.outsize > 0)
|
||
{
|
||
bool outdata_updated;
|
||
uint16_t outdata_length = submodule_ptr->m_data_cfg.outsize;
|
||
uint8_t outdata_iops;
|
||
|
||
/// Копируем данные от контроллера в m_out_data_ptr
|
||
(void)pnet_output_get_data_and_iops (
|
||
m_pnet_data.pnet_ptr,
|
||
m_pnet_data.api,
|
||
slot_ptr->m_slot_nbr,
|
||
subslot_ptr->m_subslot_nbr,
|
||
&outdata_updated,
|
||
(uint8_t*)submodule_ptr->m_out_data_ptr,
|
||
&outdata_length,
|
||
&outdata_iops);
|
||
|
||
submodule_ptr->m_outdata_iops = outdata_iops;
|
||
|
||
if (submodule_ptr->m_data_cfg.outsize != outdata_length)
|
||
{
|
||
/// Неправильная длина данных
|
||
}
|
||
else if (outdata_iops == PNET_IOXS_BAD)
|
||
{
|
||
/// Что-то не так с данными от контроллера
|
||
}
|
||
}
|
||
|
||
if (submodule_ptr->m_data_cfg.insize > 0)
|
||
{
|
||
uint8_t indata_iocs;
|
||
uint8_t indata_iops = PNET_IOXS_GOOD;
|
||
/* Send input data to the PLC */
|
||
(void)pnet_input_set_data_and_iops (
|
||
m_pnet_data.pnet_ptr,
|
||
m_pnet_data.api,
|
||
slot_ptr->m_slot_nbr,
|
||
subslot_ptr->m_subslot_nbr,
|
||
(uint8_t*)submodule_ptr->m_inp_data_ptr,
|
||
submodule_ptr->m_data_cfg.insize,
|
||
indata_iops);
|
||
|
||
(void)pnet_input_get_iocs (
|
||
m_pnet_data.pnet_ptr,
|
||
m_pnet_data.api,
|
||
slot_ptr->m_slot_nbr,
|
||
subslot_ptr->m_subslot_nbr,
|
||
&indata_iocs);
|
||
|
||
submodule_ptr->m_indata_iocs = indata_iocs;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void Profinet::Start()
|
||
{
|
||
m_periodic_thread = std::move(std::thread(&Profinet::PeriodicOperations, this));
|
||
}
|
||
|
||
Profinet::~Profinet()
|
||
{
|
||
///Остановка потока
|
||
m_periodic_stop.store(true, std::memory_order_relaxed);
|
||
|
||
if (m_periodic_thread.joinable())
|
||
{
|
||
m_periodic_thread.join();
|
||
}
|
||
}
|
||
|
||
|
||
/**\
|
||
* =========================================================================================
|
||
* Callbacks
|
||
* =========================================================================================
|
||
*/
|
||
int Profinet::callbackStateInd ( uint32_t arep, pnet_event_values_t event)
|
||
{
|
||
uint16_t err_cls = 0; /* Error code 1 */
|
||
uint16_t err_code = 0; /* Error code 2 */
|
||
const char * error_class_description = "";
|
||
const char * error_code_description = "";
|
||
|
||
if (event == PNET_EVENT_ABORT)
|
||
{
|
||
if (pnet_get_ar_error_codes (m_pnet_data.pnet_ptr, arep, &err_cls, &err_code) == 0)
|
||
{
|
||
/**
|
||
* @brief Коды ошибок в err_cls и err_code
|
||
* TODO: Нужно передавать в управляющее приложение
|
||
*/
|
||
}
|
||
else
|
||
{
|
||
/// Ошибок нет
|
||
}
|
||
|
||
|
||
/* Only abort AR with correct session key */
|
||
///os_event_set (app->main_events, APP_EVENT_ABORT);
|
||
}
|
||
else if (event == PNET_EVENT_PRMEND)
|
||
{
|
||
if (isConnectedToController())
|
||
{
|
||
/// Если уже были подключены и пришел запрос на еще одно подключение
|
||
}
|
||
|
||
m_pnet_data.arep = arep;
|
||
|
||
initDataAndIoxs();
|
||
|
||
(void)pnet_set_provider_state (m_pnet_data.pnet_ptr, true);
|
||
/**
|
||
* @brief Костыль библиотеки pnet:
|
||
* По правилам Profinet нужно ответить Application ready.
|
||
* Однако тут нельзя вызвать pnet_application_ready, т.к. это испортит
|
||
* внутреннее состояние стэка pnet.
|
||
* Это нужно делать на следующем "тике".
|
||
* См. описание pnet_application_ready:
|
||
* This function must be called after the application has received the pnet_state_ind()
|
||
* user callback with PNET_EVENT_PRMEND, in order for a connection to be established.
|
||
*
|
||
*/
|
||
/* Send application ready at next tick
|
||
Do not call pnet_application_ready() here as it will affect
|
||
the internal stack states */
|
||
m_pnet_data.arep_for_appl_ready = arep;
|
||
|
||
///os_event_set (app->main_events, APP_EVENT_READY_FOR_DATA); ///
|
||
}
|
||
else if (event == PNET_EVENT_DATA)
|
||
{
|
||
/// Стартовал обмен cyclic io
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackConnectInd ( uint32_t arep, pnet_result_t * p_result)
|
||
{
|
||
/**
|
||
* @brief
|
||
* Если будет возвращено 0, то соединение будет установлено.
|
||
* Если будет возвращено значение отличное от 0, то не будет, в этом случае нужно указать p_result.
|
||
*/
|
||
|
||
/// Сообщить приложению об установлении связи с контроллером.
|
||
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackReleaseInd ( uint32_t arep, pnet_result_t * p_result)
|
||
{
|
||
/**
|
||
* @brief
|
||
* Cоединение будет разорвано при любом возвращаемом значении
|
||
*/
|
||
|
||
/// Сообщить приложению об разрыве связи с контроллером.
|
||
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackDcontrolInd ( uint32_t arep,
|
||
pnet_control_command_t control_command,
|
||
pnet_result_t * p_result)
|
||
{
|
||
/**
|
||
* @brief
|
||
* Можно оставить пустым
|
||
*/
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackCcontrolInd ( uint32_t arep, pnet_result_t * p_result)
|
||
{
|
||
/**
|
||
* @brief
|
||
* Можно оставить пустым
|
||
*/
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackReadInd ( uint32_t arep,
|
||
uint16_t slot_nbr,
|
||
uint16_t subslot_nbr,
|
||
uint16_t idx,
|
||
uint16_t sequence_number,
|
||
uint8_t ** pp_read_data,
|
||
uint16_t * p_read_length,
|
||
pnet_result_t * p_result)
|
||
{
|
||
///1. Проверяем, что у подмодуля есть параметр с индексом idx
|
||
auto param_ptr = getSubmoduleParameter(slot_nbr, subslot_nbr, idx);
|
||
|
||
if (param_ptr == nullptr)
|
||
{
|
||
p_result->pnio_status.error_code = PNET_ERROR_CODE_READ;
|
||
p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
|
||
p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_READ_ERROR;
|
||
p_result->pnio_status.error_code_2 = 0; /* User specific */
|
||
return -1;
|
||
}
|
||
|
||
///2. Проверяем корректность длины данных (меньше быть не должно, если больше - это норм)
|
||
if (*p_read_length < param_ptr->length)
|
||
{
|
||
p_result->pnio_status.error_code = PNET_ERROR_CODE_READ;
|
||
p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
|
||
p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_READ_ERROR;
|
||
p_result->pnio_status.error_code_2 = 0; /* User specific */
|
||
return -1;
|
||
}
|
||
|
||
///3. Передаем данные параметра
|
||
*pp_read_data = param_ptr->data_ptr;
|
||
*p_read_length = param_ptr->length;
|
||
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackWriteInd ( uint32_t arep,
|
||
uint16_t slot_nbr,
|
||
uint16_t subslot_nbr,
|
||
uint16_t idx,
|
||
uint16_t sequence_number,
|
||
uint16_t write_length,
|
||
const uint8_t * p_write_data,
|
||
pnet_result_t * p_result)
|
||
{
|
||
///1. Проверяем, что у подмодуля есть параметр с индексом idx
|
||
auto param_ptr = getSubmoduleParameter(slot_nbr, subslot_nbr, idx);
|
||
|
||
if (param_ptr == nullptr)
|
||
{
|
||
p_result->pnio_status.error_code = PNET_ERROR_CODE_WRITE;
|
||
p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
|
||
p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_WRITE_ERROR;
|
||
p_result->pnio_status.error_code_2 = 0; /* User specific */
|
||
return -1;
|
||
}
|
||
///2. Проверяем корректность длины данных
|
||
if (write_length != param_ptr->length)
|
||
{
|
||
p_result->pnio_status.error_code = PNET_ERROR_CODE_WRITE;
|
||
p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
|
||
p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_WRITE_ERROR;
|
||
p_result->pnio_status.error_code_2 = 0; /* User specific */
|
||
return -1;
|
||
}
|
||
|
||
///3. Копируем данные
|
||
std::memcpy(param_ptr->data_ptr, p_write_data, param_ptr->length);
|
||
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackExpModuleInd ( uint16_t slot, uint32_t module_ident)
|
||
{
|
||
bool ret = addSlotAndPlugModule(slot, module_ident);
|
||
|
||
if (ret == false)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackExpSubmoduleInd ( uint16_t slot,
|
||
uint16_t subslot,
|
||
uint32_t module_id,
|
||
uint32_t submodule_id,
|
||
const pnet_data_cfg_t * p_exp_data)
|
||
{
|
||
bool ret = addSubslotAndPlugSubmodule(slot, subslot, module_id, submodule_id, p_exp_data);
|
||
|
||
if (ret == false)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackNewDataStatusInd ( uint32_t arep,
|
||
uint32_t crep,
|
||
uint8_t changes,
|
||
uint8_t data_status)
|
||
{
|
||
/// Пока ничего не делаем
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackAlarmInd ( uint32_t arep,
|
||
const pnet_alarm_argument_t * p_alarm_arg,
|
||
uint16_t data_len,
|
||
uint16_t data_usi,
|
||
const uint8_t * p_data)
|
||
{
|
||
/**
|
||
* @brief Должен быть ответ pnet_alarm_send_ack
|
||
* Пока заглушка
|
||
*
|
||
*/
|
||
pnet_pnio_status_t pnio_status = {0, 0, 0, 0};
|
||
|
||
pnet_alarm_send_ack (m_pnet_data.pnet_ptr, arep, p_alarm_arg, &pnio_status);
|
||
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackAlarmCnf ( uint32_t arep, const pnet_pnio_status_t * p_pnio_status)
|
||
{
|
||
/// Пока ничего не делаем
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackAlarmAckCnf ( uint32_t arep, int res)
|
||
{
|
||
/// Пока ничего не делаем
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackResetInd ( bool should_reset_application, uint16_t reset_mode)
|
||
{
|
||
/// Пока ничего не делаем
|
||
return 0;
|
||
}
|
||
|
||
int Profinet::callbackSignalLedInd (bool led_state)
|
||
{
|
||
/// Пока ничего не делаем
|
||
return 0;
|
||
} |