FWT-146 Solve Dat One (SDO) Issue

include/core/io/CANOpenMacros.hpp

@@ -121,12 +121,12 @@
     },                                                  \
     {                                                   \
         /* SDO Server Request COBID */                  \
-        .Key  = CO_KEY(0x1200, 0x01, CO_OBJ__N__R_),    \
+        .Key  = CO_KEY(0x1200, 0x01, CO_OBJ_DN__R_),    \
         .Type = CO_TUNSIGNED32,                         \
         .Data = (CO_DATA) CO_COBID_SDO_REQUEST(),       \
     },                                                  \
     { /* SDO Server Response COBID */                   \
-        .Key  = CO_KEY(0x1200, 0x02, CO_OBJ__N__R_),    \
+        .Key  = CO_KEY(0x1200, 0x02, CO_OBJ_DN__R_),    \
         .Type = CO_TUNSIGNED32,                         \
         .Data = (CO_DATA) CO_COBID_SDO_RESPONSE(),      \
     }

include/core/io/CANopen.hpp

@@ -14,11 +14,13 @@
 #include <core/io/CAN.hpp>
 #include <core/io/types/CANMessage.hpp>
 #include <core/utils/types/FixedQueue.hpp>
+#include <core/utils/log.hpp>
 
 // Allows for resizable CANOpen queue if needed
 #ifndef CANOPEN_QUEUE_SIZE
     #define CANOPEN_QUEUE_SIZE 150
 #endif
