//
//called when the OnBus button is pressed
//
void CCanTestMFCDlg::OnOnBus()
{
int stat;
int handle;
char tmpStr[200];
//init canlib
canInitializeLibrary();
//open channel 0
handle = canOpenChannel(0, canOPEN_EXCLUSIVE);
if (handle < 0) {
sprintf(tmpStr, "ERROR: canOpenChannel() handle returned: %d", handle);
AfxMessageBox(tmpStr);
}
//set up the bus
stat = canSetBusParams(handle, canBITRATE_125K, 0, 0, 0, 0, 0);
if (stat < 0) {
AfxMessageBox("ERROR: canSetBusParams().");
}
//go on bus
stat = canBusOn(handle);
if (stat < 0) {
AfxMessageBox("ERROR: canBusOn().");
}
//set notification
stat = canSetNotify(handle, this->GetSafeHwnd() , canNOTIFY_RX | canNOTIFY_ERROR | canNOTIFY_STATUS | canEVENT_TX | canNOTIFY_ENVVAR);
//save the can channel handle in the global variable m_handle
m_handle = handle;
}
int CDeviceCAN::OpenCAN()
{
int stat;
canInitializeLibrary();
canhandle = canOpenChannel(0, canOPEN_EXCLUSIVE);
if (canhandle < 0) {
AfxMessageBox("打开端口错误");
return STATUS_ERR;
}
Baudrates PCANBaudrate;
if(m_canBaudrate == 500)
{
PCANBaudrate = BAUD_500K;
}
else
{
PCANBaudrate = BAUD_250K;
}
stat = canSetBusParams(canhandle, PCANBaudrate, 0, 0, 1, 1, 0);
if (stat < 0)
{
AfxMessageBox("设置参数错误");
return STATUS_ERR;
}
stat = canBusOn(canhandle);
if (stat < 0) {
AfxMessageBox("挂入总线错误");
return STATUS_ERR;
}
return STATUS_OK;
}
void incBusLoad (int channel, int flags, int load)
{
canHandle handle;
unsigned char msg[8] = "Kvaser!";
int i, id = 100, dlc = sizeof(msg);
canStatus stat = canERR_PARAM;
handle = canOpenChannel(channel, flags);
if (handle < 0) {
check("canOpenChannel", handle);
return;
}
stat = canBusOff(handle);
check("canBusOff", stat);
stat = canSetBusParams(handle, bitrate, 0, 0, 0, 0, 0);
check("canSetBusParams", stat);
canBusOn(handle);
for (i = 0; i < load; i++) {
stat = canWrite(handle, id, &msg, dlc, 0);
check("canWrite", stat);
}
stat = canWriteWait(handle, id, &msg, dlc, 0, -1);
check("canWriteWait", stat);
canBusOff(handle);
canClose(handle);
}
/**
* Channels and their descriptors are numbered starting from zero.
* So I need to increment by 1 the handle returned by CANLIB because
* CANfestival CAN_HANDLEs with value zero are considered NOT VALID.
*
* The baud rate could be given directly as bit/s
* or using one of the BAUD_* constants defined
* in canlib.h
*/
CAN_HANDLE canOpen_driver(s_BOARD *board)
{
int fd0 = -1;
int channel, baud;
canStatus retval = canOK;
fd0--;
sscanf(board->busname, "%d", &channel);
baud = TranslateBaudRate(board->baudrate);
if (baud == 0)
{
sscanf(board->baudrate, "%d", &baud);
}
fd0 = canOpenChannel(channel, canWANT_EXCLUSIVE|canWANT_EXTENDED);
if (fd0 < 0)
{
fprintf(stderr, "canOpen_driver (Kvaser) : error opening channel %d\n", channel);
return (CAN_HANDLE)(fd0+1);
}
canBusOff(fd0);
/* values for tseg1, tseg2, sjw, noSamp and syncmode
* come from canlib example "simplewrite.c". The doc
* says that default values will be taken if baud is one of
* the BAUD_* values
*/
retval = canSetBusParams(fd0, baud, 4, 3, 1, 1, 0);
if (retval != canOK)
{
fprintf(stderr, "canOpen_driver (Kvaser) : canSetBusParams() error, returned value = %d, baud=%d, \n", retval, baud);
canClose((int)fd0);
return (CAN_HANDLE)retval;
}
canSetBusOutputControl(fd0, canDRIVER_NORMAL);
if (retval != canOK)
{
fprintf(stderr, "canOpen_driver (Kvaser) : canSetBusOutputControl() error, returned value = %d\n", retval);
canClose((int)fd0);
return (CAN_HANDLE)retval;
}
retval = canBusOn(fd0);
if (retval != canOK)
{
fprintf(stderr, "canOpen_driver (Kvaser) : canBusOn() error, returned value = %d\n", retval);
canClose((int)fd0);
return (CAN_HANDLE)retval;
}
return (CAN_HANDLE)(fd0+1);
}
static bool dbg_kavaser_can_init(uint32 ch)
{
CanHandle handle;
canInitializeLibrary();
handle = canOpenChannel(dbg_kvaser.channel, canOPEN_EXCLUSIVE);
if (handle < 0) {
printf("dbg_kavaser_can_init:canOpenChannel err=%d\n", handle);
return FALSE;
}
printf("dbg_kavaser_can_init:canOpenChannel OK:handle=%d\n", handle);
dbg_kvaser.handle = handle;
dbg_kvaser.status = canSetBusParams(handle, dbg_kvaser.baud, 0, 0, 0, 0, 0);
if (dbg_kvaser.status != canOK) {
printf("dbg_kavaser_can_init:canSetBusParams err=%d\n", dbg_kvaser.status);
return FALSE;
}
printf("dbg_kavaser_can_init:canSetBusParams OK:baud=%d\n", dbg_kvaser.baud);
dbg_kvaser.status = canBusOn(handle);
if (dbg_kvaser.status != canOK) {
printf("dbg_kavaser_can_init:canBusOn err=%d\n", dbg_kvaser.status);
return FALSE;
}
printf("dbg_kavaser_can_init:canBusOn OK\n");
return TRUE;
}
/* canOpenExt_
* open channel for extended CAN frames
* set baud rate to 250 Kbit/s
* typically used for refuse (J1939)
*/
int canOpenExt_ (int channel) {
int h;
h = canOpenChannel(channel, canWANT_EXTENDED);
canSetBusParams(h, BAUD_250K, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
return h;
}
/* canOpenStd_
* open channel for standard CAN frames
* set baud rate to 500 Kbit/s
* typically used for shuttle
*/
int canOpenStd_ (int channel) {
int h;
h = canOpenChannel(channel, 0);
canSetBusParams(h, BAUD_500K, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
return h;
}
void MainWindow::on_pushButtonBusOn_clicked()
{
bool fail = false;
canStatus stat;
qDebug() debugprefix << "on_pushButtonBusOn_clicked()";
if(busOn){
canBusOff(canHandle);
ui->pushButtonBusOn->setText("Go On Bus");
busOn = false;
}else{
// Timings
int br = (ui->comboBoxBaud->currentIndex()+1)*-1;
stat = canSetBusParams(canHandle, br , 0, 0, 0, 0, 0);
if (stat < 0){
char buf[128];
canGetErrorText(stat,buf,128);
qDebug() debugprefix << "canSetBusParams("<< br << ") " << buf;
fail = true;
}
// Silent
bool silent = ui->checkBox_Silent->isChecked();
if(silent){
stat = canSetBusOutputControl(canHandle, canDRIVER_SILENT);
}else{
stat = canSetBusOutputControl(canHandle, canDRIVER_NORMAL);
}
if (stat < 0){
char buf[128];
canGetErrorText(stat,buf,128);
qDebug() debugprefix << "canSetBusOutputControl("<< silent << ") " << buf;
fail = true;
}
if(fail == false){
stat = canBusOn(canHandle);
if (stat < 0){
char buf[128];
canGetErrorText(stat,buf,128);
qDebug() debugprefix << "canBusOn("<< br << ") " << buf;
fail = true;
QMessageBox msgBox;
msgBox.setText("Couldn't enter Bus-On state");
msgBox.exec();
}else{
ui->pushButtonBusOn->setText("Go Off Bus");
busOn = true;
}
}else{
QMessageBox msgBox;
msgBox.setText("Couldn't enter Bus-On state due to initialization errors");
msgBox.exec();
}
}
}
int main (int argc, char *argv[])
{
canHandle h;
int channel;
int bitrate = BAUD_1M;
errno = 0;
if (argc != 3 || (channel = 0, errno) != 0) {
printf("usage %s address byte1\n", argv[0]);
exit(1);
} else {
printf("Sending a message on channel %d\n", channel);
}
/* Allow signals to interrupt syscalls(e.g in canReadBlock) */
siginterrupt(SIGINT, 1);
/* Open channel, set parameters and go on bus */
//h = canOpenChannel(channel, canOPEN_EXCLUSIVE | canOPEN_REQUIRE_EXTENDED);
h = canOpenChannel(channel, canOPEN_EXCLUSIVE);
if (h < 0) {
printf("canOpenChannel %d failed\n", channel);
return -1;
}
canBusOff(h);
check("canSetBusParams", canSetBusParams(h, bitrate, 4, 3, 2, 1, 0));
// Work-around for Leaf bug
check("canSetBusOutputControl", canSetBusOutputControl(h, canDRIVER_NORMAL));
check("canBusOn", canBusOn(h));
//OY 8/20/2014
//unsigned char channel = atoi(argv[2]));
msg[0] = 0;
msg[2] = 1;
msg[3] = 41;
if (atoi(argv[2]) == 0)
msg[1] = 1;
if (atoi(argv[2]) == 100)
msg[1] = 2;
check("canWrite", canWrite(h, atoi(argv[1]), msg, 4, 0));
//printf("%d %d %d %d %d\n",msg[0], msg[1], msg[2], msg[3], channel);
check("canWriteSync", canWriteSync(h, 1000));
//check("canBusOff", canBusOff(h));
check("canClose", canClose(h));
return 0;
}
UNS8 canChangeBaudRate_driver( CAN_HANDLE fd0, char* baud)
{
int baudrate;
canStatus retval = canOK;
baudrate = TranslateBaudRate(baud);
if (baudrate == 0)
{
sscanf(baud, "%d", &baudrate);
}
fprintf(stderr, "%x-> changing to baud rate %s[%d]\n", (int)fd0, baud, baudrate);
canBusOff((int)fd0);
/* values for tseg1, tseg2, sjw, noSamp and syncmode
* come from canlib example "simplewrite.c". The doc
* says that default values will be taken if baud is one of
* the BAUD_* values
*/
retval = canSetBusParams((int)fd0, baudrate, 4, 3, 1, 1, 0);
if (retval != canOK)
{
fprintf(stderr, "canChangeBaudRate_driver (Kvaser) : canSetBusParams() error, returned value = %d, baud=%d, \n", retval, baud);
canClose((int)fd0);
return (UNS8)retval;
}
canSetBusOutputControl((int)fd0, canDRIVER_NORMAL);
if (retval != canOK)
{
fprintf(stderr, "canChangeBaudRate_driver (Kvaser) : canSetBusOutputControl() error, returned value = %d\n", retval);
canClose((int)fd0);
return (UNS8)retval;
}
retval = canBusOn((int)fd0);
if (retval != canOK)
{
fprintf(stderr, "canChangeBaudRate_driver (Kvaser) : canBusOn() error, returned value = %d\n", retval);
canClose((int)fd0);
return (UNS8)retval;
}
return 0;
}
int InitCtrlWithFlags(int ctrl, int bitrate, int flags)
{
int stat;
int hnd;
canInitializeLibrary();
hnd = canOpenChannel(ctrl, flags);
if (hnd < 0) Check("canOpenChannel", hnd);
if (bitrate < 0) {
stat = canSetBusParams(hnd, bitrate, 0, 0, 0, 0, 0);
Check("canSetBusParams", stat);
} else if (bitrate == 5000) {
// Special case for 5k
stat = canSetBusParams(hnd, 5000, 16, 8, 1, 1, 0);
Check("canSetBusParams", stat);
} else if (bitrate == 10000) {
// Special case for 10k
stat = canSetBusParams(hnd, 10000, 12, 7, 1, 1, 0);
Check("canSetBusParams", stat);
} else if (bitrate == 20000) {
// Special case for 20k
stat = canSetBusParamsC200(hnd, 0x58, 0x67);
Check("canSetBusParamsC200", stat);
} else if (bitrate == 40000) {
// Special case for 40k
stat = canSetBusParamsC200(hnd, 0x58, 0x14);
Check("canSetBusParamsC200", stat);
} else {
Error("Bad bitrate sent to canInitCtrlWithFlags: %u",bitrate);
}
stat = canBusOn(hnd);
Check("canBusOn", stat);
stat = canSetBusOutputControl(hnd, canDRIVER_NORMAL);
Check("canSetBusOutputControl", stat);
return hnd;
}
/* Initializes the class by opening 2 can channels at the given
* frequency, which should be one of the #defined BAUD_*K values
*
* returns: 0 on success, negative on error
*/
int
CANIOKvaser::Init(long channel_freq)
{
int ret;
// Open up both CAN channels
for (int i =0; i < DUALCAN_NR_CHANNELS; i++)
{
if((channels[i] =
canOpenChannel(i, canWANT_EXCLUSIVE | canWANT_EXTENDED)) < 0) {
return channels[i];
}
// set the channel params: 500Kbps ... CANLIB will set the other params
// to defaults if we use BAUD_500K
// if ((ret = canSetBusParams(channels[i], channel_freq, 4, 3, 1, 1, 0)) < 0) {
if ((ret = canSetBusParams(channels[i], channel_freq, 0, 0, 0, 0, 0)) < 0)
return ret;
// set filter to only accept packets we are interested in...
// that is, messages with IDs 0x400, 0x401, ..., 0x404
if ((ret = canAccept(channels[i], 0x400, canFILTER_SET_MASK_STD)) < 0) {
return ret;
}
if ((ret = canAccept(channels[i], 0x400, canFILTER_SET_CODE_STD)) < 0) {
return ret;
}
// turn on the bus!
if ((ret = canBusOn(channels[i])) < 0)
return ret;
}
return 0;
}
int main(int argc, char *argv[]) {
// Setup CAN and ROS. Then loop forever processing received CAN messages, and publishing
// info on ROS topics as needed. Also subcribed to ROS topic for commands.
int ret = -1;
long id;
unsigned char msg[8];
unsigned int dlc;
unsigned int flag;
unsigned long t;
int channel = 0;
int bitrate = BAUD_250K;
///////////////
// ROS setup //
///////////////
ros::init(argc, argv, "globe_epas");
ros::NodeHandle n;
ros::Publisher globe_epas_command_mode_pub = n.advertise<globe_epas::globe_epas_cmd>("report/command_mode", 1000);
ros::Publisher globe_epas_inc_position_pub = n.advertise<std_msgs::Float64>("report/inc_position", 1000);
ros::Publisher globe_epas_current_pub = n.advertise<std_msgs::Float64>("report/current", 1000);
ros::Publisher globe_epas_velocity_pub = n.advertise<std_msgs::Float64>("report/velocity", 1000);
ros::Publisher globe_epas_unit_temp_pub = n.advertise<std_msgs::Float64>("report/unit_temp", 1000);
ros::Publisher globe_epas_encoder_temp_pub = n.advertise<std_msgs::Float64>("report/encoder_temp", 1000);
ros::Publisher globe_epas_torque_input_pub = n.advertise<std_msgs::Float64>("report/torque_input", 1000);
ros::Publisher globe_epas_torque_output_pub = n.advertise<std_msgs::Float64>("report/torque_output", 1000);
ros::Subscriber globe_epas_set_command_mode_sub = n.subscribe("command/set_command_mode", 1000, callback_set_command_mode);
ros::Subscriber globe_epas_set_current_sub = n.subscribe("command/set_current", 1000, callback_set_current);
ros::Subscriber globe_epas_set_speed_sub = n.subscribe("command/set_speed", 1000, callback_set_speed);
ros::Subscriber globe_epas_set_position_sub = n.subscribe("command/set_position", 1000, callback_set_position);
ros::Subscriber globe_epas_set_position_with_speed_limit_sub = n.subscribe("command/set_position_with_speed_limit", 1000, callback_set_position_with_speed_limit);
ros::Subscriber globe_epas_set_inc_encoder_value_sub = n.subscribe("command/set_inc_encoder_value", 1000, callback_set_inc_encoder_value);
// ros::Rate loop_rate(10);
///////////////
// CAN setup //
///////////////
// Use sighand as our signal handler
signal(SIGALRM, sighand);
signal(SIGINT, sighand);
alarm(1);
// Allow signals to interrupt syscalls (in canReadBlock)
siginterrupt(SIGINT, 1);
// Open channels, set parameters and go on bus
h = canOpenChannel(channel, canOPEN_REQUIRE_EXTENDED);
if(h < 0) {
ROS_INFO("canOpenChannel %d failed\n", channel);
return -1;
}
canSetBusParams(h, bitrate, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
// Main loop, waiting for one of the three Globe EPAS feedback messages
while(!willExit) {
do {
ret = canReadWait(h, &id, &msg, &dlc, &flag, &t, -1);
switch (ret) {
case 0:
if(id==0x18ff0113) {
double inc_position = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])*360.0/pow(2,20);
double current = ((msg[7]<<24) + (msg[6]<<16) + (msg[5]<<8) + msg[4])/pow(2,20);
std_msgs::Float64 pub_msg;
pub_msg.data=inc_position;
globe_epas_inc_position_pub.publish(pub_msg);
pub_msg.data=current;
globe_epas_current_pub.publish(pub_msg);
} else if(id==0x18ff0213) {
double velocity = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])*360.0/pow(2,20)/gear_ratio;
double unit_temp = msg[4]-40.0;
double encoder_temp = msg[5]-40.0;
std_msgs::Float64 pub_msg;
pub_msg.data=velocity;
globe_epas_velocity_pub.publish(pub_msg);
pub_msg.data=unit_temp;
globe_epas_unit_temp_pub.publish(pub_msg);
pub_msg.data=encoder_temp;
globe_epas_encoder_temp_pub.publish(pub_msg);
} else if(id==0x18ff0313) {
double torque_input = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])/pow(2,20);
double torque_output = ((msg[7]<<24) + (msg[6]<<16) + (msg[5]<<8) + msg[4])/pow(2,20);
std_msgs::Float64 pub_msg;
pub_msg.data=torque_input;
globe_epas_torque_input_pub.publish(pub_msg);
pub_msg.data=torque_output;
globe_epas_torque_output_pub.publish(pub_msg);
}
break;
case canERR_NOMSG:
break;
default:
perror("canReadBlock error");
break;
}
globe_epas::globe_epas_cmd pub_msg;
pub_msg.command_mode = command_mode;
globe_epas_command_mode_pub.publish(pub_msg);
ros::spinOnce();
} while((ret==canOK)&&(ros::ok()));
willExit=1;
}
canClose(h);
sighand(SIGALRM);
return 0;
}
int main(int argc, char *argv[])
{
canStatus stat;
canHandle hnd;
int channelRx;
int channelTx;
if (argc != 3) {
printUsageAndExit(argv[0]);
}
{
char *endPtr = NULL;
errno = 0;
channelRx = strtol(argv[1], &endPtr, 10);
if ( (errno != 0) || ((channelRx == 0) && (endPtr == argv[1])) ) {
printUsageAndExit(argv[0]);
}
channelTx = strtol(argv[2], &endPtr, 10);
if ( (errno != 0) || ((channelTx == 0) && (endPtr == argv[2])) ) {
printUsageAndExit(argv[0]);
}
}
canInitializeLibrary();
hnd = canOpenChannel(channelRx, canOPEN_REQUIRE_EXTENDED);
if (hnd < 0) {
printf("canOpenChannel %d", channelRx);
check("", hnd);
return -1;
}
stat = canBusOff(hnd);
check("canBusOff", stat);
if (stat != canOK) {
goto ErrorExit;
}
stat = canSetNotify(hnd, callback, canNOTIFY_ERROR | canNOTIFY_STATUS, NULL);
check("canSetNotify", stat);
if (stat != canOK) {
goto ErrorExit;
}
stat = canSetBusParams(hnd, bitrate, 0, 0, 0, 0, 0);
check("canSetBusParams", stat);
if (stat != canOK) {
goto ErrorExit;
}
stat = canBusOn(hnd);
check("canBusOn", stat);
if (stat != canOK) {
goto ErrorExit;
}
incBusLoad(channelTx, canOPEN_REQUIRE_EXTENDED, 0);
testBusLoad(hnd);
incBusLoad(channelTx, canOPEN_REQUIRE_EXTENDED, 100);
testBusLoad(hnd);
testBusLoad(hnd);
incBusLoad(channelTx, canOPEN_REQUIRE_EXTENDED, 300);
testBusLoad(hnd);
testBusLoad(hnd);
ErrorExit:
stat = canBusOff(hnd);
check("canBusOff", stat);
stat = canClose(hnd);
check("canClose", stat);
stat = canUnloadLibrary();
check("canUnloadLibrary", stat);
return 0;
}
int main (int argc, char *argv[])
{
canHandle h;
int ret = -1;
long id;
unsigned char msg[8];
unsigned int dlc;
unsigned int flag;
unsigned long t;
int channel = 0;
int bitrate = BAUD_500K;
int j;
kvaser::CANPacket candat;
ros::init(argc, argv, "can_listener");
ros::NodeHandle n;
ros::Publisher can_pub = n.advertise<kvaser::CANPacket>("can_raw", 10);
errno = 0;
if (argc != 2 || (channel = atoi(argv[1]), errno) != 0) {
printf("usage %s channel\n", argv[0]);
exit(1);
} else {
printf("Reading messages on channel %d\n", channel);
}
/* Use sighand as our signal handler */
signal(SIGALRM, sighand);
signal(SIGINT, sighand);
alarm(1);
/* Allow signals to interrupt syscalls(in canReadBlock) */
siginterrupt(SIGINT, 1);
/* Open channels, parameters and go on bus */
h = canOpenChannel(channel, canOPEN_EXCLUSIVE | canOPEN_REQUIRE_EXTENDED);
if (h < 0) {
printf("canOpenChannel %d failed\n", channel);
return -1;
}
canSetBusParams(h, bitrate, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
i = 0;
// while (!willExit) {
while(ros::ok()){
do {
ret = canReadWait(h, &id, &msg, &dlc, &flag, &t, -1);
switch (ret) {
case 0:
printf("(%d) id:%ld dlc:%d data: ", i, id, dlc);
if (dlc > 8) {
dlc = 8;
}
for (j = 0; j < dlc; j++){
printf("%2.2x ", msg[j]);
candat.dat[j]=msg[j];
}
printf(" flags:0x%x time:%ld\n", flag, t);
candat.count =i;
candat.time=t;
candat.id = id;
candat.len=dlc;
candat.header.stamp=ros::Time::now();
can_pub.publish(candat);
ros::spinOnce();
i++;
if (last_time == 0) {
last_time = time(0);
} else if (time(0) > last_time) {
last_time = time(0);
if (i != last) {
printf("rx : %d total: %d\n", i - last, i);
}
last = i;
}
break;
case canERR_NOMSG:
break;
default:
perror("canReadBlock error");
break;
}
} while (ret == canOK);
willExit = 1;
}
canClose(h);
sighand(SIGALRM);
printf("Ready\n");
return 0;
}
