Commit f485e85d authored by Grégory JANISZEWSKI's avatar Grégory JANISZEWSKI

Ensure bootup frame is only sent AFTER complete node init stages

parent 45326b9c
......@@ -77,6 +77,7 @@ class QTCAN_CANOPEN_EXPORT CanOpenHeartBeatStates {
public:
enum Enum {
Undefined = -1,
Initializing = 0x00,
Stopped = 0x04,
Operational = 0x05,
......
......@@ -47,7 +47,7 @@ CanOpenProtocolManager::CanOpenProtocolManager (const bool autoRemoveMsg, const
, m_nodeGuardToggleBit (false)
, m_sdoTimeout (1000)
, m_localNodeId (0x00)
, m_localNodeState (CanOpenHeartBeatStates::Initializing)
, m_localNodeState (CanOpenHeartBeatStates::Undefined)
, m_localNetworkPos (CanOpenNetPositions::Slave)
, m_driver (Q_NULLPTR)
, m_sdoTransferQueues (this)
......@@ -62,6 +62,7 @@ CanOpenProtocolManager::CanOpenProtocolManager (const bool autoRemoveMsg, const
connect (m_heartBeatTimer, &QTimer::timeout, this, &CanOpenProtocolManager::broadcastHeartBeat);
connect (m_objDict->getSubEntry (0x1006, 0x00), &CanOpenSubEntry::dataChanged, this, &CanOpenProtocolManager::onSyncIntervalChanged);
connect (m_objDict->getSubEntry (0x1017, 0x00), &CanOpenSubEntry::dataChanged, this, &CanOpenProtocolManager::onHeartBeatIntervalChanged);
connect (m_lssQueue.timer, &QTimer::timeout, this, &CanOpenProtocolManager::onLssTimeout);
for (CanOpenSubIndex subIdx = 1; subIdx <= 127; ++subIdx) {
if (CanOpenSubEntry * subEntry = m_objDict->getSubEntry (0x1016, subIdx)) {
connect (subEntry, &CanOpenSubEntry::dataChanged, this, &CanOpenProtocolManager::onHeartbeatConsumerChanged);
......@@ -96,9 +97,9 @@ void CanOpenProtocolManager::start (CanDriver * driver) {
if (!m_active) {
m_driver = driver;
if (m_driver != Q_NULLPTR) {
connect (m_driver, &CanDriver::recv, this, &CanOpenProtocolManager::onMsgRecv, Qt::UniqueConnection);
connect (m_driver, &CanDriver::recv, this, &CanOpenProtocolManager::onMsgRecv);
if (m_hooksForDebug) {
connect (m_driver, &CanDriver::diag, this, &CanOpenProtocolManager::onDiag, Qt::UniqueConnection);
connect (m_driver, &CanDriver::diag, this, &CanOpenProtocolManager::onDiag);
}
m_active = true;
doInitNode ();
......@@ -403,27 +404,23 @@ void CanOpenProtocolManager::handleNMT (CanMessage * msg,
if (reqNode == m_localNodeId || reqNode == 0x00) { // NOTE : addressed node is current node or all
switch (reqState) {
case CanOpenNmtCmdSpecifs::StartNode: {
m_localNodeState = CanOpenHeartBeatStates::Operational;
updateLocalNodeState (CanOpenHeartBeatStates::Operational);
break;
}
case CanOpenNmtCmdSpecifs::StopNode: {
m_localNodeState = CanOpenHeartBeatStates::Stopped;
updateLocalNodeState (CanOpenHeartBeatStates::Stopped);
break;
}
case CanOpenNmtCmdSpecifs::SetPreOp: {
m_localNodeState = CanOpenHeartBeatStates::PreOperational;
updateLocalNodeState (CanOpenHeartBeatStates::PreOperational);
break;
}
case CanOpenNmtCmdSpecifs::ResetNode:
case CanOpenNmtCmdSpecifs::ResetComm: {
m_localNodeState = CanOpenHeartBeatStates::Initializing;
doInitNode ();
break;
}
}
emit localNodeStateChanged (m_localNodeState);
if (m_localNodeState == CanOpenHeartBeatStates::Initializing) {
doInitNode ();
}
}
}
}
......@@ -450,30 +447,15 @@ void CanOpenProtocolManager::handleSYNC (CanMessage * msg, const CanOpenNodeId n
}
}
void CanOpenProtocolManager::doInitNode (void) {
connect (m_lssQueue.timer, &QTimer::timeout, this, &CanOpenProtocolManager::onLssTimeout, Qt::UniqueConnection);
updateLocalNodeState (CanOpenHeartBeatStates::Initializing);
refreshSdoConfig ();
refreshPdoMapping ();
broadcastHeartBeat ();
refreshHeartbeatConsumers ();
onSyncIntervalChanged ();
onHeartBeatIntervalChanged ();
for (CanOpenHeartbeatConsumer * consumer : m_hbConsumersByNodeId) {
consumer->reset ();
if (CanOpenSubEntry * subEntry = m_objDict->getSubEntry (0x1016, consumer->getNodeId ())) {
consumer->reconfigure (subEntry->readAs<quint16> ());
}
}
switch (m_localNetworkPos) {
case CanOpenNetPositions::Slave: {
m_localNodeState = CanOpenHeartBeatStates::PreOperational;
break;
}
case CanOpenNetPositions::Master: {
m_localNodeState = CanOpenHeartBeatStates::Operational;
break;
}
}
emit localNodeStateChanged (m_localNodeState);
broadcastHeartBeat ();
}
void CanOpenProtocolManager::onSyncIntervalChanged (void) {
......@@ -506,6 +488,25 @@ void CanOpenProtocolManager::onHeartbeatConsumerChanged (void) {
void CanOpenProtocolManager::broadcastHeartBeat (void) {
const quint8 frameBootUp = m_localNodeState; // NOTE : Initializing will give BootUp
doMsgSend (new CanMessage (CanId (quint16 (0x700 + m_localNodeId)), sizeof (frameBootUp), &frameBootUp));
if (m_localNodeState == CanOpenHeartBeatStates::Initializing) {
switch (m_localNetworkPos) {
case CanOpenNetPositions::Slave: {
updateLocalNodeState (CanOpenHeartBeatStates::PreOperational);
break;
}
case CanOpenNetPositions::Master: {
updateLocalNodeState (CanOpenHeartBeatStates::Operational);
break;
}
}
}
}
void CanOpenProtocolManager::updateLocalNodeState (const CanOpenHeartBeatState state) {
if (m_localNodeState != state) {
m_localNodeState = state;
emit localNodeStateChanged (state);
}
}
void CanOpenProtocolManager::broadcastSync (void) {
......@@ -1306,6 +1307,15 @@ void CanOpenProtocolManager::refreshPdoMapping (void) {
emit configuredPDOs (tmp);
}
void CanOpenProtocolManager::refreshHeartbeatConsumers (void) {
for (CanOpenHeartbeatConsumer * consumer : m_hbConsumersByNodeId) {
consumer->reset ();
if (CanOpenSubEntry * subEntry = m_objDict->getSubEntry (0x1016, consumer->getNodeId ())) {
consumer->reconfigure (subEntry->readAs<quint16> ());
}
}
}
void CanOpenProtocolManager::enqueueSdoTransfer (CanOpenSdoTransfer * transfer) {
if (transfer != Q_NULLPTR) {
if (CanOpenSdoTransferQueue * queue = m_sdoTransferQueues.getById (transfer->nodeId)) {
......
......@@ -107,6 +107,9 @@ signals:
const CanOpenSdoAbortCode statusCode,
const QByteArray & buffer);
protected:
void updateLocalNodeState (const CanOpenHeartBeatState state);
protected slots:
void doInitNode (void);
......@@ -129,8 +132,9 @@ protected slots:
void handlePDO (CanMessage * msg, const CanOpenNodeId nodeId, const bool isRtr);
void handleLSS (CanMessage * msg, const CanOpenNodeId nodeId, const bool isRtr);
void refreshSdoConfig (void);
void refreshPdoMapping (void);
void refreshSdoConfig (void);
void refreshPdoMapping (void);
void refreshHeartbeatConsumers (void);
void enqueueSdoTransfer (CanOpenSdoTransfer * transfer);
void processSdoTransfer (CanOpenSdoTransfer * transfer);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment