nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

Merge pull request #4 from jensnielsen/multipleSDO

Browse source 
Allow multiple SDO objects
This commit is contained in:
CANopenNode 2015-12-13 22:26:35 +01:00
commit 7c95d6e821
4 changed files with 79 additions and 44 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

92
CANopen.c
View file

@ -67,7 +67,7 @@
#if CO_NO_NMT_MASTER > 1 \
|| CO_NO_SYNC != 1 \
|| CO_NO_EMERGENCY != 1 \
|| CO_NO_SDO_SERVER != 1 \
|| CO_NO_SDO_SERVER == 0 \
|| (CO_NO_SDO_CLIENT != 0 && CO_NO_SDO_CLIENT != 1) \
|| (CO_NO_RPDO < 1 || CO_NO_RPDO > 0x200) \
|| (CO_NO_TPDO < 1 || CO_NO_TPDO > 0x200) \
@ -108,7 +108,7 @@
static CO_CANmodule_t COO_CANmodule;
static CO_CANrx_t COO_CANmodule_rxArray0[CO_RXCAN_NO_MSGS];
static CO_CANtx_t COO_CANmodule_txArray0[CO_TXCAN_NO_MSGS];
static CO_SDO_t COO_SDO;
static CO_SDO_t COO_SDO[CO_NO_SDO_SERVER];
static CO_OD_extension_t COO_SDO_ODExtensions[CO_OD_NoOfElements];
static CO_EM_t COO_EM;
static CO_EMpr_t COO_EMpr;
@ -173,10 +173,11 @@ CO_ReturnError_t CO_init(
#ifdef CO_USE_GLOBALS
CO = &COO;
CO->CANmodule[0] = &COO_CANmodule[0];
CO->CANmodule[0] = &COO_CANmodule;
CO_CANmodule_rxArray0 = &COO_CANmodule_rxArray0[0];
CO_CANmodule_txArray0 = &COO_CANmodule_txArray0[0];
CO->SDO = &COO_SDO;
for(i=0; i<CO_NO_SDO_SERVER; i++)
CO->SDO[i] = &COO_SDO[i];
CO_SDO_ODExtensions = &COO_SDO_ODExtensions[0];
CO->em = &COO_EM;
CO->emPr = &COO_EMpr;
@ -198,7 +199,9 @@ CO_ReturnError_t CO_init(
CO->CANmodule[0] = (CO_CANmodule_t *) calloc(1, sizeof(CO_CANmodule_t));
CO_CANmodule_rxArray0 = (CO_CANrx_t *) calloc(CO_RXCAN_NO_MSGS, sizeof(CO_CANrx_t));
CO_CANmodule_txArray0 = (CO_CANtx_t *) calloc(CO_TXCAN_NO_MSGS, sizeof(CO_CANtx_t));
CO->SDO = (CO_SDO_t *) calloc(1, sizeof(CO_SDO_t));
for(i=0; i<CO_NO_SDO_SERVER; i++){
CO->SDO[i] = (CO_SDO_t *) calloc(1, sizeof(CO_SDO_t));
}
CO_SDO_ODExtensions = (CO_OD_extension_t*) calloc(CO_OD_NoOfElements, sizeof(CO_OD_extension_t));
CO->em = (CO_EM_t *) calloc(1, sizeof(CO_EM_t));
CO->emPr = (CO_EMpr_t *) calloc(1, sizeof(CO_EMpr_t));
@ -220,7 +223,7 @@ CO_ReturnError_t CO_init(
CO_memoryUsed = sizeof(CO_CANmodule_t)
+ sizeof(CO_CANrx_t) * CO_RXCAN_NO_MSGS
+ sizeof(CO_CANtx_t) * CO_TXCAN_NO_MSGS
+ sizeof(CO_SDO_t)
+ sizeof(CO_SDO_t) * CO_NO_SDO_SERVER
+ sizeof(CO_OD_extension_t) * CO_OD_NoOfElements
+ sizeof(CO_EM_t)
+ sizeof(CO_EMpr_t)
@ -239,7 +242,9 @@ CO_ReturnError_t CO_init(
if(CO->CANmodule[0] == NULL) errCnt++;
if(CO_CANmodule_rxArray0 == NULL) errCnt++;
if(CO_CANmodule_txArray0 == NULL) errCnt++;
if(CO->SDO == NULL) errCnt++;
for(i=0; i<CO_NO_SDO_SERVER; i++){
if(CO->SDO[i] == NULL) errCnt++;
}
if(CO_SDO_ODExtensions == NULL) errCnt++;
if(CO->em == NULL) errCnt++;
if(CO->emPr == NULL) errCnt++;
@ -289,21 +294,34 @@ CO_ReturnError_t CO_init(
if(err){CO_delete(CANbaseAddress); return err;}
for (i=0; i<CO_NO_SDO_SERVER; i++)
{
uint32_t COB_IDClientToServer;
uint32_t COB_IDServerToClient;
if(i==0){
/*Default SDO server must be located at first index*/
COB_IDClientToServer = CO_CAN_ID_RSDO + nodeId;
COB_IDServerToClient = CO_CAN_ID_TSDO + nodeId;
}else{
COB_IDClientToServer = OD_SDOServerParameter[i].COB_IDClientToServer;
COB_IDServerToClient = OD_SDOServerParameter[i].COB_IDServerToClient;
}
err = CO_SDO_init(
CO->SDO,
CO_CAN_ID_RSDO + nodeId,
CO_CAN_ID_TSDO + nodeId,
OD_H1200_SDO_SERVER_PARAM,
0,
&CO_OD[0],
CO_OD_NoOfElements,
CO_SDO_ODExtensions,
nodeId,
CO->CANmodule[0],
CO_RXCAN_SDO_SRV,
CO->CANmodule[0],
CO_TXCAN_SDO_SRV);
err = CO_SDO_init(
CO->SDO[i],
COB_IDClientToServer,
COB_IDServerToClient,
OD_H1200_SDO_SERVER_PARAM+i,
i==0 ? 0 : CO->SDO[0],
&CO_OD[0],
CO_OD_NoOfElements,
CO_SDO_ODExtensions,
nodeId,
CO->CANmodule[0],
CO_RXCAN_SDO_SRV+i,
CO->CANmodule[0],
CO_TXCAN_SDO_SRV+i);
}
if(err){CO_delete(CANbaseAddress); return err;}
@ -311,7 +329,7 @@ CO_ReturnError_t CO_init(
err = CO_EM_init(
CO->em,
CO->emPr,
CO->SDO,
CO->SDO[0],
&OD_errorStatusBits[0],
ODL_errorStatusBits_stringLength,
&OD_errorRegister,
@ -353,7 +371,7 @@ CO_ReturnError_t CO_init(
err = CO_SYNC_init(
CO->SYNC,
CO->em,
CO->SDO,
CO->SDO[0],
&CO->NMT->operatingState,
OD_COB_ID_SYNCMessage,
OD_communicationCyclePeriod,
@ -373,7 +391,7 @@ CO_ReturnError_t CO_init(
err = CO_RPDO_init(
CO->RPDO[i],
CO->em,
CO->SDO,
CO->SDO[0],
&CO->NMT->operatingState,
nodeId,
((i<4) ? (CO_CAN_ID_RPDO_1+i*0x100) : 0),
@ -393,7 +411,7 @@ CO_ReturnError_t CO_init(
err = CO_TPDO_init(
CO->TPDO[i],
CO->em,
CO->SDO,
CO->SDO[0],
&CO->NMT->operatingState,
nodeId,
((i<4) ? (CO_CAN_ID_TPDO_1+i*0x100) : 0),
@ -412,7 +430,7 @@ CO_ReturnError_t CO_init(
err = CO_HBconsumer_init(
CO->HBcons,
CO->em,
CO->SDO,
CO->SDO[0],
&OD_consumerHeartbeatTime[0],
CO_HBcons_monitoredNodes,
CO_NO_HB_CONS,
@ -425,7 +443,7 @@ CO_ReturnError_t CO_init(
#if CO_NO_SDO_CLIENT == 1
err = CO_SDOclient_init(
CO->SDOclient,
CO->SDO,
CO->SDO[0],
(CO_SDOclientPar_t*) &OD_SDOClientParameter[0],
CO->CANmodule[0],
CO_RXCAN_SDO_CLI,
@ -466,7 +484,9 @@ void CO_delete(int32_t CANbaseAddress){
free(CO->emPr);
free(CO->em);
free(CO_SDO_ODExtensions);
free(CO->SDO);
for(i=0; i<CO_NO_SDO_SERVER; i++){
free(CO->SDO[i]);
}
free(CO_CANmodule_txArray0);
free(CO_CANmodule_rxArray0);
free(CO->CANmodule[0]);
@ -481,6 +501,7 @@ CO_NMT_reset_cmd_t CO_process(
uint16_t timeDifference_ms,
uint16_t *timerNext_ms)
{
uint8_t i;
bool_t NMTisPreOrOperational = false;
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
static uint16_t ms50 = 0;
@ -500,13 +521,14 @@ CO_NMT_reset_cmd_t CO_process(
}
CO_SDO_process(
CO->SDO,
NMTisPreOrOperational,
timeDifference_ms,
1000,
timerNext_ms);
for(i=0; i<CO_NO_SDO_SERVER; i++){
CO_SDO_process(
CO->SDO[i],
NMTisPreOrOperational,
timeDifference_ms,
1000,
timerNext_ms);
}
CO_EM_process(
CO->emPr,

2
CANopen.h
View file

@ -96,7 +96,7 @@ typedef enum{
*/
typedef struct{
CO_CANmodule_t *CANmodule[1]; /**< CAN module objects */
CO_SDO_t *SDO; /**< SDO object */
CO_SDO_t *SDO[CO_NO_SDO_SERVER]; /**< SDO object */
CO_EM_t *em; /**< Emergency report object */
CO_EMpr_t *emPr; /**< Emergency process object */
CO_NMT_t *NMT; /**< NMT object */

16
stack/CO_SDO.c
View file

@ -190,7 +190,8 @@ static void CO_SDO_receive(void *object, const CO_CANrxMsg_t *msg){
/*
* Function for accessing _SDO server parameter_ (index 0x1200+) from SDO server.
* Function for accessing _SDO server parameter_ for default SDO (index 0x1200)
* from SDO server.
*
* For more information see file CO_SDO.h.
*/
@ -215,8 +216,8 @@ static CO_SDO_abortCode_t CO_ODF_1200(CO_ODF_arg_t *ODF_arg){
/******************************************************************************/
CO_ReturnError_t CO_SDO_init(
CO_SDO_t *SDO,
uint16_t COB_IDClientToServer,
uint16_t COB_IDServerToClient,
uint32_t COB_IDClientToServer,
uint32_t COB_IDServerToClient,
uint16_t ObjDictIndex_SDOServerParameter,
CO_SDO_t *parentSDO,
const CO_OD_entry_t OD[],
@ -269,6 +270,11 @@ CO_ReturnError_t CO_SDO_init(
CO_OD_configure(SDO, ObjDictIndex_SDOServerParameter, CO_ODF_1200, (void*)&SDO->nodeId, 0U, 0U);
}
if((COB_IDClientToServer & 0x80000000) != 0 || (COB_IDServerToClient & 0x80000000) != 0 ){
// SDO is invalid
COB_IDClientToServer = 0;
COB_IDServerToClient = 0;
}
/* configure SDO server CAN reception */
CO_CANrxBufferInit(
CANdevRx, /* CAN device */
@ -519,6 +525,8 @@ uint32_t CO_SDO_initTransfer(CO_SDO_t *SDO, uint16_t index, uint8_t subIndex){
/* indicate total data length, if not domain */
SDO->ODF_arg.dataLengthTotal = (SDO->ODF_arg.ODdataStorage) ? SDO->ODF_arg.dataLength : 0U;
SDO->ODF_arg.offset = 0U;
/* verify length */
if(SDO->ODF_arg.dataLength > CO_SDO_BUFFER_SIZE){
return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
@ -570,6 +578,7 @@ uint32_t CO_SDO_readOD(CO_SDO_t *SDO, uint16_t SDOBufferSize){
return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
}
}
SDO->ODF_arg.offset += SDO->ODF_arg.dataLength;
SDO->ODF_arg.firstSegment = false;
/* swap data if processor is not little endian (CANopen is) */
@ -640,6 +649,7 @@ uint32_t CO_SDO_writeOD(CO_SDO_t *SDO, uint16_t length){
}
}
}
SDO->ODF_arg.offset += SDO->ODF_arg.dataLength;
SDO->ODF_arg.firstSegment = false;
/* copy data from SDO buffer to OD if not domain */

13
stack/CO_SDO.h
View file

@ -533,6 +533,9 @@ typedef struct{
is not necessary to specify this variable. By download this variable contains
total data size, if size is indicated in SDO download initiate phase */
uint32_t dataLengthTotal;
/** Used by domain data type. In case of multiple segments, this indicates the offset
into the buffer this segment starts at. */
uint32_t offset;
}CO_ODF_arg_t;
@ -690,8 +693,8 @@ void CO_memcpySwap4(uint8_t dest[], const uint8_t src[]);
* Function must be called in the communication reset section.
*
* @param SDO This object will be initialized.
* @param COB_IDClientToServer 0x600 + nodeId by default.
* @param COB_IDServerToClient 0x580 + nodeId by default.
* @param COB_IDClientToServer COB ID for client to server for this SDO object.
* @param COB_IDServerToClient COB ID for server to client for this SDO object.
* @param ObjDictIndex_SDOServerParameter Index in Object dictionary.
* @param parentSDO Pointer to SDO object, which contains object dictionary and
* its extension. For first (default) SDO object this argument must be NULL.
@ -701,7 +704,7 @@ void CO_memcpySwap4(uint8_t dest[], const uint8_t src[]);
* @param ODSize Size of the above array.
* @param ODExtensions Pointer to the externally defined array of the same size
* as ODSize.
* @param nodeId CANopen Node ID of this device. Value will be added to COB_IDs.
* @param nodeId CANopen Node ID of this device.
* @param CANdevRx CAN device for SDO server reception.
* @param CANdevRxIdx Index of receive buffer in the above CAN device.
* @param CANdevTx CAN device for SDO server transmission.
@ -711,8 +714,8 @@ void CO_memcpySwap4(uint8_t dest[], const uint8_t src[]);
*/
CO_ReturnError_t CO_SDO_init(
CO_SDO_t *SDO,
uint16_t COB_IDClientToServer,
uint16_t COB_IDServerToClient,
uint32_t COB_IDClientToServer,
uint32_t COB_IDServerToClient,
uint16_t ObjDictIndex_SDOServerParameter,
CO_SDO_t *parentSDO,
const CO_OD_entry_t OD[],