CanFestival 主站-NMT初始化
CanFestival 是开源的 CANopen 协议栈，主站（NMT Master）负责网络管理和从站状态控制。NMT（Network Management）初始化流程主要涉及主站自身的启动、从站 Boot-up 检测以及网络配置。整个过程遵循 CANopen 标准（CiA 301），主站通过 NMT 命令（COB-ID 0x000）控制从站状态切换。
提供一段基于CanFestival 中 CANOpenShell.c 修改的一段代码和头文件CANOpenShell.h
/*
This file is part of CanFestival, a library implementing CanOpen Stack.Copyright (C): Edouard TISSERANT and Francis DUPINSee COPYING file for copyrights details.This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/// 系统头文件
#if defined(WIN32) && !defined(__CYGWIN__)#include <windows.h>#define CLEARSCREEN "cls"#define SLEEP(time) Sleep(time * 1000)
#else#include <unistd.h>#include <stdio.h>#include <string.h>#include <stdlib.h>#define CLEARSCREEN "clear"#define SLEEP(time) sleep(time)
#endif// CanFestival头文件
#include "canfestival.h"
#include "CANOpenShell.h"
#include "CANOpenShellMasterOD.h"
#include "CANOpenShellSlaveOD.h"// 常量定义
#define MAX_NODES 127
#define MIN_NODE_ID 1
#define cst_str4(c1, c2, c3, c4) ((((unsigned int)0 | \(char)c4 << 8) | \(char)c3) << 8 | \(char)c2) << 8 | \(char)c1
#define INIT_ERR 2
#define QUIT 1// 全局变量
char BoardBusName[31];
char BoardBaudRate[5];
s_BOARD Board = {BoardBusName, BoardBaudRate};
CO_Data* CANOpenShellOD_Data;
char LibraryPath[512];/* 睡眠函数 */
void SleepFunction(int second)
{
#ifdef USE_RTAIsleep(second);
#elseSLEEP(second);
#endif
}/* 启动节点（Operational） */
void StartNode(UNS8 nodeid)
{if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return;}masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Start_Node);
}/* 停止节点（Stopped） */
void StopNode(UNS8 nodeid)
{if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return;}masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Stop_Node);
}/* 进入Pre-Operational状态 */
void PreOperationalNode(UNS8 nodeid)
{if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return;}masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Enter_PreOperational);
}/* 重置节点 */
void ResetNode(UNS8 nodeid)
{masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Reset_Node);
}/* 扫描节点 */
void DiscoverNodes()
{printf("等待从节点启动...\n\n");ResetNode(0x00);
}int get_info_step = 0;/* 检查SDO读取结果（节点信息） */
void CheckReadInfoSDO(CO_Data* d, UNS8 nodeid)
{UNS32 abortCode;UNS32 data = 0;UNS32 size = 64;if (getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)printf("主站: 获取节点%2.2x信息失败，错误码: %8.8x\n", nodeid, abortCode);else{switch (get_info_step){case 1:printf("设备类型 : %8.8x\n", data);break;case 2:printf("供应商ID : %8.8x\n", data);break;case 3:printf("产品代码 : %8.8x\n", data);break;case 4:printf("修订号 : %8.8x\n", data);break;}}closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);GetSlaveNodeInfo(nodeid);
}/* 获取从节点信息 */
void GetSlaveNodeInfo(UNS8 nodeid)
{if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);get_info_step = 0;return;}switch (++get_info_step){case 1:printf("##################################\n");printf("#### 节点%2.2x信息 ####\n", nodeid);printf("##################################\n");readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1000, 0x00, 0, CheckReadInfoSDO, 0);break;case 2:readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x01, 0, CheckReadInfoSDO, 0);break;case 3:readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x02, 0, CheckReadInfoSDO, 0);break;case 4:readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x03, 0, CheckReadInfoSDO, 0);break;case 5:get_info_step = 0;}
}/* 检查SDO读取结果（通用） */
void CheckReadSDO(CO_Data* d, UNS8 nodeid)
{UNS32 abortCode;UNS32 data = 0;UNS32 size = 64;if (getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)printf("\n结果: 获取节点%2.2x信息失败，错误码: %8.8x，数据大小: %d\n", nodeid, abortCode, size);elseprintf("\n结果: %8.8x (大小: %d字节)\n", data, size);closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
}/* 读取SDO */
void ReadDeviceEntry(char* sdo)
{int ret = 0;int nodeid;int index;int subindex;int datatype = 0;ret = sscanf(sdo, "rsdo#%2x,%4x,%2x", &nodeid, &index, &subindex);if (ret == 3){if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return;}printf("##################################\n");printf("#### 读取SDO ####\n");printf("##################################\n");printf("节点ID : %2.2x\n", nodeid);printf("索引 : %4.4x\n", index);printf("子索引 : %2.2x\n", subindex);readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO, 0);}elseprintf("命令错误: %s\n", sdo);
}/* 检查SDO写入结果 */
void CheckWriteSDO(CO_Data* d, UNS8 nodeid)
{UNS32 abortCode;if (getWriteResultNetworkDict(CANOpenShellOD_Data, nodeid, &abortCode) != SDO_FINISHED)printf("\n结果: 写入节点%2.2x失败，错误码: %8.8x\n", nodeid, abortCode);elseprintf("\n发送数据成功\n");closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
}/* 写入SDO */
void WriteDeviceEntry(char* sdo)
{int ret = 0;int nodeid;int index;int subindex;int size;int data;ret = sscanf(sdo, "wsdo#%2x,%4x,%2x,%2x,%x", &nodeid, &index, &subindex, &size, &data);if (ret == 5){if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return;}printf("##################################\n");printf("#### 写入SDO ####\n");printf("##################################\n");printf("节点ID : %2.2x\n", nodeid);printf("索引 : %4.4x\n", index);printf("子索引 : %2.2x\n", subindex);printf("大小 : %2.2x\n", size);printf("数据 : %x\n", data);writeNetworkDictCallBack(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, size, 0, &data, CheckWriteSDO, 0);}elseprintf("命令错误: %s\n", sdo);
}/* 发送RPDO1设置控制字和速度 */
void SetVelocity(CO_Data* d, UNS8 nodeid, UNS16 controlword, UNS32 velocity)
{if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return;}Message m;m.cob_id = 0x200 + nodeid; // RPDO1的COB-ID（0x200+nodeid）m.len = 7; // 数据长度：控制字(2字节)+操作模式(1字节)+目标速度(4字节)m.rtr = 0;m.data[0] = controlword & 0xFF; // 控制字低字节m.data[1] = (controlword >> 8) & 0xFF; // 控制字高字节m.data[2] = 0x03; // 操作模式固定为3（Profile Velocity Mode）m.data[3] = velocity & 0xFF; // 目标速度低字节m.data[4] = (velocity >> 8) & 0xFF;m.data[5] = (velocity >> 16) & 0xFF;m.data[6] = (velocity >> 24) & 0xFF;if (canSend(d->canHandle, &m) == 0)printf("发送RPDO1成功: 节点=%2.2x, COB-ID=%3.3x, 控制字=%4.4x, 速度=%d rpm\n", nodeid, m.cob_id, controlword, velocity);elseprintf("发送RPDO1失败: 节点=%2.2x, COB-ID=%3.3x\n", nodeid, m.cob_id);
}/* 处理速度设置命令 */
void ProcessVelocityCommand(char* command)
{int ret = 0;int nodeid;int controlword;int velocity;ret = sscanf(command, "vset#%2x,%4x,%d", &nodeid, &controlword, &velocity);if (ret == 3){if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return;}printf("##################################\n");printf("#### 设置控制字和速度 (RPDO1) ####\n");printf("##################################\n");printf("节点ID : %2.2x\n", nodeid);printf("控制字 : %4.4x\n", controlword);printf("目标速度 : %d rpm\n", velocity);SetVelocity(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)controlword, (UNS32)velocity);}elseprintf("命令错误: %s\n", command);
}/* 从站启动回调 */
void CANOpenShellOD_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
{printf("从节点 %2.2x 启动\n", nodeid);
}/* 初始化回调 */
void CANOpenShellOD_initialisation(CO_Data* d)
{printf("节点初始化\n");
}/* 进入Pre-Operational回调 */
void CANOpenShellOD_preOperational(CO_Data* d)
{printf("节点进入Pre-Operational状态\n");
}/* 进入Operational回调 */
void CANOpenShellOD_operational(CO_Data* d)
{printf("节点进入Operational状态\n");
}/* 进入Stopped回调 */
void CANOpenShellOD_stopped(CO_Data* d)
{printf("节点进入Stopped状态\n");
}/* 同步回调 */
void CANOpenShellOD_post_sync(CO_Data* d)
{// printf("主站同步\n");
}/* 发送TPDO回调 */
void CANOpenShellOD_post_TPDO(CO_Data* d)
{// printf("主站发送TPDO\n");
}/* 初始化主站或从站 */
void Init(CO_Data* d, UNS32 id)
{if (Board.baudrate){setState(CANOpenShellOD_Data, Initialisation);}
}/* 清理资源 */
void Exit(CO_Data* d, UNS32 nodeid)
{if (strcmp(Board.baudrate, "none")){masterSendNMTstateChange(CANOpenShellOD_Data, 0, NMT_Reset_Node);setState(CANOpenShellOD_Data, Stopped);}
}/* 初始化节点 */
int NodeInit(int NodeID, int NodeType)
{if (NodeID < MIN_NODE_ID || NodeID > MAX_NODES) {printf("错误: 主站节点ID %d 超出范围（%d-%d）\n", NodeID, MIN_NODE_ID, MAX_NODES);return INIT_ERR;}if (NodeType)CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;elseCANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;LoadCanDriver(LibraryPath);CANOpenShellOD_Data->initialisation = CANOpenShellOD_initialisation;CANOpenShellOD_Data->preOperational = CANOpenShellOD_preOperational;CANOpenShellOD_Data->operational = CANOpenShellOD_operational;CANOpenShellOD_Data->stopped = CANOpenShellOD_stopped;CANOpenShellOD_Data->post_sync = CANOpenShellOD_post_sync;CANOpenShellOD_Data->post_TPDO = CANOpenShellOD_post_TPDO;CANOpenShellOD_Data->post_SlaveBootup = CANOpenShellOD_post_SlaveBootup;if (!canOpen(&Board, CANOpenShellOD_Data)) return INIT_ERR;setNodeId(CANOpenShellOD_Data, (UNS8)NodeID);StartTimerLoop(&Init);return 0;
}/* 帮助菜单 */
void help_menu(void)
{printf(" 必选命令（必须是第一个命令）:\n");printf(" load#CanLibraryPath,channel,baudrate,nodeid,type (0:从站, 1:主站)\n");printf(" nodeid范围: 0x01-0x7F\n");printf("\n");printf(" 网络命令: (nodeid=0x00表示广播，0x01-0x7F为有效节点ID)\n");printf(" ssta#nodeid : 启动节点（Operational）\n");printf(" ssto#nodeid : 停止节点（Stopped）\n");printf(" spre#nodeid : 进入Pre-Operational状态\n");printf(" srst#nodeid : 重置节点\n");printf(" scan : 重置所有节点并打印启动消息\n");printf(" wait#seconds : 睡眠n秒\n");printf("\n");printf(" SDO命令: (size表示字节数)\n");printf(" info#nodeid : 获取节点信息\n");printf(" rsdo#nodeid,index,subindex : 读取SDO\n");printf(" 示例: rsdo#0B,1018,01\n");printf(" wsdo#nodeid,index,subindex,size,data : 写入SDO\n");printf(" 示例: wsdo#0B,6200,01,01,FF\n");printf("\n");printf(" PDO命令:\n");printf(" vset#nodeid,controlword,velocity : 设置控制字和速度（通过RPDO1，COB-ID=0x200+nodeid）\n");printf(" 示例: vset#0B,000F,1000\n");printf("\n");printf(" 注意: 所有数字均为十六进制\n");printf("\n");printf(" help : 显示此菜单\n");printf(" quit : 退出程序\n");printf("\n");
}/* 提取节点ID */
UNS8 ExtractNodeId(char *command) {int nodeid;sscanf(command, "%2x", &nodeid);if (nodeid < MIN_NODE_ID || nodeid > MAX_NODES) {printf("错误: 节点ID %2.2x 超出范围（%d-%d）\n", nodeid, MIN_NODE_ID, MAX_NODES);return 0;}return (UNS8)nodeid;
}/* 处理命令 */
int ProcessCommand(char* command)
{int ret = 0;int sec = 0;int NodeID;int NodeType;EnterMutex();switch (cst_str4(command[0], command[1], command[2], command[3])){case cst_str4('h', 'e', 'l', 'p'):help_menu();break;case cst_str4('s', 's', 't', 'a'):StartNode(ExtractNodeId(command + 5));break;case cst_str4('s', 's', 't', 'o'):StopNode(ExtractNodeId(command + 5));break;case cst_str4('s', 'p', 'r', 'e'):PreOperationalNode(ExtractNodeId(command + 5));break;case cst_str4('s', 'r', 's', 't'):ResetNode(ExtractNodeId(command + 5));break;case cst_str4('i', 'n', 'f', 'o'):GetSlaveNodeInfo(ExtractNodeId(command + 5));break;case cst_str4('r', 's', 'd', 'o'):ReadDeviceEntry(command);break;case cst_str4('w', 's', 'd', 'o'):WriteDeviceEntry(command);break;case cst_str4('s', 'c', 'a', 'n'):DiscoverNodes();break;case cst_str4('w', 'a', 'i', 't'):ret = sscanf(command, "wait#%d", &sec);if (ret == 1) {LeaveMutex();SleepFunction(sec);return 0;}break;case cst_str4('v', 's', 'e', 't'):ProcessVelocityCommand(command);break;case cst_str4('q', 'u', 'i', 't'):LeaveMutex();return QUIT;case cst_str4('l', 'o', 'a', 'd'):ret = sscanf(command, "load#%100[^,],%30[^,],%4[^,],%d,%d",LibraryPath,BoardBusName,BoardBaudRate,&NodeID,&NodeType);if (ret == 5){LeaveMutex();ret = NodeInit(NodeID, NodeType);return ret;}else{printf("无效的加载参数\n");}break;default:help_menu();}LeaveMutex();return 0;
}/* 主函数 */
int main(int argc, char** argv)
{char command[200];char* res;int ret = 0;int sysret = 0;int i = 0;if (argc < 2){help_menu();exit(1);}TimerInit();for (i = 1; i < argc; i++){if (ProcessCommand(argv[i]) == INIT_ERR) goto init_fail;}while (ret != QUIT){res = fgets(command, sizeof(command), stdin);sysret = system(CLEARSCREEN);ret = ProcessCommand(command);fflush(stdout);}printf("程序结束。\n");StopTimerLoop(&Exit);canClose(CANOpenShellOD_Data);init_fail:TimerCleanup();return 0;
}
/*
This file is part of CanFestival, a library implementing CanOpen Stack.Copyright (C): Edouard TISSERANT and Francis DUPINSee COPYING file for copyrights details.This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifdef USE_XENO
#define eprintf(...)
#else
#define eprintf(...) printf (__VA_ARGS__)
#endif#include "canfestival.h"void help(void);
void StartNode(UNS8);
void StopNode(UNS8);
void ResetNode(UNS8);
void DiscoverNodes(void);
void CheckReadInfoSDO(CO_Data*, UNS8);
void GetSlaveNodeInfo(UNS8);
void CheckReadSDO(CO_Data*, UNS8);
void CheckWriteSDO(CO_Data*, UNS8);
void ReadDeviceEntry(char*);
void WriteDeviceEntry(char*);
void SleepFunction(int);
替换主站中的上述两个文件，使用指令 make 进行编译。
1. 硬件和环境准备
- 确保CAN总线连接正确（CAN_H、CAN_L、GND，120欧姆终端电阻）。
- 配置SocketCAN：
sudo ip link set can0 up type can bitrate 500000另启终端验证通信：candump can0
candump can02. 主站和从站初始化
- 启动主站：
&&&:~/CanFestival-master/examples/CANOpenShell$ ./CANOpenShell load#libcanfestival_can_socket.so,can0,500k,1,1
节点初始化
节点进入Pre-Operational状态终端：
&&&:~$ candump can0can0 701 [1] 00
重置从站（ID=7）：
srst#07
wait#2终端：
配置心跳（可选）：对象0x1017，2字节，1000ms心跳。
wsdo#07,1017,00,02,03E8终端显示：
到此代表NMT网络已配置成功。此时从站按照1s的时间间隔发送心跳报文到主站，此时从站已经进入预操作模式。