+namespace log = core::log;
 
 namespace core::io {
 
@@ -94,6 +96,39 @@ void initializeCANopenNode(CO_NODE* canNode, CANDevice* canDevice, CO_IF_DRV* ca
  */
 void processCANopenNode(CO_NODE* canNode);
 
+/**
+ * Update Object Dictionary entry
+ *
+ * @param node[in] The SDO node
+ * @param sampleDataArray[in] The value to set sample data to
+ */
+CO_ERR SDOTransfer(CO_NODE &node, uint8_t *data, uint8_t size, uint32_t entry);
+
+/**
+ * Read Object Dictionary entry
+ *
+ * @param node[in] The SDO node
+ * @param sampleDataArray[in] The value to set sample data to
+ */
+CO_ERR SDOReceive(CO_NODE &node, uint8_t *data, uint8_t size, uint32_t entry);
+
+/**
+ * The application-specific callback function for finalizing an SDO transfer operation.
+ * @param csdo[in] is the client-SDO object.
+ * @param index[in] is the object dictionary index.
+ * @param sub[in] is the object dictionary subindex.
+ * @param code[in] indicates the completion status of the operation (0 for success, error code otherwise).
+ */
+void AppCSdoTransferCb(CO_CSDO *csdo, uint16_t index, uint8_t sub, uint32_t code);
+
+/**
+ * The application-specific callback function for finalizing an SDO receive operation.
+ * @param csdo[in] is the client-SDO object.
+ * @param index[in] is the object dictionary index.
+ * @param sub[in] is the object dictionary subindex.
+ * @param code[in] indicates the completion status of the operation (0 for success, error code otherwise).
+ */
+void AppCSdoReceiveCb(CO_CSDO *csdo, uint16_t index, uint8_t sub, uint32_t code);
 } // namespace core::io
 
 #endif

samples/canopen/CMakeLists.txt

@@ -1,3 +1,4 @@
 add_subdirectory(canopen_rpdo)
 add_subdirectory(canopen_sample)
+add_subdirectory(canopen_sdo)
 add_subdirectory(canopen_tpdo)

samples/canopen/canopen_rpdo/main.cpp

@@ -11,6 +11,7 @@
 #include <core/manager.hpp>
 #include <core/utils/time.hpp>
 #include <core/utils/types/FixedQueue.hpp>
+#include <core/utils/log.hpp>
 
 #include <core/io/CANopen.hpp>
 
@@ -19,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,20 +38,18 @@ namespace time = core::time;
  * @param message[in] The passed in CAN message that was read.
  */
 
-io::UART& uart = io::getUART<io::Pin::UART_TX, io::Pin::UART_RX>(9600);
-
 // create a can interrupt handler
 void canInterrupt(io::CANMessage& message, void* priv) {
     auto* queue = (core::types::FixedQueue<CANOPEN_QUEUE_SIZE, io::CANMessage>*) priv;
 
     // print out raw received data
-    uart.printf("Got RAW message from %X of length %d with data: ", message.getId(), message.getDataLength());
+    log::LOGGER.log(log::Logger::LogLevel::INFO, "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);
+        log::LOGGER.log(log::Logger::LogLevel::INFO, "%X ", *data);
         data++;
     }
-    uart.printf("\r\n");
+    log::LOGGER.log(log::Logger::LogLevel::INFO, "\r\n");
 
     if (queue != nullptr)
         queue->append(message);
@@ -59,6 +59,8 @@ int main() {
     // Initialize system
     core::platform::init();
 
+    io::UART& uart = io::getUART<io::Pin::UART_TX, io::Pin::UART_RX>(9600);
+
     // Initialize the timer
     dev::Timer& timer = dev::getTimer<dev::MCUTimer::Timer2>(100);
 

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}")

samples/canopen/canopen_sdo/SDOCanNode.cpp

@@ -0,0 +1,72 @@
+#include "SDOCanNode.hpp"
+
+SDOCanNode::SDOCanNode() {
+    sampleDataA = 6;
+    sampleDataB = 0;
+}
+
+void SDOCanNode::SDO_Transfer(CO_NODE &node) {
+    /* Increment the first element of sampleDataArray by 1. */
+    sampleDataArray[0]++;
+    /* Set the second element of sampleDataArray to twice the new value of the first element. */
+    sampleDataArray[1] = sampleDataArray[0] * 2;
+
+    /* Initiate an SDO transfer using the specified node and sampleDataArray,
+     * targeting object dictionary entry 0x2100:02. */
+    CO_ERR err = core::io::SDOTransfer(node, sampleDataArray, 2, CO_DEV(0x2100,0x02));
+
+    /* 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::SDO_Receive(CO_NODE &node) {
+    /* Initiate an SDO receive operation, reading data into sampleDataArray
+     * from object dictionary entry 0x2100:01. */
+    CO_ERR err = core::io::SDOReceive(node, sampleDataArray, 1, CO_DEV(0x2100,0x01));
+
+    /* 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");
+    }
+}
+
+uint8_t SDOCanNode::getSampleDataA() {
+    return sampleDataA;
+}
+
+uint16_t SDOCanNode::getSampleDataB() {
+    return sampleDataB;
+}
+
+void SDOCanNode::update() {
+    sampleDataA++;
+    if (sampleDataA % 20 == 0) {
+        sampleDataB *= 3;
+    }
+}
+
+CO_OBJ_T* SDOCanNode::getObjectDictionary() {
+    return &objectDictionary[0];
+}
+
+uint8_t SDOCanNode::getNumElements() {
+    return OBJECT_DICTIONARY_SIZE;
+}
+
+uint8_t SDOCanNode::getNodeID() {
+    return NODE_ID;
+}

samples/canopen/canopen_sdo/SDOCanNode.hpp

@@ -0,0 +1,141 @@
+#include <cstdint>
+#include <core/utils/log.hpp>
+
+#include <co_core.h>
+#include <co_csdo.h>
+#include <core/io/CANDevice.hpp>
+#include <core/io/CANOpenMacros.hpp>
+#include <core/io/CANOpen.hpp>
+
+/**
+ * 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.
+ */
+
+namespace log = core::log;
+
+class SDOCanNode : public CANDevice {
+public:
+    SDOCanNode();
+
+    /**
+     * Update Object Dictionary entry
+     *
+     * @param node[in] The canopen node to write to
+     */
+    void SDO_Transfer(CO_NODE &node);
+
+    /**
+     * Read Object Dictionary entry
+     *
+     * @param node[in] The canopen node to read from
+     */
+    void SDO_Receive(CO_NODE &node);
+
+    /**
+     * Get the contained sample data
+     *
+     * @return The value of the sample data
+     */
+    uint8_t getSampleDataA();
+    uint16_t getSampleDataB();
+
+    /**
+     * increments counters up
+     */
+    void update();
+
+    /**
+     * 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;
+    uint8_t sampleDataArray[2] = {0, 0};
+
+    /**
+     * 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,
+
+        {
+            /* 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(),
+        },
+        {
+            .Key  = CO_KEY(0x1280, 0x03, CO_OBJ_D___R_),
+            .Type = CO_TUNSIGNED8,
+            .Data = (CO_DATA) 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,
+    };
+};