diff --git a/include/core/io/CANOpenMacros.hpp b/include/core/io/CANOpenMacros.hpp index 8298b50c..44b598b2 100644 --- a/include/core/io/CANOpenMacros.hpp +++ b/include/core/io/CANOpenMacros.hpp @@ -131,6 +131,36 @@ .Data = (CO_DATA) CO_COBID_SDO_RESPONSE(), \ } +/** + * This macro helps create an SDO configuration range of the object dictionary. + * This lives at the key 0x1280 with 4 sub indices. + */ +#define SDO_CONFIGURATION_1280(SERVER_NODE_ID) \ + { \ + /* Communication Object SDO Server */ \ + .Key = CO_KEY(0x1280, 0x00, CO_OBJ_D___R_), \ + .Type = CO_TUNSIGNED32, \ + .Data = (CO_DATA) 0x03, \ + }, \ + { \ + /* SDO Server Request COBID */ \ + .Key = CO_KEY(0x1280, 0x01, CO_OBJ_D___R_), \ + .Type = CO_TUNSIGNED32, \ + .Data = (CO_DATA) CO_COBID_SDO_REQUEST(), \ + }, \ + { \ + /* SDO Server Response COBID */ \ + .Key = CO_KEY(0x1280, 0x02, CO_OBJ_D___R_), \ + .Type = CO_TUNSIGNED32, \ + .Data = (CO_DATA) CO_COBID_SDO_RESPONSE(), \ + }, \ + { \ + /* Node-ID of the SDO server */ \ + .Key = CO_KEY(0x1280, 0x03, CO_OBJ_D___R_), \ + .Type = CO_TUNSIGNED8, \ + .Data = (CO_DATA) SERVER_NODE_ID, \ + } + /** * This macro creates an RPDO settings object. This macro itself is abstract, * allowing it to be used with any RPDO number supported by CANOpen. To make diff --git a/include/core/io/CANopen.hpp b/include/core/io/CANopen.hpp index eec4aca8..5b010575 100644 --- a/include/core/io/CANopen.hpp +++ b/include/core/io/CANopen.hpp @@ -22,13 +22,15 @@ namespace core::io { +typedef void (*csdo_callback_t)(CO_CSDO* csdo, uint32_t entry, uint32_t status, void* context); + /** * Get an instance of the CAN driver that can be used with the CANopen * stack. This will populate a struct with function pointers that can * handle CAN operations. * * @param[in] can The CAN interface that will be used for the stack driver - * @param[in] messageQueue Queue that will be read from for receiveing CAN messages + * @param[in] messageQueue Queue that will be read from for receiving CAN messages * @param[out] canDriver The CANopen stack driver to populate */ void getCANopenCANDriver(CAN* can, types::FixedQueue* messageQueue, @@ -94,6 +96,59 @@ void initializeCANopenNode(CO_NODE* canNode, CANDevice* canDevice, CO_IF_DRV* ca */ void processCANopenNode(CO_NODE* canNode); +/** + * This function sets up and starts an SDO download (write) request to transfer data + * to the specified object dictionary entry on the target CANopen node. + * + * @param node[in] Reference to the CANopen node object + * @param data[in] Pointer to the data buffer that holds the data to send + * @param size[in] Size of the data to transfer in bytes + * @param entry[in] Object dictionary entry (index + subindex) to write to + * @param transferCallback[in] Callback function for the transfer operation + * @param transferContext[in] Context for the callback function + * @return CO_ERR[out] Returns the result of the transfer operation + */ +CO_ERR SDOTransfer(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry, csdo_callback_t transferCallback, + void* transferContext); + +/** + * This function starts an SDO upload (read) request to fetch data from the specified + * object dictionary entry on the target CANopen node + * + * @param node[in] Reference to the CANopen node object + * @param data[in] Pointer to the buffer where received data will be stored + * @param size[in] Size of the buffer provided to receive data + * @param entry[in] Object dictionary entry (index + subindex) to read from + * @param receiveCallback[in] Callback function for the receive operation + * @param receiveContext[in] Context for the callback function + * @return CO_ERR[out] Returns the result of the receive operation + */ +CO_ERR SDOReceive(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry, csdo_callback_t receiveCallback, + void* receiveContext); + +/** + * This function sets up and starts an SDO download (write) request to transfer data + * to the specified object dictionary entry on the target CANopen node. + * + * @param node[in] Reference to the CANopen node object + * @param data[in] Pointer to the data buffer that holds the data to send + * @param size[in] Size of the data to transfer in bytes + * @param entry[in] Object dictionary entry (index + subindex) to write to + * @return CO_ERR[out] Returns the result of the transfer operation + */ +CO_ERR SDOTransferBlocking(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry); + +/** + * This function starts an SDO upload (read) request to fetch data from the specified + * object dictionary entry on the target CANopen node + * + * @param node[in] Reference to the CANopen node object + * @param data[in] Pointer to the buffer where received data will be stored + * @param size[in] Size of the buffer provided to receive data + * @param entry[in] Object dictionary entry (index + subindex) to read from + * @return CO_ERR[out] Returns the result of the receive operation + */ +CO_ERR SDOReceiveBlocking(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry); } // namespace core::io #endif diff --git a/samples/canopen/CMakeLists.txt b/samples/canopen/CMakeLists.txt index 83f08ce6..c5cfae44 100644 --- a/samples/canopen/CMakeLists.txt +++ b/samples/canopen/CMakeLists.txt @@ -1,3 +1,4 @@ add_subdirectory(canopen_rpdo) add_subdirectory(canopen_sample) +add_subdirectory(canopen_sdo) add_subdirectory(canopen_tpdo) diff --git a/samples/canopen/canopen_rpdo/main.cpp b/samples/canopen/canopen_rpdo/main.cpp index b7d8412b..9fd59e86 100644 --- a/samples/canopen/canopen_rpdo/main.cpp +++ b/samples/canopen/canopen_rpdo/main.cpp @@ -9,8 +9,11 @@ #include #include #include +#include #include #include +#include +#include #include @@ -19,6 +22,7 @@ namespace io = core::io; namespace dev = core::dev; namespace time = core::time; +namespace log = core::log; /////////////////////////////////////////////////////////////////////////////// // EVT-core CAN callback and CAN setup. This will include logic to set @@ -41,18 +45,29 @@ io::UART* uart; // create a can interrupt handler void canInterrupt(io::CANMessage& message, void* priv) { auto* queue = (core::types::FixedQueue*) priv; + char messageString[50]; +#ifdef EVT_CORE_LOG_ENABLE // print out raw received data - uart->printf("Got RAW message from %X of length %d with data: ", message.getId(), message.getDataLength()); + snprintf(&messageString[0], + 25, + "Got RAW message from %X of length %d with data: ", + message.getId(), + message.getDataLength()); +#endif + uint8_t* data = message.getPayload(); + for (int i = 0; i < message.getDataLength(); i++) { - uart->printf("%X ", *data); + snprintf(&messageString[strlen(messageString)], 1, "%X ", *data); data++; } - uart->printf("\r\n"); - if (queue != nullptr) + log::LOGGER.log(log::Logger::LogLevel::INFO, "\r\n\t%s\r\n", messageString); + + if (queue != nullptr) { queue->append(message); + } } int main() { @@ -60,6 +75,7 @@ int main() { core::platform::init(); uart = &io::getUART(9600); + // Initialize the timer dev::Timer& timer = dev::getTimer(100); diff --git a/samples/canopen/canopen_sdo/CMakeLists.txt b/samples/canopen/canopen_sdo/CMakeLists.txt new file mode 100644 index 00000000..a702b201 --- /dev/null +++ b/samples/canopen/canopen_sdo/CMakeLists.txt @@ -0,0 +1,4 @@ +include(../../../cmake/evt-core_build.cmake) + +set( SAMPLE_SOURCES main.cpp SDOCanNode.cpp) +make_exe(canopen_sdo "${SAMPLE_SOURCES}") \ No newline at end of file diff --git a/samples/canopen/canopen_sdo/SDOCanNode.cpp b/samples/canopen/canopen_sdo/SDOCanNode.cpp new file mode 100644 index 00000000..f8c19e8a --- /dev/null +++ b/samples/canopen/canopen_sdo/SDOCanNode.cpp @@ -0,0 +1,66 @@ +#include "SDOCanNode.hpp" +#include +#include + +namespace log = core::log; + +SDOCanNode::SDOCanNode(CO_NODE& canNode) : node(canNode), sampleDataA(0), sampleDataB(0), transferBuffArray{0, 0} {} + +void SDOCanNode::transferData(io::csdo_callback_t callback, void* context) { + /* Increment the first element of transferBuffArray by 1. */ + transferBuffArray[0]++; + /* Set the second element of transferBuffArray to twice the new value of the first element. */ + transferBuffArray[1] = transferBuffArray[0] * 2; + + /* + * Initiates an SDO transfer for the specified node using the provided + * transfer buffer array. Targets the object dictionary entry at index 0x2100, + * sub-index 0x02. Registers and executes the SDOTransferCallback function upon completion. + */ + CO_ERR err = core::io::SDOTransfer(node, transferBuffArray, 2, CO_DEV(0x2100, 0x02), callback, context); + + /* Check if the SDO transfer was successfully started. */ + if (err == CO_ERR_NONE) { + /* Transfer is started successfully */ + log::LOGGER.log(log::Logger::LogLevel::INFO, "SDOTransfer Sent Request"); + + /* Note: don't use the 'readValue' until transfer is finished! */ + } else { + /* Unable to start the SDO transfer */ + log::LOGGER.log(log::Logger::LogLevel::ERROR, "SDOTransfer Request Error"); + } +} + +void SDOCanNode::receiveData(io::csdo_callback_t callback, void* context) { + static uint8_t receiveBuffArray[1]; + + /* + * Initiates an SDO receive operation for the specified node, reading data into + * the provided receive buffer array. Targets the object dictionary entry at + * index 0x2100, sub-index 0x01. Registers and executes the SDOReceiveCallback function upon completion. + */ + CO_ERR err = core::io::SDOReceive(node, receiveBuffArray, 1, CO_DEV(0x2100, 0x01), callback, context); + + /* Check if the SDO receive operation was successfully started. */ + if (err == CO_ERR_NONE) { + /* Transfer is started successfully */ + log::LOGGER.log(log::Logger::LogLevel::INFO, "SDOReceive Sent Request"); + + /* Note: don't use the 'readValue' until transfer is finished! */ + } else { + /* Unable to start the SDO transfer */ + log::LOGGER.log(log::Logger::LogLevel::ERROR, "SDOReceive Request Error"); + } +} + +CO_OBJ_T* SDOCanNode::getObjectDictionary() { + return &objectDictionary[0]; +} + +uint8_t SDOCanNode::getNumElements() { + return OBJECT_DICTIONARY_SIZE; +} + +uint8_t SDOCanNode::getNodeID() { + return NODE_ID; +} \ No newline at end of file diff --git a/samples/canopen/canopen_sdo/SDOCanNode.hpp b/samples/canopen/canopen_sdo/SDOCanNode.hpp new file mode 100644 index 00000000..7a734def --- /dev/null +++ b/samples/canopen/canopen_sdo/SDOCanNode.hpp @@ -0,0 +1,109 @@ +#include + +#include +#include +#include +#include + +namespace io = core::io; + +/** + * Representation of the CAN node. Handles constructing the object + * dictionary and other baseline settings. The idea is that each "board" + * will have a specific object dictionary associated with it. The object + * dictionary itself will also need to have information on "data of interest". + * For example, a temperature management system may to expose water pump + * flow rate in the object dictionary. + */ +class SDOCanNode : public CANDevice { +public: + SDOCanNode(CO_NODE& canNode); + + /** + * Update Object Dictionary entry + * + * @param callback[in] Callback function for the transfer operation + * @param context[in] Context for the callback function + */ + void transferData(io::csdo_callback_t callback, void* context); + + /** + * Read Object Dictionary entry + * + * @param callback[in] Callback function for the receive operation + * @param context[in] Context for the callback function + */ + void receiveData(io::csdo_callback_t callback, void* context); + + /** + * Get a pointer to the start of the object dictionary + * + * @return Pointer to the start of the object dictionary + */ + CO_OBJ_T* getObjectDictionary() override; + + /** + * Get the number of elements in the object dictionary. + * + * @return The number of elements in the object dictionary + */ + uint8_t getNumElements() override; + + /** + * Get the device's node ID + * + * @return The node ID of the can device. + */ + uint8_t getNodeID() override; + + /** + * Get the device's node ID + * + * @return The node ID of the can device. + */ + static constexpr uint8_t NODE_ID = 2; + +private: + /** + * This sample data will be exposed over CAN through the object + * dictionary. The address of the variable will be included in the + * object dictionary and can be updated via SDO via a CANopen client. + * This device will then broadcast the value via a triggered PDO. + */ + uint8_t sampleDataA; + uint16_t sampleDataB; + + /** Holds the data to be transferred */ + uint8_t transferBuffArray[2]{}; + + CO_NODE& node; + + /** + * Have to know the size of the object dictionary for initialization + * process. + */ + static constexpr uint8_t OBJECT_DICTIONARY_SIZE = 24; + + /** + * The object dictionary itself. Will be populated by this object during + * construction. + * + * The plus one is for the special "end of dictionary" marker. + */ + CO_OBJ_T objectDictionary[OBJECT_DICTIONARY_SIZE + 1] = { + MANDATORY_IDENTIFICATION_ENTRIES_1000_1014, + HEARTBEAT_PRODUCER_1017(2000), + IDENTITY_OBJECT_1018, + SDO_CONFIGURATION_1200, + SDO_CONFIGURATION_1280(1), + + // User defined data, this will be where we put elements that can be + // accessed via SDO and depending on configuration PDO + DATA_LINK_START_KEY_21XX(0, 0x02), + DATA_LINK_21XX(0x00, 0x01, CO_TUNSIGNED8, &sampleDataA), + DATA_LINK_21XX(0x00, 0x02, CO_TUNSIGNED16, &sampleDataB), + + // End of dictionary marker + CO_OBJ_DICT_ENDMARK, + }; +}; \ No newline at end of file diff --git a/samples/canopen/canopen_sdo/main.cpp b/samples/canopen/canopen_sdo/main.cpp new file mode 100644 index 00000000..651ea8e5 --- /dev/null +++ b/samples/canopen/canopen_sdo/main.cpp @@ -0,0 +1,161 @@ +/** + * This sample shows the CANopen SDO feature. This will + * set up the SDO client and will send and receive data + * from the server node. + * + * This sample is intended to be run alongside canopen_tpdo. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "SDOCanNode.hpp" + +namespace io = core::io; +namespace dev = core::dev; +namespace time = core::time; +namespace log = core::log; + +io::UART* uart; + +// Create a can interrupt handler +void canInterrupt(io::CANMessage& message, void* priv) { + auto* queue = (core::types::FixedQueue*) priv; + char messageString[50]; + + uint8_t* data = message.getPayload(); + if (queue != nullptr) { + queue->append(message); + } + + for (int i = 0; i < message.getDataLength(); i++) { + snprintf(&messageString[strlen(messageString)], 6, "0x%02X ", data[i]); + } + +#ifdef EVT_CORE_LOG_ENABLE + log::LOGGER.log(log::Logger::LogLevel::DEBUG, + "[CAN1] Got RAW message from %X of length %d with data: \r\n\t%s\r\n", + message.getId(), + message.getDataLength(), + messageString); +#endif +} + +void SdoTransferCallback(CO_CSDO* csdo, uint32_t entry, uint32_t code, void* context) { + char messageString[50]; + if (code == 0) { + /* read data is available in 'readValue' */ + snprintf(&messageString[0], 25, "Value transferred %x, %x\r\n", csdo->Tfer.Buf[0], csdo->Tfer.Buf[1]); + } else { + /* a timeout or abort is detected during SDO transfer */ + snprintf(&messageString[0], 36, "SDO transfer callback don goofed 0x%x\r\n", code); + } + + log::LOGGER.log(log::Logger::LogLevel::DEBUG, "SDO Transfer Operation: \r\n\t%s\r\n", messageString); +} + +void SdoReceiveCallback(CO_CSDO* csdo, uint32_t entry, uint32_t code, void* context) { + char messageString[50]; + if (code == 0) { + /* read data is available in 'readValue' */ + snprintf(&messageString[0], 25, "Value received %x, %x\r\n", csdo->Tfer.Buf[0], csdo->Tfer.Buf[1]); + } else { + /* a timeout or abort is detected during SDO transfer */ + snprintf(&messageString[0], 36, "SDO receive callback don goofed 0x%x\r\n", code); + } + + log::LOGGER.log(log::Logger::LogLevel::DEBUG, "SDO Receive Operation: \r\n\t%s\r\n", messageString); +} + +int main() { + // Initialize system + core::platform::init(); + + uart = &io::getUART(9600); + log::LOGGER.setUART(uart); + log::LOGGER.setLogLevel(log::Logger::LogLevel::DEBUG); + + dev::Timer& timer = dev::getTimer(100); + + /////////////////////////////////////////////////////////////////////////// + // Setup CAN configuration, this handles making drivers, applying settings. + // And generally creating the CANopen stack node which is the interface + // between the application (the code we write) and the physical CAN network + /////////////////////////////////////////////////////////////////////////// + + // Will store CANopen messages that will be populated by the EVT-core CAN + // interrupt + core::types::FixedQueue canOpenQueue; + + // Initialize CAN, add an IRQ which will add messages to the queue above + io::CAN& can = io::getCAN(); + can.addIRQHandler(canInterrupt, reinterpret_cast(&canOpenQueue)); + + // Reserved memory for CANopen stack usage + uint8_t sdoBuffer[CO_SSDO_N * CO_SDO_BUF_BYTE]; + CO_TMR_MEM appTmrMem[16]; + + // Reserve driver variables + CO_IF_DRV canStackDriver; + + CO_IF_CAN_DRV canDriver; + CO_IF_TIMER_DRV timerDriver; + CO_IF_NVM_DRV nvmDriver; + + CO_NODE canNode; + + // create the SDO node + SDOCanNode testCanNode(canNode); + + // Attempt to join the CAN network + io::CAN::CANStatus result = can.connect(); + + // test that the board is connected to the can network + if (result != io::CAN::CANStatus::OK) { + uart->printf("Failed to connect to CAN network\r\n"); + return 1; + } + + // Initialize all the CANOpen drivers. + io::initializeCANopenDriver(&canOpenQueue, &can, &timer, &canStackDriver, &nvmDriver, &timerDriver, &canDriver); + + // Initialize the CANOpen node we are using. + io::initializeCANopenNode(&canNode, &testCanNode, &canStackDriver, sdoBuffer, appTmrMem); + + // Set the node to operational mode + CONmtSetMode(&canNode.Nmt, CO_OPERATIONAL); + + time::wait(500); + + // print any CANopen errors + uart->printf("Error: %d\r\n", CONodeGetErr(&canNode)); + + /////////////////////////////////////////////////////////////////////////// + // Main loop + /////////////////////////////////////////////////////////////////////////// + uint32_t lastUpdate1 = HAL_GetTick(); + uint32_t lastUpdate2 = HAL_GetTick(); + + while (1) { + if ((HAL_GetTick() - lastUpdate1) >= 1000) { // If 1000ms have passed receive CAN message. + testCanNode.receiveData(SdoReceiveCallback, &canNode); // Receive data from server + lastUpdate1 = HAL_GetTick(); // Set to current time. + } else if ((HAL_GetTick() - lastUpdate2) >= 5000) { // If 5000ms have passed write CAN message. + testCanNode.transferData(SdoTransferCallback, &canNode); // Send data to server + lastUpdate2 = HAL_GetTick(); // Set to current time. + } + + io::processCANopenNode(&canNode); + // Wait for new data to come in + time::wait(10); + } +} \ No newline at end of file diff --git a/samples/canopen/canopen_tpdo/main.cpp b/samples/canopen/canopen_tpdo/main.cpp index aae68b98..006c9652 100644 --- a/samples/canopen/canopen_tpdo/main.cpp +++ b/samples/canopen/canopen_tpdo/main.cpp @@ -7,8 +7,11 @@ #include #include #include +#include #include #include +#include +#include #include @@ -17,6 +20,7 @@ namespace io = core::io; namespace dev = core::dev; namespace time = core::time; +namespace log = core::log; /////////////////////////////////////////////////////////////////////////////// // EVT-core CAN callback and CAN setup. This will include logic to set @@ -36,32 +40,40 @@ io::UART* uart; * @param message[in] The passed in CAN message that was read. */ -// create a can interrupt handler +// Create a can interrupt handler void canInterrupt(io::CANMessage& message, void* priv) { auto* queue = (core::types::FixedQueue*) priv; + char messageString[50]; // print out raw received data - uart->printf("Got RAW message from %X of length %d with data: ", message.getId(), message.getDataLength()); + snprintf(&messageString[5], + 6, + "Got RAW message from %X of length %d with data: ", + message.getId(), + message.getDataLength()); uint8_t* data = message.getPayload(); for (int i = 0; i < message.getDataLength(); i++) { - uart->printf("%X ", *data); + snprintf(&messageString[i * 5], 1, "%X ", *data); data++; } - uart->printf("\r\n"); + log::LOGGER.log(log::Logger::LogLevel::INFO, "\r\n\t%s\r\n", messageString); - if (queue != nullptr) + if (queue != nullptr) { queue->append(message); + } } // setup a TPDO event handler to print the raw TPDO message when sending extern "C" void COPdoTransmit(CO_IF_FRM* frm) { - uart->printf("Sending PDO as 0x%X with length %d and data: ", frm->Identifier, frm->DLC); + char messageString[50]; + snprintf(&messageString[5], 6, "Sending PDO as 0x%X with length %d and data: ", frm->Identifier, frm->DLC); + uint8_t* data = frm->Data; for (int i = 0; i < frm->DLC; i++) { - uart->printf("%X ", *data); + snprintf(&messageString[i * 5], 1, "%X ", *data); data++; } - uart->printf("\r\n"); + log::LOGGER.log(log::Logger::LogLevel::INFO, "\r\n\t%s\r\n", messageString); } int main() { diff --git a/src/core/io/CANopen.cpp b/src/core/io/CANopen.cpp index 290d9827..69cbe6db 100644 --- a/src/core/io/CANopen.cpp +++ b/src/core/io/CANopen.cpp @@ -1,5 +1,7 @@ +#include #include #include +#include #include #include @@ -9,6 +11,8 @@ #define MAX_SIZE 64 +namespace log = core::log; + /* * Empty namespace to contain "global" variables. These will be used within * the driver implementations. @@ -31,6 +35,30 @@ uint8_t testerStorage[MAX_SIZE]; // Queue that stores the CAN messages to send to the CANopen parser core::types::FixedQueue* canQueue; + +// SDO variables +typedef struct SDOState { + bool inProgress = false; + void* context; + core::io::csdo_callback_t callback; + CO_NODE* node; + uint32_t lastErr; +} sdo_state_t; + +sdo_state_t state; + +void internalCallback(CO_CSDO* csdo, uint16_t index, uint8_t sub, uint32_t code) { + if (state.callback != nullptr) { + state.callback(csdo, CO_DEV(index, sub), code, state.context); + } + + state.callback = nullptr; + state.node = nullptr; + state.context = nullptr; + state.lastErr = code; + state.inProgress = false; +} + } // namespace /////////////////////////////////////////////////////////////////////////////// @@ -131,6 +159,90 @@ void processCANopenNode(CO_NODE* canNode) { COTmrProcess(&canNode->Tmr); } +CO_ERR SDOTransfer(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry, csdo_callback_t transferCallback, + void* transferContext) { + while (state.inProgress == true) { + processCANopenNode(state.node); + time::wait(100); + } + + // Find the Client-SDO (CO_CSDO) object for the specified node. + CO_CSDO* csdo = COCSdoFind(&(node), 0); + CO_ERR err = CO_ERR_BAD_ARG; + + if (csdo != nullptr) { + state.callback = transferCallback; + state.context = transferContext; + state.node = &node; + + // Initiate an SDO download request. + err = COCSdoRequestDownload(csdo, entry, data, size, internalCallback, 1000); + } + + if (err == CO_ERR_NONE) { + state.inProgress = true; + } + + return err; +} + +CO_ERR SDOReceive(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry, csdo_callback_t receiveCallback, + void* receiveContext) { + while (state.inProgress == true) { + processCANopenNode(state.node); + time::wait(100); + } + + // Find the Client-SDO (CO_CSDO) object for the specified node. + CO_CSDO* csdo = COCSdoFind(&(node), 0); + CO_ERR err = CO_ERR_BAD_ARG; + + if (csdo != nullptr) { + state.callback = receiveCallback; + state.context = receiveContext; + state.node = &node; + + // Initiate an SDO upload request. + err = COCSdoRequestUpload(csdo, entry, data, size, internalCallback, 1000); + } + + if (err == CO_ERR_NONE) { + state.inProgress = true; + } + + return err; +} + +CO_ERR SDOTransferBlocking(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry) { + CO_ERR err = SDOTransfer(node, data, size, entry, nullptr, nullptr); + + while (state.inProgress == true) { + processCANopenNode(state.node); + time::wait(100); + } + + if (state.lastErr != 0) { + return CO_ERR_SDO_ABORT; + } + + return err; +} + +CO_ERR SDOReceiveBlocking(CO_NODE& node, uint8_t* data, uint8_t size, uint32_t entry) { + CO_ERR err = SDOReceive(node, data, size, entry, nullptr, nullptr); + + while (state.inProgress == true) { + processCANopenNode(state.node); + time::wait(100); + } + + if (state.lastErr != 0) { + return CO_ERR_SDO_ABORT; + } + + return err; +} + } // namespace core::io ///////////////////////////////////////////////////////////////////////////////