//-----------------------------------------------
CANPeakSys2PCI::~CANPeakSys2PCI()
{
if (m_bInitialized)
{
CAN_Close(m_handle);
}
}
//-----------------------------------------------
CANPeakSysDongle::~CANPeakSysDongle()
{
if (m_bInitialized)
{
CAN_Close(m_handle);
}
}
//-----------------------------------------------
CANPeakSysUSB::~CANPeakSysUSB()
{
if (m_bInitialized)
{
CAN_Close(m_handle);
}
}
//-----------------------------------------------
ros_for_can::~ros_for_can()
{
if (m_bInitialized)
{
CAN_Close(m_handle);
}
}
//-----------------------------------------------
CanPeakSys::~CanPeakSys()
{
if (m_bInitialized)
{
CAN_Close(m_handle);
}
}
// what has to be done at program exit
void do_exit(int error)
{
if (h) {
print_diag("receivetest");
CAN_Close(h);
}
printf("receivetest: finished (%d).\n\n", error);
exit(error);
}
// centralized entry point for all exits
static void my_private_exit(int error)
{
if (h)
{
print_diag("bitratetest");
CAN_Close(h);
}
printf("bitratetest: finished (%d).\n\n", error);
exit(error);
}
BVCan::~BVCan(){
#ifdef USE_CAN
if (!h){
CAN_Close(h);
BV_DEBUGINFO4("BVCan::dtor close can");
}
#endif
}
void init(std::string deviceFile, std::chrono::milliseconds syncInterval) {
// canopen::devices must be set up before this function is called
CAN_Close(h);
syncMsg.ID = 0x80;
syncMsg.MSGTYPE = 0x00;
syncMsg.LEN = 0x00;
NMTmsg.ID = 0;
NMTmsg.MSGTYPE = 0x00;
NMTmsg.LEN = 2;
nodeguardMsg.MSGTYPE = 0x01; // remote frame
nodeguardMsg.LEN = 0;
// if (atFirstInit) {
if (!canopen::openConnection(deviceFile)) {
std::cout << "Cannot open CAN device; aborting." << std::endl;
exit(EXIT_FAILURE);
}
if (atFirstInit)
canopen::initListenerThread(canopen::defaultListener);
// atFirstInit = false;
// }
for (auto device : devices) {
sendSDO(device.second.CANid_, ip_time_units, (uint8_t) syncInterval.count() );
sendSDO(device.second.CANid_, ip_time_index, ip_time_index_milliseconds);
sendSDO(device.second.CANid_, sync_timeout_factor, sync_timeout_factor_disable_timeout);
// NMT & motor state machine:
if (atFirstInit) {
canopen::sendNMT(device.second.CANid_, canopen::NMT_stop);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
canopen::sendNMT(device.second.CANid_, canopen::NMT_start);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
setMotorState(device.second.CANid_, "operation_enable");
/* canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_shutdown);
canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_switch_on);
canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_enable_operation); */
}
if (atFirstInit)
atFirstInit = false;
}
int main(int argc, char *argv[])
{
int i, err;
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
mlockall(MCL_CURRENT | MCL_FUTURE);
can_handle = CAN_Open(HW_PCI, 1);
CAN_Init(can_handle, CAN_BAUD_1M, CAN_INIT_TYPE_EX);
printf("Status = %i\n", CAN_Status(can_handle));
for (i = 0; i < 5; i++) {
can_msg[i].MSGTYPE = MSGTYPE_EXTENDED;
can_msg[i].LEN = 8;
printf("Frame = %08lx %02x %02x %02x %02x %02x %02x %02x %02x, "
"Time diff = %llu ns\n",
(unsigned long) (can_msg[i].ID = i*13),
can_msg[i].DATA[0] = i*6,
can_msg[i].DATA[1] = i*7,
can_msg[i].DATA[2] = i*8,
can_msg[i].DATA[3] = i*9,
can_msg[i].DATA[4] = i*10,
can_msg[i].DATA[5] = i*11,
can_msg[i].DATA[6] = i*12,
can_msg[i].DATA[7] = i*13,
i == 0 ? 0 : (unsigned long long) TIME_DIFF
);
}
err = rt_task_create(&test_task, "test", 0, 99, T_JOINABLE);
if (err) {
printf("receivetest: Failed to create rt task, code %d\n",
errno);
return err;
}
rt_task_start(&test_task, test, NULL);
if (err) {
printf("receivetest: Failed to start rt task, code %d\n",
errno);
return errno;
}
rt_task_join(&test_task);
CAN_Close(can_handle);
return 0;
}
void do_exit(int error, HANDLE h)
{
/*!
*\brief The function is based on the do_exit() function from both the transmi- and receivetest programs.
*
*
*
* */
if (h)
{
print_diag("pcan_topics", h);
CAN_Close(h);
}
printf("pcan_topics: finished (%d).\n\n", error);
exit(error);
}
int main()
{
int i=0;
int err=0;
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
mlockall(MCL_CURRENT | MCL_FUTURE);
can_handle = CAN_Open(HW_PCI, 1);
CAN_Init(can_handle, CAN_BAUD_1M, CAN_INIT_TYPE_EX);
printf("Status = %i\n", CAN_Status(can_handle));
err=rt_task_create(&test_task, "test", 0, 99, T_JOINABLE);
if (err) {
printf("receivetest: Failed to create rt task, code %d\n",errno);
return err;
}
rt_task_start(&test_task, test, NULL);
if (err) {
printf("receivetest: Failed to start rt task, code %d\n",errno);
return errno;
}
rt_task_join(&test_task);
for (i = 0; i < 5; i++)
{
printf(
"Frame = %08lx %02x %02x %02x %02x %02x %02x %02x %02x, Time diff = %llu ns\n",
(unsigned long) can_msg[i].Msg.ID,
can_msg[i].Msg.DATA[0],
can_msg[i].Msg.DATA[1],
can_msg[i].Msg.DATA[2],
can_msg[i].Msg.DATA[3],
can_msg[i].Msg.DATA[4],
can_msg[i].Msg.DATA[5],
can_msg[i].Msg.DATA[6],
can_msg[i].Msg.DATA[7],
i == 0 ? 0 :
(can_msg[i].dwTime*(unsigned long long)1000+can_msg[i].wUsec)*1000-
(can_msg[i-1].dwTime*(unsigned long long)1000+can_msg[i-1].wUsec)*1000
);
}
CAN_Close(can_handle);
return 0;
}
bool Device::init(std::string deviceFile)
{
if(canopen::atFirstInit)
{
canopen::atFirstInit = false;
if(std::find(canopen::openDeviceFiles.begin(), canopen::openDeviceFiles.end(), deviceFile) == canopen::openDeviceFiles.end())
{
CAN_Close(canopen::h);
if (!canopen::openConnection(deviceFile, canopen::baudRate)) // check if connection was successful
{
ROS_ERROR_STREAM("Cannot open CAN device " << deviceFile << "... aborting.");
exit(EXIT_FAILURE);
}
canopen::initListenerThread(canopen::defaultListener);
canopen::openDeviceFiles.push_back(deviceFile);
}
sendNMT(NMT_RESET_COMMUNICATION, true);
}
ros::Time start = ros::Time::now();
while(!nmt_init)
{
if((ros::Time::now() - start).toSec() > 5.0)
{
ROS_ERROR_STREAM("Node: " << (int)CANid_ << " is not ready for operation. Please check for potential problems.");
return false;
}
ros::Duration(0.01).sleep();
}
// Configure PDO channels
ROS_DEBUG_STREAM("mapping PDOs of device " << name);
init_pdo();
ros::Duration(0.01).sleep();
sendNMT(NMT_START_REMOTE_NODE);
// std::cout << std::hex << "Initialized the PDO mapping for Node: " << (int)CANid << std::endl;
initialized = true;
set_objects();
return true;
}
/****************************************************************************
* Name: arm_caninit(void)
****************************************************************************/
int arm_caninit(){
/* Initialization of the CAN hardware is performed by logic external to
* this test.
*/
const char *szDeviceNode = DEFAULT_NODE;
errno = 0;
/* Open the CAN device for reading */
// open the CAN port
// please use what is appropriate
// HW_DONGLE_SJA
// HW_DONGLE_SJA_EPP
// HW_ISA_SJA
// HW_PCI
h = LINUX_CAN_Open(szDeviceNode, O_RDWR);
if (!h)
{
errno = nGetLastError();
perror("bitratetest: CAN_Open()");
return 0;
}
/*set the bitrate*/
errno = CAN_Init(h,CAN_BAUD_1M,CAN_INIT_TYPE_ST);
if(errno)
{
perror("set can speed error");
CAN_Close(h);
return 0;
}
/* Now loop the appropriate number of times, performing one loopback test
* on each pass.
*/
printf("can init is ok!\n");
//return 1 is ok;
return 1;
}
void do_exit(int error)
{
shutdownnow = 1;
if (current_state & STATE_FILE_OPENED) {
print_diag("transmitest");
CAN_Close(h);
RESET_INIT_STATE(STATE_FILE_OPENED);
}
#ifdef XENOMAI
if (current_state & STATE_TASK_CREATED) {
rt_task_delete(&writing_task);
RESET_INIT_STATE(STATE_TASK_CREATED);
}
#endif
#ifdef RTAI
pthread_join(writing_thr, NULL);
if (current_state & STATE_TASK_CREATED) {
RESET_INIT_STATE(STATE_TASK_CREATED);
}
rt_task_delete(maint);
#endif
}
PcanPci::~PcanPci()
{
std::cout << "Disconnecting!" << std::endl;
CAN_Close(pcanHandle);
}
int canClose_driver(CAN_HANDLE fd0)
{
CAN_Close(fd0);
return 0;
}
void closeConnection() { CAN_Close(h); }
void signal_handler(int status)
{
rt_task_delete(&test_task);
CAN_Close(can_handle);
exit(0);
}
/*----------------------------------------------------------------------------*/
int main (void)
{
uint8_t item;
UNLOCKREG();
SYS_Init();
UART0_Init();
/* Set GPD14 to control CAN transceiver for Nuvoton board */
PD14 = 0;
/* Select CAN Multi-Function */
CAN_Init();
Note_Configure();
SelectCANSpeed();
do
{
TestItem();
item = GetChar();
printf("\n");
switch (item)
{
case '0':
{
printf("[0] Transmit a message by basic mode\n\n");
printf("Please confirm receiver is ready.\n");
printf("Press any key to continue ...\n\n");
GetChar();
Test_BasicMode_Tx();
break;
}
case '1':
{
printf("[1] Receive a message by basic mode\n\n");
Test_BasicMode_Rx();
break;
}
case '2':
{
printf("[2] Transmit a message by normal mode\n\n");
printf("Please confirm receiver is ready.\n");
printf("Press any key to continue ...\n\n");
GetChar();
Test_NormalMode_Tx();
break;
}
case '3':
{
printf("[3] Receive a message by normal mode\n\n");
Test_NormalMode_Rx();
break;
}
case '4':
{
printf("[4] Test Mask Filter\n\n");
printf("Please confirm [Set Mask Filter] of receiver is selected.\n");
printf("Press any key to continue ...\n\n");
GetChar();
Test_TestMaskFilter();
break;
}
case '5':
{
printf("[5] Set Mask Filter\n\n");
Test_SetMaskFilter();
break;
}
default:
{
printf("Unknown command!\n\n");
}
}
}while(item != 27);
CAN_Close();
}