int NodeInit(int NodeID, int NodeType)
{
if(NodeType)
CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;
else
CANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;
/* Load can library */
LoadCanDriver(LibraryPath);
/* Define callback functions */
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;
/* Open the Peak CANOpen device */
if(!canOpen(&Board,CANOpenShellOD_Data)) return INIT_ERR;
/* Defining the node Id */
setNodeId(CANOpenShellOD_Data, NodeID);
/* Start Timer thread */
StartTimerLoop(&Init);
return 0;
}
bool QCAN::init()
{
s_BOARD Board = {(char*)port, (char*)"125K"};
/* Define callback functions */
mData->classObject = this;
mData->initialisation = &QCAN::initialisation;
mData->preOperational = &QCAN::preOperational;
mData->operational = &QCAN::operational;
mData->stopped = &QCAN::stopped;
mData->post_sync = &QCAN::post_sync;
mData->post_TPDO = &QCAN::post_TPDO;
mData->post_SlaveBootup = &QCAN::post_SlaveBootup;
mPort = canOpen(&Board, mData);
if(!mPort)
return false;
// Defining the node Id
setNodeId(mData, mNodeID);
setState(mData, Initialisation);
// masterSendNMTstateChange(mData, mNodeID, NMT_Reset_Node);
return true;
}
void InitNode(CO_Data* d, UNS32 id)
{
/* Defining the node Id */
setNodeId(&SillySlave_Data, slaveNode);
/* CAN init */
setState(&SillySlave_Data, Initialisation);
}
//*************************** INIT *****************************************
void
InitNodes (CO_Data * d, UNS32 id)
{
//****************************** INITIALISATION SLAVE *******************************
/* Defining the node Id */
setNodeId (&ObjDict_Data, node_id_ext);
/* init */
setState (&ObjDict_Data, Initialisation);
}
int main(void)
{
sys_init(); // Initialize system
canInit(CAN_BAUDRATE); // Initialize the CANopen bus
initTimer(); // Start timer for the CANopen stack
nodeID = read_bcd(); // Read node ID first
setNodeId (&ObjDict_Data, nodeID);
setState(&ObjDict_Data, Initialisation); // Init the state
for(;;) // forever loop
{
if (sys_timer) // Cycle timer, invoke action on every time slice
{
reset_sys_timer(); // Reset timer
digital_input[0] = get_inputs();
digital_input_handler(&ObjDict_Data, digital_input, sizeof(digital_input));
digital_output_handler(&ObjDict_Data, digital_output, sizeof(digital_output));
set_outputs(digital_output[0]);
// Check if CAN address has been changed
if(!( nodeID == read_bcd()))
{
nodeID = read_bcd(); // Save the new CAN adress
setState(&ObjDict_Data, Stopped); // Stop the node, to change the node ID
setNodeId(&ObjDict_Data, nodeID); // Now the CAN adress is changed
setState(&ObjDict_Data, Pre_operational); // Set to Pre_operational, master must boot it again
}
}
// a message was received pass it to the CANstack
if (canReceive(&m)) // a message reveived
canDispatch(&ObjDict_Data, &m); // process it
else
{
// Enter sleep mode
#ifdef WD_SLEEP // Watchdog and Sleep
wdt_reset();
sleep_enable();
sleep_cpu();
#endif // Watchdog and Sleep
}
}
}
/*************************** INIT *****************************************/
static void InitNodes(CO_Data* d, UNS32 id)
{
/****************************** INITIALISATION SLAVE *******************************/
if(strcmp(SlaveBoard.baudrate, "none")) {
setNodeId(&TestSlave_Data, 0x02);
setState(&TestSlave_Data, Initialisation);
}
/****************************** INITIALISATION MASTER *******************************/
if(strcmp(MasterBoard.baudrate, "none")){
RegisterSetODentryCallBack(&TestMaster_Data, 0x2000, 0, &OnMasterMap1Update);
// Defining the node Id
setNodeId(&TestMaster_Data, 0x01);
setState(&TestMaster_Data, Initialisation);
}
}
void setup()
{
Serial.begin(115200);
printf_begin();
Serial.println(F("\n\n** NeoPixelWirelessClient ** \n\n"));
Serial.println(F("Reading client configuration...."));
if( !readClientConfiguration( (client_configuration_t *)&clientConfig) )
{
Serial.println(F("** Error reading client configuration\n"));
clientConfig.version = CLIENT_CONFIG_V10;
clientConfig.nodeId = 0x01;
if( !writeClientConfiguration((client_configuration_t *)&clientConfig) )
{
Serial.println(F("** Error writing client configuration\n"));
}
else
{
Serial.println(F("Successfully wrote client configuration\n"));
}
}
else
{
Serial.println(F("Successfully Read Configuration:\n"));
}
dumpClientConfiguration((client_configuration_t *)&clientConfig);
// Setup and configure radio
radio.begin();
radio.enableAckPayload(); // enable payload ack
radio.enableDynamicPayloads(); // Ack payloads are dynamic payloads
setNodeId(clientConfig.nodeId);
// radio.openWritingPipe(addresses[1]);
// radio.openReadingPipe(1, addresses[0]);
// radio.startListening(); // we're the client, so start listening
radio.writeAckPayload(1, &message_count, sizeof(message_count));
++message_count;
radio.printDetails(); // Dump the configuration of the rf unit for debugging
delay(50);
attachInterrupt(0, check_radio, LOW); // Attach interrupt handler to interrupt #0 (using pin 2) on BOTH the sender and receiver
if (controller.initialize(50, 2) == false)
{
}
else
{
controller.fill(CRGB::Black, true);
}
}
return_value_t can_init()
{
can_state.init_return = RET_OK;
if(!canInit(1000))
{
//fail
can_state.init_return = RET_ERROR;
return can_state.init_return;
}
LED_1;
initTimer();
//initialize CanFestival
//reset callback
Sensor_Board_Data.NMT_Slave_Node_Reset_Callback = can_reset;
#ifdef CONF71
setNodeId(&Sensor_Board_Data, 0x71);
#else
setNodeId(&Sensor_Board_Data, 0x01);
#endif
can_state.is_master = 1;
setState(&Sensor_Board_Data, Initialisation); // Init the state
setState(&Sensor_Board_Data, Operational);
#ifdef CONF71
can_enable_slave_heartbeat(0x73,200);
can_enable_slave_heartbeat(0x72,200);
can_enable_heartbeat(200);
#else
can_enable_slave_heartbeat(0x04,200);
can_enable_slave_heartbeat(0x03,200);
can_enable_slave_heartbeat(0x02,200);
can_enable_heartbeat(200);
#endif
PDOInit(&Sensor_Board_Data);
return can_state.init_return;
}
void ReadsLayout::setLayoutNodeId(size_t layout, unsigned int nodeId) {
if (getNext(layout) != 0) {
cout << "void ReadsLayout::setLayoutNodeId(size_t layout, unsigned int nodeId) problem\n";
sendBugReportPlease(cerr);
}
do {
setNodeId(layout, nodeId);
layout = getPrevious(layout);
} while (layout != 0);
}
void ReadsLayout::initLayout(size_t layout, int pos, bool dir, unsigned int nodeId) {
if (getNext(layout) != 0 || getPrevious(layout) != 0) {
cout << "void ReadsLayout::initLayout(size_t layout, int pos, bool dir, unsigned int nodeId) problem\n";
cout << "layout=" << layout << " next=" << getNext(layout) << " previous=" << getPrevious(layout) << endl;
cout << "layout-1=" << layout-1 << " next=" << getNext(layout-1) << " previous=" << getPrevious(layout-1) << endl;
cout << "layout+1=" << layout+1 << " next=" << getNext(layout+1) << " previous=" << getPrevious(layout+1) << endl;
sendBugReportPlease(cerr);
}
setPosition(layout, pos, dir);
setNodeId(layout, nodeId);
}
BarGraph::BarGraph(int nodeId, QWidget *parent) :
QDeclarativeItem(NULL)
, m_customPlot(new QCustomPlot(parent))
, m_proxy(new QGraphicsProxyWidget(this))
, m_refreshButton(new QPushButton("&Stop refresh", parent))
, m_colorId(0)
, m_active(true)
{
QWidget *widget;
QVBoxLayout *vLayout;
QHBoxLayout *hLayout;
// Important, otherwise the paint method is never called
setFlag(QGraphicsItem::ItemHasNoContents, false);
setNodeId(nodeId);
widget = new QWidget(parent);
widget->setStyleSheet("background-color:#c2bbb8;");
hLayout = new QHBoxLayout(parent);
hLayout->addItem(new QSpacerItem(150, 20, QSizePolicy::Expanding, QSizePolicy::Minimum));
hLayout->addWidget(m_refreshButton);
vLayout = new QVBoxLayout(parent);
vLayout->addWidget(m_customPlot);
vLayout->addItem(hLayout);
widget->setLayout(vLayout);
m_proxy->setWidget(widget);
connect(m_refreshButton, SIGNAL(clicked()), SLOT(toggleTimer()));
m_customPlot->yAxis->setSubGrid(true);
m_customPlot->yAxis->setRange(0, 100);
m_customPlot->xAxis->setGrid(false);
m_customPlot->xAxis->setSubTickCount(0);
m_customPlot->xAxis->setTickLength(0, 10);
m_customPlot->xAxis->setTickLabelType(QCPAxis::ltDateTime);
m_customPlot->xAxis->setDateTimeFormat("hh:mm:ss");
m_customPlot->xAxis->setAutoTickStep(false);
m_customPlot->xAxis->setTickStep(10);
m_customPlot->setupFullAxesBox();
m_customPlot->legend->setVisible(true);
m_customPlot->legend->setPositionStyle(QCPLegend::psTopLeft);
m_customPlot->legend->setBrush(QBrush(QColor(255, 255, 255, 200)));
}
int main(int argc,char **argv)
{
struct sigaction act;
uint8_t nodeid = 2;
// register handler on SIGINT signal
act.sa_handler=sortie;
sigemptyset(&act.sa_mask);
act.sa_flags=0;
sigaction(SIGINT,&act,0);
// Check that we have the right command line parameters
if(argc != 3){
display_usage(argv[0]);
exit(1);
}
// get command line parameters
nodeid = strtoul(argv[2], NULL, 10);
SlaveBoard0.busname = argv[1];
printf("Starting on %s with node id = %u\n", SlaveBoard0.busname, nodeid);
// register the callbacks we use
RegisterSetODentryCallBack(&slavedic_Data, 0x2001, 0, callback_on_position);
slavedic_Data.initialisation=state_change;
slavedic_Data.preOperational=state_change;
slavedic_Data.operational=state_change;
slavedic_Data.stopped=state_change;
// Init Canfestival
if (LoadCanDriver("./libcanfestival_can_socket.so") == NULL){
printf("Unable to load driver library\n");
printf("please put file libcanfestival_can_socket.so in the current directory\n");
exit(1);
}
if(!canOpen(&SlaveBoard0,&slavedic_Data)){
printf("Cannot open can interface %s\n",SlaveBoard0.busname);
exit(1);
}
TimerInit();
setNodeId(&slavedic_Data, nodeid);
setState(&slavedic_Data, Initialisation);
printf("Canfestival initialisation done\n");
Run = 1;
while(Run)
{
sleep(1);
EnterMutex();
counter += nodeid;
LeaveMutex();
}
// Stop timer thread
StopTimerLoop(&Exit);
// Close CAN devices (and can threads)
canClose(&slavedic_Data);
return 0;
}
/**
* Handles serial commands and changing configuration
*/
void commandMode()
{
uint8_t id = 0;
// Read first character - discard since it just gets us into command mode
char c = toupper(Serial.read());
Serial.println(F("COMMAND MODE:"));
Serial.println(F("C - Change Node ID"));
Serial.println(F("D - Dump Configuration"));
Serial.println(F("E - exit"));
while(!Serial.available() ) {}
c = toupper(Serial.read());
switch( c )
{
case 'C':
Serial.print(F("** Change Node Id **\nCurrent "));
dumpClientConfiguration(&clientConfig);
Serial.print(F("\nEnter 1 digit node ID: "));
while(!Serial.available() ) {}
id = Serial.read();
Serial.println( (char)id ); // echo what the user typed
Serial.println(F("\nProcessing...\n"));
if( id >= '0' && id <= '9' )
{
// set node id
clientConfig.nodeId = id;
dumpClientConfiguration(&clientConfig);
if( writeClientConfiguration(&clientConfig) )
{
Serial.print(F("Node ID accepted: "));
Serial.println( clientConfig.nodeId, HEX);
setNodeId(id);
// radio.stopListening();
// addresses[0][0] = clientConfig.nodeId;
// radio.openReadingPipe(1, addresses[0]);
// radio.startListening(); // we're the client, so start listening
radio.printDetails(); // Dump the configuration of the rf unit for debugging
}
else
{
Serial.println(F("Error saving configuration information."));
}
}
else
{
Serial.print(F("Illegal node value entered:"));
Serial.println(id);
}
break;
case 'D':
dumpClientConfiguration(&clientConfig);
radio.printDetails();
break;
case 'E':
break;
}
}
int main(int argc, char *argv[])
{
UNS8 node_id = 0;
s_BOARD MasterBoard = {"1", "125K"};
char* dll_file_name;
/* process command line arguments */
if (argc < 2)
{
printf("USAGE: win32test <node_id> [can driver dll filename [baud rate]]\n");
return 1;
}
node_id = atoi(argv[1]);
if (node_id < 2 || node_id > 127)
{
printf("ERROR: node_id shoule be >=2 and <= 127\n");
return 1;
}
if (argc > 2)
dll_file_name = argv[2];
else
dll_file_name = "can_uvccm_win32.dll";
if (argc > 3)
MasterBoard.baudrate = argv[3];
// load can driver
if (!LoadCanDriver(dll_file_name))
{
printf("ERROR: could not load diver %s\n", dll_file_name);
return 1;
}
if (canOpen(&MasterBoard,&win32test_Data))
{
/* Defining the node Id */
setNodeId(&win32test_Data, 0x01);
/* init */
setState(&win32test_Data, Initialisation);
/****************************** START *******************************/
/* Put the master in operational mode */
setState(&win32test_Data, Operational);
/* Ask slave node to go in operational mode */
masterSendNMTstateChange (&win32test_Data, 0, NMT_Start_Node);
printf("\nStarting node %d (%xh) ...\n",(int)node_id,(int)node_id);
/* wait untill mode will switch to operational state*/
if (GetChangeStateResults(node_id, Operational, 3000) != 0xFF)
{
/* modify Client SDO 1 Parameter */
UNS32 COB_ID_Client_to_Server_Transmit_SDO = 0x600 + node_id;
UNS32 COB_ID_Server_to_Client_Receive_SDO = 0x580 + node_id;
UNS32 Node_ID_of_the_SDO_Server = node_id;
UNS8 ExpectedSize = sizeof (UNS32);
if (OD_SUCCESSFUL == writeLocalDict(&win32test_Data, 0x1280, 1, &COB_ID_Client_to_Server_Transmit_SDO, &ExpectedSize, RW)
&& OD_SUCCESSFUL == writeLocalDict(&win32test_Data, 0x1280, 2, &COB_ID_Server_to_Client_Receive_SDO, &ExpectedSize, RW)
&& OD_SUCCESSFUL == writeLocalDict(&win32test_Data, 0x1280, 3, &Node_ID_of_the_SDO_Server, &ExpectedSize, RW))
{
UNS32 dev_type = 0;
char device_name[64]="";
char hw_ver[64]="";
char sw_ver[64]="";
UNS32 vendor_id = 0;
UNS32 prod_code = 0;
UNS32 ser_num = 0;
UNS8 size;
UNS8 res;
printf("\nnode_id: %d (%xh) info\n",(int)node_id,(int)node_id);
printf("********************************************\n");
size = sizeof (dev_type);
res = ReadSDO(node_id, 0x1000, 0, uint32, &dev_type, &size);
printf("device type: %d\n",dev_type & 0xFFFF);
size = sizeof (device_name);
res = ReadSDO(node_id, 0x1008, 0, visible_string, device_name, &size);
printf("device name: %s\n",device_name);
size = sizeof (hw_ver);
res = ReadSDO(node_id, 0x1009, 0, visible_string, hw_ver, &size);
printf("HW version: %s\n",hw_ver);
size = sizeof (sw_ver);
res = ReadSDO(node_id, 0x100A, 0, visible_string, sw_ver, &size);
printf("SW version: %s\n",sw_ver);
size = sizeof (vendor_id);
res = ReadSDO(node_id, 0x1018, 1, uint32, &vendor_id, &size);
printf("vendor id: %d\n",vendor_id);
size = sizeof (prod_code);
res = ReadSDO(node_id, 0x1018, 2, uint32, &prod_code, &size);
printf("product code: %d\n",prod_code);
size = sizeof (ser_num);
res = ReadSDO(node_id, 0x1018, 4, uint32, &ser_num, &size);
printf("serial number: %d\n",ser_num);
printf("********************************************\n");
}
else
{
printf("ERROR: Object dictionary access failed\n");
}
}
else
{
printf("ERROR: node_id %d (%xh) is not responding\n",(int)node_id,(int)node_id);
}
masterSendNMTstateChange (&win32test_Data, 0x02, NMT_Stop_Node);
setState(&win32test_Data, Stopped);
canClose(&win32test_Data);
}
return 0;
}
void InitNodes(CO_Data* d, UNS32 id)
{
setNodeId(&master_Data,0x7f);
setState(&master_Data, Initialisation);
setState(&master_Data, Operational);
}
void CommanderCallBack(super_modified_servo::commanderConfig &config, uint32_t level)
{
if (config.command == "search")
{
for (motor_id = 4; motor_id < MaxNumSMS; motor_id++)
{
start(fd, motor_id);
sleep(1);
printf("Start Node ID %d\n", motor_id);
if (getCommunicationSuccess())
printf("ID is %d\n", motor_id);
resetErrors(fd, motor_id);
sleep(1);
stop(fd, motor_id);
printf("Stop Node ID %d\n", motor_id);
}
}
if (config.command == "start")
{
printf("[INFO] Start\n");
start(fd, motor_id);
start_flag = true;
}
if (config.command == "stop")
{
printf("[INFO] Stop\n");
stop(fd, motor_id);
start_flag = false;
}
if (config.command == "reset")
{
printf("[INFO] Reset\n");
stop(fd, motor_id);
sleep(1);
start(fd, motor_id);
}
if (config.command == "setID")
{
printf("[INFO] Set ID: %d\n", new_motor_id);
setNodeId(fd, motor_id, new_motor_id);
printf("[INFO] Disconnect the servo\n");
}
if (config.command == "setBaud")
{
printf("[INFO] Set baud rate: %d\n", baudRate);
setBaudRate(fd, motor_id, baudRate);
printf("[INFO] Disconnect the servo\n");
printf("[INFO] Change the bad rate in serialPortOpen function");
}
if (config.command == "error_reaction")
{
printf("[INFO] Error reaction\n");
uint8_t resp[20];
uint8_t errorReaction[20] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x02,
0x02, 0x01};
setErrorReaction(fd, motor_id, errorReaction);
sleep(3);
if (getCommunicationSuccess() == false)
{
resetErrors(fd, motor_id);;
printf("[ERROR]: %d \n", getWarning());
}
getErrorReaction(fd, motor_id, resp);
for (int i=0; i<20; i++)
printf("D[%d]: %d\n", i, resp[i]);
}
if (config.command == "setGain")
{
printf("[INFO] Set Gain: \n");
setPGain(fd, motor_id, kp);
setIGain(fd, motor_id, ki);
setDGain(fd, motor_id, kd);
printf("[INFO] kp %d \n", getPGain(fd, motor_id));
printf("[INFO] ki %d \n", getIGain(fd, motor_id));
printf("[INFO] kd %d \n", getDGain(fd, motor_id));
}
if (config.command == "getGain")
{
printf("[INFO] kp %d \n", getPGain(fd, motor_id));
printf("[INFO] ki %d \n", getIGain(fd, motor_id));
printf("[INFO] kd %d \n", getDGain(fd, motor_id));
}
motor_id = config.motor_id;
new_motor_id = config.new_motor_id;
baudRate = config.baudRate;
kp = config.p_gain;
ki = config.i_gain;
kd = config.d_gain;
set_point = config.set_point;
}
/*!
**
**
** @param d
** @param m
**
** @return
**/
UNS8 proceedLSS_Slave(CO_Data* d, Message* m )
{
UNS8 msg_cs;
MSG_WAR(0x3D21, "SlaveLSS proceedLSS; command ", m->data[0]);
switch(msg_cs=m->data[0]){
case LSS_SM_GLOBAL: /* Switch Mode Global */
/* if there is not a mode change break*/
if(m->data[1] == d->lss_transfer.mode){
MSG_WAR(0x3D22, "SlaveLSS already in the mode ", m->data[1]);
break;
}
if(m->data[1]==LSS_CONFIGURATION_MODE) {
MSG_WAR(0x3D23, "SlaveLSS switching to configuration mode ", 0);
/* Store the NodeId in case it will be changed */
//d->lss_transfer.nodeID=getNodeId(d);
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
}
else if(m->data[1]==LSS_WAITING_MODE){
MSG_WAR(0x3D24, "SlaveLSS switching to operational mode ", 0);
/* If the nodeID has changed update it and put the node state to Initialisation. */
if(d->lss_transfer.nodeID!=getNodeId(d)){
if(getNodeId(d)==0xFF){/* The nodeID was 0xFF; initialize the application*/
MSG_WAR(0x3D25, "The node Id has changed. Reseting to Initialisation state",0);
setNodeId(d, d->lss_transfer.nodeID);
setState(d, Initialisation);
}
else{/* The nodeID will be changed on NMT_Reset_Comunication Request*/
}
}
d->lss_transfer.mode=LSS_WAITING_MODE;
}
break;
case LSS_CONF_NODE_ID: /* Configure Node-ID */
{
UNS8 error_code=0;
UNS8 spec_error=0;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
if(m->data[1]>127 && m->data[1]!=0xFF){
MSG_ERR(0x1D26, "NodeID out of range",0);
error_code=1; /* NodeID out of range */
}
else{
d->lss_transfer.nodeID=m->data[1];
}
}
else{
MSG_WAR(0x3D27, "SlaveLSS not in configuration mode",0);
//error_code=0xFF;
break;
}
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
}
break;
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
{
UNS8 error_code=0;
UNS8 spec_error=0;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
/* If a baud rate is not supported just comment the line. */
switch(m->data[2]){
case 0x00:d->lss_transfer.baudRate="1M";break;
case 0x01:d->lss_transfer.baudRate="800K";break;
case 0x02:d->lss_transfer.baudRate="500K";break;
case 0x03:d->lss_transfer.baudRate="250K";break;
case 0x04:d->lss_transfer.baudRate="125K";break;
case 0x05:d->lss_transfer.baudRate="100K";break;
case 0x06:d->lss_transfer.baudRate="50K";break;
case 0x07:d->lss_transfer.baudRate="20K";break;
case 0x08:d->lss_transfer.baudRate="10K";break;
default:
MSG_ERR(0x1D28, "Baud rate not supported",0);
error_code=0xFF; /* Baud rate not supported*/
break;
}
}
else{
MSG_WAR(0x3D2A, "SlaveLSS not in configuration mode",0);
//error_code=0xFF;
break;
}
/* if bit timing is not supported comment the previous code and uncomment the following */
/*{
MSG_ERR(0x1D29, "Bit timing not supported",0);
error_code=0x01; // bit timing not supported
}*/
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
}
break;
case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */
if(d->lss_transfer.mode!=LSS_CONFIGURATION_MODE){
MSG_ERR(0x3D2B, "SlaveLSS not in configuration mode",0);
break;
}
if(d->lss_transfer.baudRate!="none"){
d->lss_transfer.switchDelay=getLSSDelay(m);
MSG_WAR(0x3D2C, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay);
d->lss_transfer.switchDelayState=SDELAY_FIRST;
//d->lss_transfer.currentState=getState(d);
//setState(d, LssTimingDelay);
d->lss_transfer.canHandle_t=d->canHandle;
d->canHandle=NULL;
StartLSS_SDELAY_TIMER();
}
break;
case LSS_CONF_STORE: /* Store Configured Parameters */
{
UNS8 error_code=0;
UNS8 spec_error=0;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
if(d->lss_StoreConfiguration){
/* call lss_StoreConfiguration with NodeId */
(*d->lss_StoreConfiguration)(d,&error_code,&spec_error);
}
else{
MSG_ERR(0x1D2E, "Store configuration not supported",0);
error_code=1; /* store configuration is not supported */
}
}
else{
MSG_WAR(0x3D2F, "SlaveLSS not in configuration mode",0);
//error_code=0xFF;
break;
}
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
}
break;
case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */
case LSS_SM_SELECTIVE_PRODUCT:
case LSS_SM_SELECTIVE_REVISION:
case LSS_SM_SELECTIVE_SERIAL:
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
UNS32 _SpecificNodeInfo;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
{
MSG_ERR(0x1D30, "Switch Mode Selective only supported in operational mode",0);
break;
}
_SpecificNodeInfo=getLSSIdent(m);
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
if(_SpecificNodeInfo==*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_SM_SELECTIVE_VENDOR-1)].pObject){
d->lss_transfer.addr_sel_match|=(0x01<<(msg_cs-LSS_SM_SELECTIVE_VENDOR));
/* If all the fields has been set */
if(d->lss_transfer.addr_sel_match==0x0F){
MSG_WAR(0x3D31, "SlaveLSS switching to configuration mode ", 0);
d->lss_transfer.addr_sel_match=0;
d->lss_transfer.nodeID=getNodeId(d);
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
sendSlaveLSSMessage(d,LSS_SM_SELECTIVE_RESP,0,0);
}
}
else {
MSG_WAR(0x3D32, "LSS identity field doesn't match ", _SpecificNodeInfo);
d->lss_transfer.addr_sel_match=0;
}
}
break;
case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */
case LSS_IDENT_REMOTE_PRODUCT:
case LSS_IDENT_REMOTE_REV_LOW:
case LSS_IDENT_REMOTE_REV_HIGH:
case LSS_IDENT_REMOTE_SERIAL_LOW:
case LSS_IDENT_REMOTE_SERIAL_HIGH:
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
UNS32 _SpecificNodeInfo;
_SpecificNodeInfo=getLSSIdent(m);
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
/* Check if the data match the identity object. */
switch(msg_cs){
case LSS_IDENT_REMOTE_VENDOR:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[1].pObject)? d->lss_transfer.addr_ident_match|0x01:0; break;
case LSS_IDENT_REMOTE_PRODUCT:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[2].pObject)? d->lss_transfer.addr_ident_match|0x02:0; break;
case LSS_IDENT_REMOTE_REV_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x04:0; break;
case LSS_IDENT_REMOTE_REV_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x08:0; break;
case LSS_IDENT_REMOTE_SERIAL_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x10:0; break;
case LSS_IDENT_REMOTE_SERIAL_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x20:0; break;
}
/* If all the fields has been set.. */
if(d->lss_transfer.addr_ident_match==0x3F){
MSG_WAR(0x3D33, "SlaveLSS identified ", 0);
d->lss_transfer.addr_ident_match=0;
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
else if(d->lss_transfer.addr_ident_match==0){
MSG_WAR(0x3D34, "LSS identify field doesn't match ", _SpecificNodeInfo);
}
}
break;
case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */
{
if(getNodeId(d)==0xFF){
MSG_WAR(0x3D35, "SlaveLSS non-configured ", 0);
sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0);
}
else{
MSG_WAR(0x3D36, "SlaveLSS already configured ", 0);
}
}
break;
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
UNS32 _SpecificNodeInfo;
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
_SpecificNodeInfo=*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_INQ_VENDOR_ID-1)].pObject;
MSG_WAR(0x3D37, "SlaveLSS identity field inquired ", _SpecificNodeInfo);
sendSlaveLSSMessage(d,msg_cs,&_SpecificNodeInfo,0);
}
break;
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
{
UNS8 NodeID;
NodeID=getNodeId(d);
MSG_WAR(0x3D38, "SlaveLSS Node ID inquired ", NodeID);
sendSlaveLSSMessage(d,msg_cs,&NodeID,0);
}
else{
MSG_WAR(0x3D39, "SlaveLSS not in configuration mode",0);
}
break;
#ifdef CO_ENABLE_LSS_FS
case LSS_IDENT_FASTSCAN:
{
/* If the nodeID isn't 0xFF the slave shall not participate */
if(getNodeId(d)!=0xFF)break;
if(getLSSBitCheck(m)==128)d->lss_transfer.FastScan_SM=LSS_FS_RESET;
switch(d->lss_transfer.FastScan_SM){
case LSS_FS_RESET:
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
MSG_WAR(0x3D3A, "SlaveLSS Reseting LSSPos", 0);
d->lss_transfer.LSSPos=0;
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject;
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
break;
case LSS_FS_PROCESSING:/*if(getLSSBitCheck(m)<32)*/
if(d->lss_transfer.LSSPos==getLSSSub(m))
{
UNS32 Mask=0xFFFFFFFF<<getLSSBitCheck(m);
MSG_WAR(0x3D3B, "SlaveLSS FastScan IDNumber", getLSSIdent(m));
MSG_WAR(0x3D3C, "SlaveLSS FastScan BitMask ", Mask);
MSG_WAR(0x3D3D, "SlaveLSS FastScan LSS-ID ", d->lss_transfer.IDNumber);
if((getLSSIdent(m) & Mask)==(d->lss_transfer.IDNumber & Mask))
{
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
if(getLSSBitCheck(m)==0)
{
d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION;
}
}
break;
case LSS_FS_CONFIRMATION:
if(d->lss_transfer.LSSPos==getLSSSub(m))
{
if(getLSSIdent(m)==d->lss_transfer.IDNumber)
{
/* Current LSS-ID[sub] confirmed correctly */
MSG_WAR(0x3D3E, "SlaveLSS FastScan IDNumber and LSS-ID match=>", d->lss_transfer.IDNumber);
if(d->lss_transfer.LSSPos==3)
{
/* All LSS-ID[sub] identified correctly, switching to configuration mode */
MSG_WAR(0x3D3F, "SlaveLSS switching to configuration mode ", 0);
d->lss_transfer.nodeID=getNodeId(d);
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
d->lss_transfer.FastScan_SM=LSS_FS_RESET;
d->lss_transfer.LSSPos=0xFF;
}
else
{
/* Switch to the next LSS-ID[sub] */
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
d->lss_transfer.LSSPos=getLSSNext(m);
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject;
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
}
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
}
break;
}
}
break;
#endif
default:
MSG_ERR(0x1D40, "SlaveLSS command not implemented", msg_cs);
return 0xFF;
}
return 0;
}
