Paul Paterson
library for C++ CANOpen implementation. mbed independant, but is easy to attach into with mbed.
Dependents: ppCANOpen_Example DISCO-F746NG_rtos_test
Example:
Import programppCANOpen_Example
I am no longer actively working on the ppCANOpen library, however, I want to publish this project so that anyone who wants to pick up any of the pieces can have a good example. This is a a project I was working on using the ppCANOpen library. It has a pretty in deep use of the object dictionary structure. And a number of functions to control high voltage pinball drivers, if you're into that sort of thing.
Last commit 19 Mar 2016 by 
Paul Paterson
Revision 5:22a337cdc0e3, committed 2016-02-13
- Comitter:
- ptpaterson
- Date:
- Sat Feb 13 20:22:59 2016 +0000
- Parent:
- 4:2034b04c86d2
- Commit message:
- PDO receive complete
Changed in this revision
--- a/drivers/mbed-Nucleo-F091RC/canopen_api.cpp Sat Jan 09 17:15:29 2016 +0000
+++ b/drivers/mbed-Nucleo-F091RC/canopen_api.cpp Sat Feb 13 20:22:59 2016 +0000
@@ -28,44 +28,78 @@
#include "canopen_api.h"
#include "canopen_config.h"
-#include "CanOpenHandle.h"
-#include "mbed.h"
-
-#include "CanOpenHandle.h"
#include "CanOpenMessage.h"
-int CanOpenApiInit (CanOpenHandle *hCanOpen)
+#include "mbed.h"
+#include "CAN.h" /* use with CANNucleo library */
+
+#include "stdio.h"
+
+
+void *pServiceObject = 0;
+
+/* Can declarations ---------------------------------------------------------*/
+CAN * can; //(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
+ServiceProviderRxInterruptCallback serviceRxCallback = 0;
+void CanReadInterruptCallback (void) {
+ if (serviceRxCallback && pServiceObject) {
+ serviceRxCallback(pServiceObject);
+ }
+}
+
+/* Ticker declarations ---------------------------------------------------------*/
+Ticker updateTicker;
+ServiceProviderTickCallback serviceTickCallback = 0;
+void UpdateTickerCallback(void) {
+ if (serviceTickCallback && pServiceObject) {
+ serviceTickCallback(pServiceObject);
+ }
+}
+
+int CanOpenApiInit (void *object, ServiceProviderRxInterruptCallback rxCallback, ServiceProviderTickCallback tickCallback)
{
printf ("----------- CANOPEN API: API INIT\r\n");
- /** CAN instance to do all of the communication
- *
- * @note For some reason, I cannot use "static CAN can". It will compile,
- * but code get's stuck during initialization, and will not even enter
- * main(). Works great like this though! Only one ServiceProvider, though,
- * so mem leaks should not be an issue if we don't delete it???
- */
+ /* CAN instance to do all of the communication
+ *
+ * For some reason, I cannot use "static CAN can". It will compile,
+ * but code get's stuck during initialization, and will not even enter
+ * main(). Works great like this though! Only one ServiceProvider, though,
+ * so mem leaks should not be an issue if we don't delete it???
+ */
//static CAN can(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
- CAN * can = new CAN(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
+ //can = new CAN(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
+ can = new CAN(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
+ //can.reset();
- hCanOpen->can = can;
+ /* init the callback system ---------------------------------------------*/
+ pServiceObject = object;
+
+ serviceRxCallback = rxCallback;
+ serviceTickCallback = tickCallback;
+
+ can->attach(CanReadInterruptCallback);
+ updateTicker.attach(UpdateTickerCallback, .0005);
return 1;
}
-int CanOpenApiRead (CanOpenHandle *hCanOpen, CanOpenMessage *canOpenMsg)
+int CanOpenApiRead (CanOpenMessage *canOpenMsg)
{
int result = 0;
- printf("api: can_read: Anything to read?\r\n");
+ /* CAUTON ********************************
+ * Could be used in interrupt!!!
+ * ***************************************
+ */
+ //printf("canopen_api: CanOpenApiRead()\r\n");
- /*
- if (hCanOpen->can) {
+ if (can) {
CANMessage canMsg;
- result = hCanOpen->can->read (canMsg);
+ result = can->read (canMsg);
if (result) {
canOpenMsg->id = canMsg.id;
canOpenMsg->dataCount = canMsg.len,
@@ -82,42 +116,53 @@
canOpenMsg->data[5] = canMsg.data[5];
canOpenMsg->data[6] = canMsg.data[6];
canOpenMsg->data[7] = canMsg.data[7];
+
+ // DEBUG
+ //printf(" ID: %d\r\n", (int)canOpenMsg->id);
+ //printf(" Count: %d\r\n", (int)canOpenMsg->dataCount);
+ //printf(" Type: %d\r\n", (int)canOpenMsg->type);
+ //printf(" Format: %d\r\n", (int)canOpenMsg->format);
+ //printf(" Data[0]: %d\r\n", (int)canOpenMsg->data[0]);
}
}
- */
return result;
}
-int CanOpenApiWrite (CanOpenHandle *hCanOpen, CanOpenMessage *canOpenMsg)
+int CanOpenApiWrite (CanOpenMessage *canOpenMsg)
{
int result = 0;
- if (hCanOpen->can) {
+ CANMessage canMsg;
+ canMsg.id = canOpenMsg->id;
+ canMsg.len = canOpenMsg->dataCount,
+ canMsg.type = canOpenMsg->type == CANOPEN_TYPE_DATA ? CANData : CANRemote;
+ canMsg.format = canOpenMsg->format == CANOPEN_FORMAT_STANDARD ? CANStandard : CANExtended;
- CANMessage canMsg;
- canMsg.id = canOpenMsg->id;
- canMsg.len = canOpenMsg->dataCount,
- canMsg.type = canOpenMsg->type == CANOPEN_TYPE_DATA ? CANData : CANRemote;
- canMsg.format = canOpenMsg->format == CANOPEN_FORMAT_STANDARD ? CANStandard : CANExtended;
-
- // NOTE: memcpy is freezing execution
- //memcpy(canMsg.data, canOpenMsg->data, canOpenMsg->dataCount);
- canMsg.data[0] = canOpenMsg->data[0];
- canMsg.data[1] = canOpenMsg->data[1];
- canMsg.data[2] = canOpenMsg->data[2];
- canMsg.data[3] = canOpenMsg->data[3];
- canMsg.data[4] = canOpenMsg->data[4];
- canMsg.data[5] = canOpenMsg->data[5];
- canMsg.data[6] = canOpenMsg->data[6];
- canMsg.data[7] = canOpenMsg->data[7];
-
- result = hCanOpen->can->write (canMsg);
- }
+ // NOTE: memcpy is freezing execution
+ //memcpy(canMsg.data, canOpenMsg->data, canOpenMsg->dataCount);
+ canMsg.data[0] = canOpenMsg->data[0];
+ canMsg.data[1] = canOpenMsg->data[1];
+ canMsg.data[2] = canOpenMsg->data[2];
+ canMsg.data[3] = canOpenMsg->data[3];
+ canMsg.data[4] = canOpenMsg->data[4];
+ canMsg.data[5] = canOpenMsg->data[5];
+ canMsg.data[6] = canOpenMsg->data[6];
+ canMsg.data[7] = canOpenMsg->data[7];
+
+ result = can->write (canMsg);
+
return result;
}
+static int timeSyncOffset = 0;
+
+uint32_t CanOpenApiGetHardwareTime (void)
+{
+ return HAL_GetTick();
+}
+
@@ -127,4 +172,3 @@
-
--- a/include/CanOpenMessage.h Sat Jan 09 17:15:29 2016 +0000
+++ b/include/CanOpenMessage.h Sat Feb 13 20:22:59 2016 +0000
@@ -29,6 +29,8 @@
#ifndef PPCAN_CANOPEN_MESSAGE_H
#define PPCAN_CANOPEN_MESSAGE_H
+#include "stdint.h"
+
#ifdef __cplusplus
extern "C" {
#endif
@@ -54,13 +56,91 @@
/** CANOpen Message */
typedef struct CanOpenMessage {
- unsigned int id;
- char data[8];
+ uint32_t id;
+ uint8_t data[8];
CanOpenFormat format;
CanOpenType type;
- unsigned char dataCount;
+ uint8_t dataCount;
} CanOpenMessage;
+
+/*=========================================================================
+ * CANOpen Function Codes
+ *=========================================================================
+ */
+
+/** CANOpen Function Codes */
+typedef enum {
+ CANOPEN_FUNCTION_CODE_NMT = 0x00,
+ CANOPEN_FUNCTION_CODE_SYNC = 0x01,
+ CANOPEN_FUNCTION_CODE_TIME = 0x02,
+ CANOPEN_FUNCTION_CODE_PDO1T = 0x03,
+ CANOPEN_FUNCTION_CODE_PDO1R = 0x04,
+ CANOPEN_FUNCTION_CODE_PDO2T = 0x05,
+ CANOPEN_FUNCTION_CODE_PDO2R = 0x06,
+ CANOPEN_FUNCTION_CODE_PDO3T = 0x07,
+ CANOPEN_FUNCTION_CODE_PDO3R = 0x08,
+ CANOPEN_FUNCTION_CODE_PDO4T = 0x09,
+ CANOPEN_FUNCTION_CODE_PDO4R = 0x0A,
+ CANOPEN_FUNCTION_CODE_SDOT = 0x0B,
+ CANOPEN_FUNCTION_CODE_SDOR = 0x0C,
+ CANOPEN_FUNCTION_CODE_NODE_GUARD = 0x0E,
+ CANOPEN_FUNCTION_CODE_LSS = 0x0F
+} CanOpenFunctionCodes;
+
+/* Message Constants */
+#define MESSAGE_NODEID_BITS 0b00001111111
+#define MESSAGE_COMMAND_BITS 0b11110000000
+
+/* Message Macros -----------------------------------------------------------*/
+#define MESSAGE_GET_NODEID(cobId) (cobId & MESSAGE_NODEID_BITS)
+#define MESSAGE_GET_COMMAND(cobId) ((cobId & MESSAGE_COMMAND_BITS) >> 7)
+
+/*=========================================================================
+ * SDO MESSAGE PARAMETERS
+ *=========================================================================
+ */
+
+/** SDO initiate protocol command specifiers */
+typedef enum {
+ SDO_CCS_DOWNLOAD_SEGMENT_REQUEST = 0x00,
+ SDO_CCS_INITIATE_DOWNLOAD_REQUEST = 0x01,
+ SDO_CCS_INITIATE_UPLOAD_REQUEST = 0x02,
+} SdoClientCommandSpecifier;
+
+/** SDO segment protocol command specifiers */
+typedef enum {
+ SDO_SCS_DOWNLOAD_SEGMENT_RESPONSE = 0x01,
+ SDO_SCS_INITIATE_UPLOAD_RESPONSE = 0x02,
+ SDO_SCS_INITIATE_DOWNLOAD_RESPONSE = 0x03,
+} SdoServerCommandSpecifier;
+
+/* SDO constants --------------------------------------------------------*/
+#define SDO_SIZE_INDICATOR_BIT 0b00000001
+#define SDO_TRANSFER_TYPE_BIT 0b00000010
+#define SDO_DATA_COUNT_BITS 0b00001100
+#define SDO_TOGGLE_BIT 0b00010000
+#define SDO_CS_BITS 0b11100000
+
+/* SDO macros -----------------------------------------------------------*/
+#define SDO_GET_CS(data0) ((data0 & SDO_CS_BITS) >> 5)
+#define SDO_GET_DATA_COUNT(data0) ((data0 & SDO_DATA_COUNT_BITS) >> 2)
+
+
+/*=========================================================================
+ * NMT MESSAGE PARAMETERS
+ *=========================================================================
+ */
+
+/** NMT node control protocol command specifiers */
+typedef enum {
+ NMT_CS_START = 0x01,
+ NMT_CS_STOP = 0x02,
+ NMT_CS_ENTER_PREOP = 0x80,
+ NMT_CS_RESET_NODE = 0x81,
+ NMT_CS_RESET_COM = 0x82
+} NmtCommandSpecifier;
+
#ifdef __cplusplus
};
#endif
--- a/include/Node.h Sat Jan 09 17:15:29 2016 +0000
+++ b/include/Node.h Sat Feb 13 20:22:59 2016 +0000
@@ -29,12 +29,14 @@
#ifndef PPCAN_NODE_H
#define PPCAN_NODE_H
+#include "ObjectDictionary.h"
/*=========================================================================
* Forward declarations
*=========================================================================
*/
struct CanOpenMessage;
+//struct ObjectData;
/* NMT Constants --------------------------------------------------------*/
#define NMT_STATE_TOGGLE_BIT 0x80
@@ -47,8 +49,6 @@
/* Avoid circular reference */
class ServiceProvider;
-class ObjectDictionary;
-
/** Node Class to implement feature of a CANOpen NMT node
@@ -57,6 +57,11 @@
{
public:
+ /* ========================================================================
+ * Internal structures and constants
+ * ========================================================================
+ */
+
/** Node network management state */
struct State {
static const int INITIALIZED = 0x00;
@@ -80,45 +85,60 @@
int bLifeGuardToggle;
};
+ /* ========================================================================
+ * Construction
+ * ========================================================================
+ */
+
/** Maintain the multitude of states for a node */
-
- Node (ServiceProvider * provider);
+ Node (int id, ServiceProvider * provider, int bLoop = 0);
/* ========================================================================
- * Methods to handle operation of node device
+ * Public Methods
* ========================================================================
*/
- // TODO: pass elapsed time to Update method
- virtual void Update (void);
+ /** Call from ServiceProvider on an interrupt */
+ void FixedUpdate (uint32_t time);
+
+ /** Call from ServiceProvider every loop of Run() */
+ void Update (void);
+ /** Handle message given by the ServiceProvider*/
+ int DispatchMessage(CanOpenMessage *canOpenMsg);
+
/* ========================================================================
- * Message indication
- * Called by a ServiceProvider when a message is received.
+ * Public Member Variables
* ========================================================================
*/
- /**
- * @note
+ /* Common Node properties -----------------------------------------------*/
+ /** Network id for the node
+ * @note Ref is held in object dictionary and upon setting, may need to
+ * update some other objects.
*/
- int DispatchMessage(CanOpenMessage *canOpenMsg);
+ int nodeId;
+ /** Loopback Mode. Default off (0), if on (1) then will hear it's own
+ * messages
+ */
+ int bLoopbackOn;
protected:
/* ========================================================================
- * Methods to handle various messages
- * Called by a ServiceProvider when a message is received.
+ * Protected? Methods to handle various messages
+ * Used to deal with everything from inside of Dispatch Message
* ========================================================================
*/
/* PDO (7.2.2), MPDO (7.2.3) --------------------------------------------*/
- int ConsumePdo (int pdoNum, char * dataIn);
- int HandlePdoReadRequest (int pdoNum);
+ int HandlePdo (CanOpenMessage *canOpenMsg);
/* SDO (7.2.4) ----------------------------------------------------------*/
+ int HandleSdo (CanOpenMessage *canOpenMsg);
int HandleExpeditedDownload (int sdoNum, int index, int subIndex, int dataCount, char * data);
int HandleInitiateDownloadRequest (int sdoNum, int index, int subIndex, int dataCount, char * data);
@@ -129,12 +149,10 @@
// TODO: express and not express
/* SYNC object (7.2.5) --------------------------------------------------*/
-
- // TODO: SYNC consumer
+ int HandleSync (CanOpenMessage *canOpenMsg);
/* Time Stamp object (7.2.6) --------------------------------------------*/
-
- // TODO: time consumer
+ /* Handled all inside of the ServiceProvider */
/* Emergency object (7.2.7) ---------------------------------------------*/
@@ -143,7 +161,7 @@
/* Network Management (7.2.8) -------------------------------------------*/
/* ---- Node Control (7.2.8.2.1) ----------------------------------------*/
- int HandleNodeControl (int commandSpecifier);
+ int HandleNodeControl (CanOpenMessage *canOpenMsg);
/* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
@@ -151,17 +169,34 @@
int HandleNodeGuardRequest (int masterId);
int ConsumeHeartbeat (int producerId);
-
+protected:
+
/* ========================================================================
- * Methods to implement node control in derived classes
+ * Protected Methods to implement node application in derived classes
* ========================================================================
*/
- /** Perform actions every cycle
+ /** Perform actions every cycle (when in operational mode)
+ * @note Override to implement user application
+ * @note The OnTick function should avoid modifying data in the object
+ * dictionary unless necessary, because there will almost certainly be
+ * a race condition.
+ */
+ virtual void OnFixedUpdate(void) = 0;
+
+
+
+ virtual void OnUpdate(void) = 0;
+
+
+ /* SYNC -----------------------------------------------------------------*/
+ /** Perform actions when state changed to stop
* @note Override to implement user application
*/
- virtual void OnUpdate(void) = 0;
+ virtual void OnSync (uint8_t counter) = 0;
+
+ /* NMT Node Control -----------------------------------------------------*/
/** Perform actions when node reset
* @note Override to implement user application
*/
@@ -182,36 +217,61 @@
*/
virtual void OnStopped (void) = 0;
+ /* Object Dictionary Handling -------------------------------------------*/
+ /** Abstract method to give access to the object entries of derived
+ * classes
+ */
+ virtual ObjectData * ScanIndex(IndexSize index) = 0;
+
+
/* ========================================================================
- * Protected Members
+ * Protected Methods to Post Messages
* ========================================================================
*/
+
+ int PostTPDO (int cobId);
- /** object dictionary */
- ObjectDictionary * pMyDictionary;
-private:
+ /* ========================================================================
+ * Protected Member Variables
+ * ========================================================================
+ */
+
+ /* Object Dictionary Handling -------------------------------------------*/
+ /** Array of ObjectData to contain all of the object dictionary data */
+ ObjectData *dictionary;
+
+ /* Elapsed Time ---------------------------------------------------------*/
+ /** Millisecond time stamp at this OnTick() */
+ uint32_t timeCurrentTick;
+
+ /** Millisecond difference between current OnTick() and the previous */
+ uint32_t timeSinceLastTick;
+
+// should be private
+// TODO: implement functions in Node class to post all types of messages so
+// derived classes do not have to consider message structure.
+protected:
/* ========================================================================
* Private members
* ========================================================================
*/
-
- /** Reference to Service Provider that this node is attached to.
- * @note May need to be a pointer, because node id is held in object dictionary
- */
+ /* ServiceProvider ------------------------------------------------------*/
+ /** Reference to Service Provider that this node is attached to */
ServiceProvider * pMyProvider;
-
- /** Network id for the node
- * @note May need to be a pointer, because node id is held in object dictionary
- */
- int nodeId;
+
+private:
+ /* Common Node Methods --------------------------------------------------*/
+ /** Change state of the node */
+ void ChangeState(int newState);
- /** Network and communication state of the node
- * @note
- */
- State state;
+ /* Common Node properties -----------------------------------------------*/
+ /** Network and communication state of the node */
+ State state;
+
+
};
} /* namespace ppCANOpen */
--- a/include/ObjectDictionary.h Sat Jan 09 17:15:29 2016 +0000
+++ b/include/ObjectDictionary.h Sat Feb 13 20:22:59 2016 +0000
@@ -34,27 +34,21 @@
namespace ppCANOpen
{
-/** Abstract Object Dictionary class to provide constants and virtual scan
- * function.
- */
-class ObjectDictionary
+/** Data Type alias to provide context when defining the object library */
+typedef uint8_t DataType;
+
+/** Data Property alias to provide context when defining the object library */
+typedef uint8_t DataProperty;
+
+/** defines the data in a single entry of data (subindex)
+ */
+struct EntryData
{
-
-public:
-
- /*=========================================================================
- * Constants wrapped into the Object Dictionary to use with construction
- *=========================================================================
- */
-
/* Data value type constants ----------------------------------------------
* Taken from the CANOpen Standard. they are part of the object dictionary
* at indices 0x0001 to 0x0023. Given to every Sub Index.
*/
- /** Data Type alias to provide context when defining the object library */
- typedef uint8_t DataType;
-
static const DataType TYPE_BOOLEAN = 0x01;
static const DataType TYPE_INT8 = 0x02;
static const DataType TYPE_INT16 = 0x03;
@@ -94,69 +88,46 @@
* Can be strung together into a mask.
*/
- /** Data Property alias to provide context when defining the object library */
- typedef uint8_t DataProperty;
-
static const DataProperty PROPERTY_READABLE = 0x01;
static const DataProperty PROPERTY_WRITEABLE = 0x02;
static const DataProperty PROPERTY_READ_WRITEABLE = 0x03;
/* static const uint8_t PROPERTY_STATIC = 0x04 *//* possible to save setting after powerdown */
-
- /* Index Constants --------------------------------------------------------
+ /* EntryData data --------------------------------------------------------
*/
-
- /** Index alias to provide context when defining the object library */
- typedef uint8_t Index;
+
+ void *pData;
+ uint16_t size;
+ DataType type;
+ DataProperty properties;
+
+ EntryData(){}
+
+ EntryData(void *d, uint16_t s, DataType t, DataProperty p)
+ : pData(d), size(s), type(t), properties(p)
+ {}
+};
- /*=========================================================================
- * Internal structures
- *=========================================================================
- */
-
- /** defines the data in a single entry of data (subindex)
- */
- struct EntryData
- {
- void *pData;
- uint16_t size;
- DataType type;
- DataProperty properties;
-
- EntryData(){}
-
- EntryData(void *d, uint16_t s, DataType t, DataProperty p)
- : pData(d), size(s), type(t), properties(p)
- {}
- };
+/** Index alias to provide context when defining the object library */
+typedef uint16_t IndexSize;
+
+/** Sub-Index alias to provide context when defining the object library */
+typedef uint8_t SubIndexSize;
+
+/** defines the data for a single index, including an array of subindices
+ */
+struct ObjectData
+{
+ EntryData *entries;
+ IndexSize index;
+ SubIndexSize entryCount;
- /** defines the data for a single index, including an array of subindices
- */
- struct ObjectData
- {
- EntryData *entries;
- uint16_t index;
- uint16_t entryCount;
-
- ObjectData(){}
-
- ObjectData(EntryData *e, uint16_t i, uint16_t c)
- : entries(e), index(i), entryCount(c)
- {}
- };
-
-
- /*=========================================================================
- * Public Methods
- *=========================================================================
- */
-
- /** Abstract method to give access to the object entries of derived
- * classes
- */
- virtual ObjectData * ScanIndex(int index) = 0;
+ ObjectData(){}
+ ObjectData(EntryData *e, IndexSize i, SubIndexSize c)
+ : entries(e), index(i), entryCount(c)
+ {}
};
} /* namespace ppCANOpen */
--- a/include/ServiceProvider.h Sat Jan 09 17:15:29 2016 +0000
+++ b/include/ServiceProvider.h Sat Feb 13 20:22:59 2016 +0000
@@ -51,65 +51,117 @@
{
public:
-
-
ServiceProvider (void);
~ServiceProvider (void);
+ /* ========================================================================
+ * Public Methods
+ * ========================================================================
+ */
+
+ /* Main application -----------------------------------------------------*/
/** Main loop */
void Run (void);
- /** Register a node to get messages and get update calls
- * @note
- * @param
- * @retval
- */
+ /** Iterate through Nodes and update */
+ void UpdateNodes(void);
+
+ /** C helper function to pass to API */
+ static void UpdateNodes_CWrapper(void *pServiceObject);
+
+ /* Node Management ------------------------------------------------------*/
+ /** Register a node to get messages and get update calls */
int AddNode (Node * node);
- /** Register a node to get messages and get update calls
- * @note
- * @param
- * @retval
- */
+ // TODO: remove node (swap pointers to fill in nicely
+
- /** Add a message to the message queue outbox
- * @note
- * @param
+ /* CanMessage Transmission ----------------------------------------------*/
+ /** Add a message to the message queue outbox */
+ void PostMessage (int nodeId, CanOpenMessage * msg);
+
+ /** quick way to create and send an NMT Control message
+ * @note Make posting a message easier!
*/
- void PostMessage (CanOpenMessage * msg);
+ void PostNmtControl (char targetNodeId, NmtCommandSpecifier cs);
+
+ /* Elapsed Time ---------------------------------------------------------*/
+ /** A way for the the Nodes to get elapsed time when they want it */
+ uint32_t GetTime(void);
- // TODO: remove node (swap pointers to fill in nicely
private:
+ /* ========================================================================
+ * Private Constants
+ * ========================================================================
+ */
- /** define size of arrays in one place */
+ /** Define size of arrays in one place */
static const int SERVICE_MAX_NODES = 8;
/* ========================================================================
- * Private Members
+ * Private Methods
* ========================================================================
*/
- /** array of messages in a queue
- * @note twice number of max nodes is a bit arbitrary. Will need at
- * least max nodes, but not sure how much more to be safe.
+ /* CanMessage Reading ---------------------------------------------------*/
+ /** Function to be called on every CAN read interrupt */
+ void ReadIT(void);
+
+ /** C helper function used to pass ReadIT() to API */
+ static void ReadIT_CWrapper(void *pServiceObject);
+
+ /** Helper function that utilizes backup message inbox when the main one
+ * is locked by the application.
*/
- std::queue<CanOpenMessage> outbox;
- //CanOpenMessage outbox[SERVICE_MAX_NODES * 2];
- //int messageCount;
+ void ReadIT_clearBuffer(void);
- /** array of nodes to cycle through.
- * @note
+ /* Elapsed Time ---------------------------------------------------------*/
+ /** Synchronize elapsed time*/
+ void HandleTimeMessage(CanOpenMessage *canOpenMsg);
+
+
+ /* ========================================================================
+ * Private Member Variables
+ * ========================================================================
*/
+
+ /* Node Management -------------------------------------------------------*/
+ /** array of nodes to cycle through. */
Node * nodes[SERVICE_MAX_NODES];
- /** Keeps count of current number of nodes.
- * @note
+ /** Keeps count of current number of nodes. */
+ int nodeCount;
+
+ /* CanMessage Reading ---------------------------------------------------*/
+ /** array of messages in a queue */
+ std::queue<CanOpenMessage> inbox;
+
+ /** Extra buffer only handled by ISR. Used if inbox is locked */
+ std::queue<CanOpenMessage> inboxBuffer;
+
+ /** boolean to lock and unlock inbox */
+ int bInboxUnlocked;
+
+ /** Data to keep track of internal messages. Node ID allows SP to avoid
+ * dispatching message to the one who sent it
*/
- int nodeCount;
+ struct InternalMessage {
+ int nodeId;
+ CanOpenMessage message;
+
+ InternalMessage(uint8_t n, CanOpenMessage m):
+ nodeId(n),
+ message(m)
+ {}
+ };
+
+ /** array of messages in a queue */
+ std::queue<InternalMessage> outbox;
- /** stores handle for can open api */
- CanOpenHandle * hCanOpen;
-
+ /* Elapsed Time ---------------------------------------------------------*/
+ /** Elapsed time offset used for synchronization*/
+ int32_t elapsedTimeOffset;
+
};
} /* namspace ppCANOpen */
--- a/include/canopen_api.h Sat Jan 09 17:15:29 2016 +0000
+++ b/include/canopen_api.h Sat Feb 13 20:22:59 2016 +0000
@@ -29,6 +29,8 @@
#ifndef PPCAN_CANOPEN_API_H
#define PPCAN_CANOPEN_API_H
+#include "stdint.h"
+
#ifdef __cplusplus
extern "C" {
#endif
@@ -36,8 +38,7 @@
/*=========================================================================
* Forward declarations
*=========================================================================
- */
- struct CanOpenHandle;
+ */
struct CanOpenMessage;
/*=========================================================================
@@ -45,10 +46,15 @@
*=========================================================================
*/
- int CanOpenApiInit (CanOpenHandle * hCanOpen);
+ typedef void (* ServiceProviderRxInterruptCallback)(void *pServiceObject);
+ typedef void (* ServiceProviderTickCallback) (void *pServiceObject);
+
+ int CanOpenApiInit (void *object, ServiceProviderRxInterruptCallback rxCallback, ServiceProviderTickCallback tickCallback);
- int CanOpenApiRead (CanOpenHandle * hCanOpen, CanOpenMessage * canOpenMsg);
- int CanOpenApiWrite (CanOpenHandle * hCanOpen, CanOpenMessage * canOpenMsg);
+ int CanOpenApiRead (CanOpenMessage * canOpenMsg);
+ int CanOpenApiWrite (CanOpenMessage * canOpenMsg);
+
+ uint32_t CanOpenApiGetHardwareTime (void);
#ifdef __cplusplus
};
--- a/source/Node.cpp Sat Jan 09 17:15:29 2016 +0000
+++ b/source/Node.cpp Sat Feb 13 20:22:59 2016 +0000
@@ -36,84 +36,17 @@
namespace ppCANOpen
{
-
- /** CANOpen Function Codes */
- typedef enum {
- CANOPEN_FUNCTION_CODE_NMT = 0x00,
- CANOPEN_FUNCTION_CODE_SYNC = 0x01,
- CANOPEN_FUNCTION_CODE_TIME = 0x02,
- CANOPEN_FUNCTION_CODE_PDO1T = 0x03,
- CANOPEN_FUNCTION_CODE_PD01R = 0x04,
- CANOPEN_FUNCTION_CODE_PD02T = 0x05,
- CANOPEN_FUNCTION_CODE_PD02R = 0x06,
- CANOPEN_FUNCTION_CODE_PD03T = 0x07,
- CANOPEN_FUNCTION_CODE_PD03R = 0x08,
- CANOPEN_FUNCTION_CODE_PD04T = 0x09,
- CANOPEN_FUNCTION_CODE_PD04R = 0x0A,
- CANOPEN_FUNCTION_CODE_SD0T = 0x0B,
- CANOPEN_FUNCTION_CODE_SD0R = 0x0C,
- CANOPEN_FUNCTION_CODE_NODE_GUARD = 0x0E,
- CANOPEN_FUNCTION_CODE_LSS = 0x0F
- } CanOpenFunctionCodes;
-
- /* Message Constants */
- #define MESSAGE_NODEID_BITS 0b00001111111
- #define MESSAGE_COMMAND_BITS 0b11110000000
-
- /* Message Macros -----------------------------------------------------------*/
- #define MESSAGE_GET_NODEID(cobId) (cobId & MESSAGE_NODEID_BITS)
- #define MESSAGE_GET_COMMAND(cobId) ((cobId & MESSAGE_COMMAND_BITS) >> 7)
+Node::Node (int id, ServiceProvider * pProvider, int bLoop)
+{
+ nodeId = id;
- /*=========================================================================
- * SDO MESSAGE PARAMETERS
- *=========================================================================
- */
-
- /** SDO initiate protocol command specifiers */
- typedef enum {
- SDO_CCS_DOWNLOAD_SEGMENT_REQUEST = 0x00,
- SDO_CCS_INITIATE_DOWNLOAD_REQUEST = 0x01,
- SDO_CCS_INITIATE_UPLOAD_REQUEST = 0x02,
- } SdoClientCommandSpecifier;
-
- /** SDO segment protocol command specifiers */
- typedef enum {
- SDO_SCS_DOWNLOAD_SEGMENT_RESPONSE = 0x01,
- SDO_SCS_INITIATE_UPLOAD_RESPONSE = 0x02,
- SDO_SCS_INITIATE_DOWNLOAD_RESPONSE = 0x03,
- } SdoServerCommandSpecifier;
-
- /* SDO constants --------------------------------------------------------*/
- #define SDO_SIZE_INDICATOR_BIT 0b00000001
- #define SDO_TRANSFER_TYPE_BIT 0b00000010
- #define SDO_DATA_COUNT_BITS 0b00001100
- #define SDO_TOGGLE_BIT 0b00010000
- #define SDO_CS_BITS 0b11100000
-
- /* SDO macros -----------------------------------------------------------*/
- #define SDO_GET_CS(data0) ((data0 & SDO_CS_BITS) >> 5)
- #define SDO_GET_DATA_COUNT(data0) ((data0 & SDO_DATA_COUNT_BITS) >> 2)
-
-
- /*=========================================================================
- * NMT MESSAGE PARAMETERS
- *=========================================================================
- */
+ state.nmtState = State::INITIALIZED;
- /** NMT node control protocol command specifiers */
- typedef enum {
- NMT_CS_START = 0x01,
- NMT_CS_STOP = 0x02,
- NMT_CS_ENTER_PREOP = 0x80,
- NMT_CS_RESET_NODE = 0x81,
- NMT_CS_RESET_COM = 0x82
- } NmtCommandSpecifier;
+ bLoopbackOn = bLoop;
-Node::Node (ServiceProvider * pProvider)
-{
pMyProvider = pProvider;
- pProvider->AddNode(this);
+ pProvider->AddNode(this);
}
/*=============================================================================
@@ -121,29 +54,41 @@
*=============================================================================
*/
-int Node::DispatchMessage(CanOpenMessage *msg)
+int Node::DispatchMessage(CanOpenMessage *canOpenMsg)
{
- int command = MESSAGE_GET_COMMAND(msg->id);
- int nodeId = MESSAGE_GET_NODEID(msg->id);
+ int command = MESSAGE_GET_COMMAND(canOpenMsg->id);
- //printf("*** N.Dispatch: got com and id\r\n");
-
switch (command) {
case CANOPEN_FUNCTION_CODE_NMT:
- printf("*** N.Dispatch: it's an NMT Control!!!\r\n");
- if (msg->data[0] == nodeId) {
- HandleNodeControl ((int)msg->data[1]);
+ printf(" NMT Control: \r\n");
+ HandleNodeControl(canOpenMsg);
+ break;
+
+ case CANOPEN_FUNCTION_CODE_PDO1T:
+ case CANOPEN_FUNCTION_CODE_PDO1R:
+ case CANOPEN_FUNCTION_CODE_PDO2T:
+ case CANOPEN_FUNCTION_CODE_PDO2R:
+ case CANOPEN_FUNCTION_CODE_PDO3T:
+ case CANOPEN_FUNCTION_CODE_PDO3R:
+ case CANOPEN_FUNCTION_CODE_PDO4T:
+ case CANOPEN_FUNCTION_CODE_PDO4R:
+ if (state.bPDO) {
+ HandlePdo(canOpenMsg);
}
break;
+
+ case CANOPEN_FUNCTION_CODE_SDOT:
+ case CANOPEN_FUNCTION_CODE_SDOR:
+ if (state.bSDO) {
+ HandleSdo(canOpenMsg);
+ }
+ break;
+
default:
- printf("*** N.Dispatch: some random message\r\n");
+ printf(" some random message\r\n");
break;
}
- // ECHO ***********************
- pMyProvider->PostMessage(msg);
- // END ECHO *******************
-
return 1;
}
@@ -152,14 +97,191 @@
*=============================================================================
*/
-int Node::ConsumePdo (const int pdoNum, char *const data)
+static void CopyBits(uint8_t *sourceData,
+ uint8_t *destData,
+ uint8_t mappedBits,
+ uint8_t &sourceBitNum,
+ uint8_t &destBitNum);
+
+int Node::HandlePdo (CanOpenMessage *canOpenMsg)
{
- return 0;
+ printf(" RPDO:\r\n");
+
+ int result = 0;
+
+ if (CANOPEN_TYPE_REMOTE == canOpenMsg->type) {
+
+ } else {
+ int pdoNum = 0;
+ int bSearching = 1;
+
+ ObjectData * parameterObject;
+
+ /* search through PDO parameters until we find a match */
+ while (bSearching) {
+
+ //printf(" PDO %d?", pdoNum);
+ parameterObject = ScanIndex (0x1400 + pdoNum);
+ if (parameterObject) {
+
+ uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData;
+ if (canOpenMsg->id == *cobId) {
+ bSearching = 0;
+
+ //printf(" ... match!!!\r\n");
+ } else {
+ pdoNum++;
+ //printf(" ... nope\r\n");
+ }
+
+ } else {
+ bSearching = 0;
+ printf("\r\n ERROR: No PDO Parameter match found\r\n");
+ }
+
+
+ }
+
+ /* if a matching parameter object was found then get mapping */
+ if (parameterObject) {
+
+ ObjectData *mappingObject = ScanIndex (0x1600 + pdoNum);
+
+ if (mappingObject) {
+ SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData;
+
+ /* variable to track position in data copy */
+ uint8_t sourceBitNum = 0;
+
+ int bError = 0;
+
+ /* for each mapped variable, write in data from message */
+ for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) {
+
+ uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData;
+
+ uint8_t mappedBits = (uint8_t) (*map);
+ uint8_t mappedBytes = mappedBits / 8;
+ SubIndexSize mappedSubIndex = (SubIndexSize) (*map >> 8);
+ IndexSize mappedIndex = (IndexSize) (*map >> 16);
+
+ printf(" mapped: %#06x, %#04x, %d\r\n",
+ mappedIndex, mappedSubIndex, mappedBits);
+
+ /* if less than 0x1000 then it is a dummy value */
+ if (mappedIndex < 0x1000) {
+ sourceBitNum += mappedBits;
+ } else {
+
+ /* get the index object */
+ ObjectData *mappedObject = ScanIndex (mappedIndex);
+ if (mappedObject) {
+
+ /* get the subindex object */
+ if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) {
+
+ EntryData *destinationEntry = &mappedObject->entries[mappedSubIndex];
+
+ /* can we write to it? */
+ if (!(destinationEntry->properties & EntryData::PROPERTY_WRITEABLE)) {
+ printf(" ERROR: Mapped SubIndex is not writeable!\r\n");
+ bError = 1;
+ }
+
+ uint8_t sourceByteNum = sourceBitNum / 8;
+ if (sourceByteNum + mappedBytes > canOpenMsg->dataCount) {
+ printf(" ERROR: Insufficient mapped data remaining!\r\n");
+ bError = 1;
+ }
+
+ if (destinationEntry->size < mappedBytes) {
+ printf(" ERROR: Too much data to pack into destination!\r\n");
+ bError = 1;
+ }
+
+ if (!bError) {
+
+ //printf(" No Errors, copying data...\r\n");
+
+ uint8_t* destData = (uint8_t*) destinationEntry->pData;
+ uint8_t* sourceData = canOpenMsg->data;
+
+ uint8_t destBitNum = 0;
+ CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum);
+ }
+
+ } else {
+ printf(" ERROR: Mapped SubIndex does not exist!\r\n");
+ bError = 1;
+ }
+
+ } else {
+ printf(" ERROR: Mapped Index does not exist!\r\n");
+ bError = 1;
+ }
+
+ } /* if mappedIndex < 0x1000 */
+
+
+ } /* for each mapping */
+
+ if (!bError) {
+ result = 1;
+ }
+
+ } else {
+ printf(" ERROR: No PDO Mapping match found\r\n");
+ }
+
+ } /* if parameter exists */
+
+ } /* if remote message */
+
+ return result;
}
-int Node::HandlePdoReadRequest (const int pdoNum)
+
+int Node::HandleSdo (CanOpenMessage *canOpenMsg)
{
- return 0;
+
+ printf(" SDO:\r\n");
+
+ int result = 0;
+
+ int bSearching = 1;
+
+ int sdoNum = 0;
+ int sdoType = 0; /* 0=unknown, 1=receive/server, 2=transmit/client */
+ ObjectData * sdoObject;
+
+ /* search through PDO parameters until we find a match */
+ while (bSearching) {
+
+ sdoObject = ScanIndex (0x1200 + sdoNum);
+ if (sdoObject) {
+
+ uint32_t *receiveCobId = (uint32_t*) sdoObject->entries[1].pData;
+ uint32_t *transmitCobId = (uint32_t*) sdoObject->entries[2].pData;
+
+ if (canOpenMsg->id == *receiveCobId) {
+ bSearching = 0;
+ sdoType = 1;
+ printf(" Receive SDO\r\n");
+ } else if (canOpenMsg->id == *transmitCobId) {
+ bSearching = 0;
+ sdoType = 2;
+ printf(" Transmit SDO\r\n");
+ }
+
+ } else {
+ printf(" ERROR: No SDO parameters found");
+ bSearching = 0;
+ }
+
+ sdoNum++;
+ }
+
+ return result;
}
int Node::ConsumeEmergency (void)
@@ -167,84 +289,109 @@
return 0;
}
-int Node::HandleNodeControl (int commandSpecifier)
+int Node::HandleNodeControl (CanOpenMessage *canOpenMsg)
{
int result = 0;
- switch (commandSpecifier) {
- case NMT_CS_START:
- if (State::INITIALIZED != state.nmtState) {
- state.nmtState = State::OPERATIONAL;
+ if (canOpenMsg->data[0] == nodeId) {
+
+ int commandSpecifier = (int)canOpenMsg->data[1];
+
+ switch (commandSpecifier) {
+ case NMT_CS_START:
+ printf(" NMT_CS_START\r\n");
+ if ((State::INITIALIZED == state.nmtState) ||
+ (State::PREOPERATIONAL == state.nmtState) ||
+ (State::STOPPED == state.nmtState))
+ {
+
+ state.nmtState = State::OPERATIONAL;
+ state.bBoot = 0;
+ state.bSDO = 1;
+ state.bEmergency = 1;
+ state.bSYNC = 1;
+ state.bLifeGuard = 1;
+ state.bPDO = 1;
+ state.bLSS = 0;
+ OnOperational();
+ result = 1;
+ }
+ break;
+
+ case NMT_CS_STOP:
+ if (State::INITIALIZED != state.nmtState) {
+ state.nmtState = State::STOPPED;
+ state.bBoot = 0;
+ state.bSDO = 0;
+ state.bEmergency = 0;
+ state.bSYNC = 0;
+ state.bLifeGuard = 1;
+ state.bPDO = 0;
+ state.bLSS = 1;
+ }
+ OnStopped();
+ result = 1;
+ break;
+
+ case NMT_CS_ENTER_PREOP:
+ state.nmtState = State::PREOPERATIONAL;
state.bBoot = 0;
state.bSDO = 1;
state.bEmergency = 1;
state.bSYNC = 1;
state.bLifeGuard = 1;
- state.bPDO = 1;
- state.bLSS = 0;
- OnOperational();
+ state.bPDO = 0;
+ state.bLSS = 1;
+ OnPreoperational();
result = 1;
- }
- break;
+ break;
- case NMT_CS_STOP:
- if (State::INITIALIZED != state.nmtState) {
- state.nmtState = State::STOPPED;
- state.bBoot = 0;
+ case NMT_CS_RESET_NODE:
+ case NMT_CS_RESET_COM:
+ printf(" NMT_CS_RESET\r\n");
+
+ state.nmtState = State::INITIALIZED;
+ state.bBoot = 1;
state.bSDO = 0;
state.bEmergency = 0;
state.bSYNC = 0;
+ state.bLifeGuard = 0;
+ state.bPDO = 0;
+ state.bLSS = 0;
+
+ state.bLifeGuardToggle = 0;
+
+ /* boot message is actually just the first node guard/ heart beat message */
+ // TODO: wrap up into heartbeat/lifeguard message
+ CanOpenMessage msgBoot;
+ msgBoot.id = CANOPEN_FUNCTION_CODE_NODE_GUARD | 5;
+ msgBoot.format = CANOPEN_FORMAT_STANDARD;
+ msgBoot.type = CANOPEN_TYPE_DATA;
+ msgBoot.dataCount = 1;
+ msgBoot.data[0] = 0;
+
+ pMyProvider->PostMessage(nodeId, &msgBoot);
+
+ OnInitialize();
+
+ state.nmtState = State::PREOPERATIONAL;
+ state.bBoot = 0;
+ state.bSDO = 1;
+ state.bEmergency = 1;
+ state.bSYNC = 1;
state.bLifeGuard = 1;
state.bPDO = 0;
state.bLSS = 1;
- }
- OnStopped();
- result = 1;
- break;
-
- case NMT_CS_ENTER_PREOP:
- state.nmtState = State::PREOPERATIONAL;
- state.bBoot = 0;
- state.bSDO = 1;
- state.bEmergency = 1;
- state.bSYNC = 1;
- state.bLifeGuard = 1;
- state.bPDO = 0;
- state.bLSS = 1;
- OnPreoperational();
- result = 1;
- break;
+
+ OnPreoperational();
- case NMT_CS_RESET_NODE:
- case NMT_CS_RESET_COM:
-
- state.nmtState = State::INITIALIZED;
- state.bBoot = 1;
- state.bSDO = 0;
- state.bEmergency = 0;
- state.bSYNC = 0;
- state.bLifeGuard = 0;
- state.bPDO = 0;
- state.bLSS = 0;
-
- state.bLifeGuardToggle = 0;
-
- /* boot message is actually just the first node guard/ heart beat message */
- // TODO: wrap up into heartbeat/lifeguard message
- CanOpenMessage msgBoot;
- msgBoot.id = CANOPEN_FUNCTION_CODE_NODE_GUARD | 5;
- msgBoot.format = CANOPEN_FORMAT_STANDARD;
- msgBoot.type = CANOPEN_TYPE_DATA;
- msgBoot.dataCount = 1;
- msgBoot.data[0] = 0;
-
- pMyProvider->PostMessage(&msgBoot);
-
- result = 1;
- break;
+ result = 1;
+ break;
- default:
- break;
+ default:
+ break;
+ }
+
}
return result;
@@ -265,29 +412,256 @@
*=============================================================================
*/
+void Node::FixedUpdate (uint32_t time)
+{
+ timeSinceLastTick = time - timeCurrentTick;
+ timeCurrentTick = time;
+
+ if (State::OPERATIONAL == state.nmtState) {
+ OnFixedUpdate();
+ }
+
+}
+
void Node::Update (void)
{
- if (State::OPERATIONAL == state.nmtState) {
+ if (State::OPERATIONAL == state.nmtState) {
OnUpdate();
}
-
- // TODO: check elapsed time to see if it is time for fixed update
}
/*=============================================================================
- * Methods to implement node control in derived classes
+ * Other Member Functions
*=============================================================================
*/
+int Node::PostTPDO (int specifiedCobId)
+{
+ printf(" TPDO:\r\n");
+
+ int result = 0;
+
+ /* initialize a blank message -------------------------------------------*/
+ CanOpenMessage msg;
+
+ msg.id = specifiedCobId;
+ msg.dataCount = 0;
+ msg.type = CANOPEN_TYPE_DATA;
+ msg.format = CANOPEN_FORMAT_STANDARD;
+
+ msg.data[0] = 0;
+ msg.data[1] = 0;
+ msg.data[2] = 0;
+ msg.data[3] = 0;
+ msg.data[4] = 0;
+ msg.data[5] = 0;
+ msg.data[6] = 0;
+ msg.data[7] = 0;
+
+ int pdoNum = 0;
+ int bSearching = 1;
+
+ /* Find the mapped data and send ----------------------------------------*/
+
+ ObjectData * parameterObject;
+
+ /* search through PDO parameters until we find a match */
+ while (bSearching) {
+
+ //printf(" PDO %d?", pdoNum);
+ parameterObject = ScanIndex (0x1800 + pdoNum);
+ if (parameterObject) {
+
+ uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData;
+ if (specifiedCobId == *cobId) {
+ bSearching = 0;
+
+ //printf(" ... match!!!\r\n");
+ } else {
+ pdoNum++;
+ //printf(" ... nope\r\n");
+ }
+
+ } else {
+ bSearching = 0;
+ printf("\r\n ERROR: No PDO Parameter match found\r\n");
+ }
+ }
+
+ /* if a matching parameter object was found then get mapping */
+ if (parameterObject) {
+
+ ObjectData *mappingObject = ScanIndex (0x1A00 + pdoNum);
+
+ if (mappingObject) {
+ SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData;
+
+ //printf(" numMappedVariables: %d\r\n", *numMappedVariables);
+
+ /* variable to track position in data copy */
+ uint8_t destBitNum = 0;
+
+ int bError = 0;
+
+ /* for each mapped variable, write in data from message */
+ for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) {
+
+ uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData;
+
+ uint8_t mappedBits = (uint8_t) (*map);
+ uint8_t mappedBytes = mappedBits / 8;
+ SubIndexSize mappedSubIndex = (SubIndexSize) (*map >> 8);
+ IndexSize mappedIndex = (IndexSize) (*map >> 16);
+
+ msg.dataCount += mappedBytes;
+
+ //printf(" mapped: %#06x, %#04x, %d\r\n",
+ // mappedIndex, mappedSubIndex, mappedBits);
+
+ /* if less than 0x1000 then it is a dummy value */
+ if (mappedIndex < 0x1000) {
+ destBitNum += mappedBits;
+ //printf(" No Errors, skipping VOID data...\r\n");
+ } else {
+
+ /* push into the TPDO data */
+ ObjectData *mappedObject = ScanIndex (mappedIndex);
+ if (mappedObject) {
+
+ /* get the subindex object */
+ if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) {
+
+ EntryData *sourceEntry = &mappedObject->entries[mappedSubIndex];
+
+ /* can we write to it? */
+ if (!(sourceEntry->properties & EntryData::PROPERTY_READABLE)) {
+ printf(" ERROR: Mapped SubIndex is not readable!\r\n");
+ bError = 1;
+ }
+
+ uint8_t destByteNum = destBitNum / 8;
+ if (destByteNum + mappedBytes > 8) {
+ printf(" ERROR: Too much data to pack into destination!\r\n");
+ bError = 1;
+ }
+
+ if (sourceEntry->size < mappedBytes) {
+ printf(" ERROR: trying to grab too much information!\r\n");
+ bError = 1;
+ }
+
+ if (!bError) {
+
+ //printf(" No Errors, copying data...\r\n");
+
+ uint8_t *destData = msg.data;
+ uint8_t *sourceData = (uint8_t*)sourceEntry->pData;
+
+ uint8_t sourceBitNum = 0;
+ CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum);
+ }
+
+ } else {
+ printf(" ERROR: Mapped SubIndex does not exist!\r\n");
+ bError = 1;
+ }
+
+ } else {
+ printf(" ERROR: Mapped Index does not exist!\r\n");
+ bError = 1;
+ }
+
+ } /* if mappedIndex < 0x1000 */
+
+
+ } /* for each mapping */
+
+ if (!bError) {
+ result = 1;
+
+ /* Send the message we built up */
+ pMyProvider->PostMessage(nodeId, &msg);
+ }
+
+ } else {
+ printf(" ERROR: No PDO Mapping match found\r\n");
+ } /* if mappingObject exists */
+
+ } /* if parameter exists */
+
+
+ return result;
+}
+
+/*=============================================================================
+ * Private functions
+ *=============================================================================
+ */
+
+void ChangeState(int newState) {
+
+}
+
/*=============================================================================
* Local functions
*=============================================================================
*/
+void CopyBits(uint8_t *sourceData,
+ uint8_t *destData,
+ uint8_t mappedBits,
+ uint8_t &sourceBitNum,
+ uint8_t &destBitNum)
+{
+ uint8_t sourceByteNum;
+ uint8_t destByteNum;
+ uint8_t bitCounter = 0;
+
+ if ((mappedBits % 8) == 0 && (sourceBitNum % 8) == 0 && (destBitNum % 8) == 0) {
+
+ //printf(" Loading BYTEwise...\r\n");
+ /* load in by bytes */
+ uint8_t destByteNum = 0;
+ while (bitCounter < mappedBits) {
+
+
+ destByteNum = destBitNum / 8;
+ sourceByteNum = sourceBitNum / 8;
+
+ destData[destByteNum] = sourceData[sourceByteNum];
+
+ destBitNum += 8;
+ sourceBitNum += 8;
+ bitCounter += 8;
+ }
+
+ } else {
+
+
+ //printf(" Loading BITwise...");
+ /* not a multiple of 8, so do bit by bit */
+ while (bitCounter < mappedBits) {
+
+ destByteNum = destBitNum / 8;
+ sourceByteNum = sourceBitNum / 8;
+
+ /* clear the destination bit */
+ destData[destByteNum] &= ~(1 << destBitNum);
+ /* get source bit value */
+ uint8_t destValue = (sourceData[sourceByteNum] & (1 << (sourceBitNum % 8))) >> (sourceBitNum % 8) << destBitNum;
+ /* set dest bit */
+ destData[destByteNum] |= destValue;
+
+ destBitNum++;
+ sourceBitNum++;
+ bitCounter++;
+ }
+ }
+}
/*=============================================================================
- *=============================================================================
+ *=============================================================================
* Methods to handle message requests and responses
* Called by the node, usually during Update() or during handling of
* incoming messages.
@@ -303,45 +677,45 @@
* @note
* @param
*/
-void RequestPdo (int pdoNum){}
+void RequestPdo (int pdoNum) {}
/** Build and send a PDO
* @note
* @param
*/
-void ProducePdo (int pdoNum, char * data){}
+void ProducePdo (int pdoNum, char * data) {}
/* SDO (7.2.4) ----------------------------------------------------------*/
/** initiate SDO download
* @note Handles automatically whether it will be a expedited transfer or
- * or if message will be split into
+ * or if message will be split into
* @param
*
* Node will create a big data array and Service provide will have to
* iterate through and send all of the data. ServiceProvider will pass
* the confirmation to the node, and the node will free up it's buffer.
*/
-void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data){}
+void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data) {}
/** initiate SDO upload
- * @note
+ * @note
* @param
*/
-void UploadSdo (int sdoNum, int index, int subindex){}
+void UploadSdo (int sdoNum, int index, int subindex) {}
/** Acknowledge that SDO was recieved properly
- * @note
+ * @note
* @param
*/
-void ConfirmSdo (int sdoNum, int bSuccess){}
+void ConfirmSdo (int sdoNum, int bSuccess) {}
/** Abort current SDO transfer
- * @note
+ * @note
* @param
*/
-void AbortSdo (int sdoNum){}
+void AbortSdo (int sdoNum) {}
/* Emergency object (7.2.7) ---------------------------------------------*/
@@ -356,7 +730,10 @@
* @note
* @param
*/
-int SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+int SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId)
+{
+ return 0;
+}
/* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
@@ -365,13 +742,19 @@
* @note
* @param
*/
-int RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+int RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId)
+{
+ return 0;
+}
/** Build a CANOpen error control response
* @note
* @param
*/
-int RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+int RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId)
+{
+ return 0;
+}
} /* namspace ppCANOpen */
--- a/source/ServiceProvider.cpp Sat Jan 09 17:15:29 2016 +0000
+++ b/source/ServiceProvider.cpp Sat Feb 13 20:22:59 2016 +0000
@@ -25,11 +25,11 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
+#include "mbed.h"
#include "ServiceProvider.h"
#include "Node.h"
-#include "CanOpenHandle.h"
#include "canopen_api.h"
#include <string.h>
@@ -37,107 +37,233 @@
namespace ppCANOpen
{
-
-/*=============================================================================
- * Construction
- *=============================================================================
- */
+
+
-ServiceProvider::ServiceProvider (void) : nodeCount(0)
+ServiceProvider::ServiceProvider (void) : nodeCount(0)
{
-
- //messageCount = 0;
-
+
+ bInboxUnlocked = 1;
+
//memset (nodes, 0, sizeof(Node) * SERVICE_MAX_NODES ); // TODO: fix incorrect <--
for (int i = 0; i < SERVICE_MAX_NODES; i++) {
- nodes[i] = 0;
+ nodes[i] = 0;
}
-
- hCanOpen = new CanOpenHandle;
- CanOpenApiInit(hCanOpen);
+
+ CanOpenApiInit((void *)this, &ReadIT_CWrapper, &UpdateNodes_CWrapper);
}
ServiceProvider::~ServiceProvider (void)
{
- delete hCanOpen;
}
-/*=============================================================================
- * main loop
- *=============================================================================
- */
+
+/* ============================================================================
+ * Public Methods
+ * ============================================================================
+ */
+
+
+/* Main application ---------------------------------------------------------*/
void ServiceProvider::Run (void)
{
- while (1) {
+ //while (1) {
+
+
+ /* Check for new messages.
+ * Dispatch one at a time in order for nodes to handle incoming data
+ */
+ bInboxUnlocked = 0;
+ if (! inbox.empty()) {
+
+ /* Handle things inside of Service Provider first if necessary */
+ if (CANOPEN_FUNCTION_CODE_TIME == MESSAGE_GET_COMMAND(inbox.front().id)) {
+
+ HandleTimeMessage(&inbox.front());
+
+ } else {
+ /* Send out remaining messages to the Nodes */
- /* Check for new messages */
- CanOpenMessage msg;
- if (hCanOpen->can) {
- if (CanOpenApiRead (hCanOpen, &msg)) {
-
- //printf("*** SP.Run: About to dispatch\r\n");
- /* if new message exists, give it to the nodes*/
- /* update all of the nodes */
- for (int i=0; i < nodeCount; i++) {
- nodes[i]->DispatchMessage(&msg);
- }
- //printf("*** SP.Run: dispatched\r\n");
- }
- }
-
- /* update all of the nodes */
- for (int i=0; i < nodeCount; i++) {
- nodes[i]->Update();
- }
-
- /* send out the responses */
- while (! outbox.empty()) {
-
- printf("msg id: %d\r\n", outbox.front().id);
- if (CanOpenApiWrite(hCanOpen, &outbox.front())) {
- outbox.pop();
- } else {
- printf("Could not send last message. Trying again\r\n");
+ // TODO if time stamp message, then the SP can sync the clock for all nodes
+
+ /* if new message exists, give it to the nodes*/
+ for (int i=0; i < nodeCount; i++) {
+ nodes[i]->DispatchMessage(&inbox.front());
}
}
- }
+ inbox.pop();
+ }
+ bInboxUnlocked = 1;
+
+ /* Update all of the nodes */
+ for (int i=0; i < nodeCount; i++) {
+ nodes[i]->Update();
+ }
+
+ /* send out the responses */
+ while (! outbox.empty()) {
+ if (CanOpenApiWrite(&outbox.front().message)) {
+ /* send message externally first.
+ * Since we want to keep the message alive and try to resend, we
+ * do not want to keep resending the message internally over and
+ * over again.*/
+
+ /* dispatch to the internal nodes */
+ for (int i=0; i < nodeCount; i++) {
+ if ((nodes[i]->bLoopbackOn) ||
+ (nodes[i]->nodeId != outbox.front().nodeId)) {
+
+ nodes[i]->DispatchMessage(&outbox.front().message);
+ }
+ }
+
+ outbox.pop();
+ } else {
+ printf("Could not send last message. Trying again\r\n");
+ }
+ }
+
+ wait (.005);
+
+ //}
}
-/*=============================================================================
- * Register a node to get messages and get update calls
- *=============================================================================
- */
+void ServiceProvider::UpdateNodes(void)
+{
+ uint32_t time = GetTime();
+
+ //printf("S::Run() update nodes\r\n");
+ /* update all of the nodes */
+ for (int i=0; i < nodeCount; i++) {
+ nodes[i]->FixedUpdate(time);
+ }
+}
-int ServiceProvider::AddNode (Node * node)
+void ServiceProvider::UpdateNodes_CWrapper(void *pServiceObject)
+{
+ ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
+ pTempProvider->UpdateNodes();
+}
+
+
+/* Node Management ----------------------------------------------------------*/
+
+int ServiceProvider::AddNode (Node * node)
{
int result = 1;
-
+
if (nodeCount < SERVICE_MAX_NODES) {
nodes[nodeCount++] = node;
} else {
result = 0;
}
-
+
return result;
}
-/* ========================================================================
- * Adds a message to the message queue outbox
- * ========================================================================
+
+/* CanMessage Transmission --------------------------------------------------*/
+
+void ServiceProvider::PostMessage (int nodeId, CanOpenMessage * msg)
+{
+ outbox.push(InternalMessage(nodeId, *msg));
+}
+
+void ServiceProvider::PostNmtControl (char targetNodeId, NmtCommandSpecifier cs)
+{
+ CanOpenMessage msg;
+
+ msg.id = 0;
+ msg.dataCount = 2;
+ msg.type = CANOPEN_TYPE_DATA;
+ msg.format = CANOPEN_FORMAT_STANDARD;
+
+ // NOTE: memcpy is freezing execution
+ //memcpy(msg.data, canMsg.data, canMsg.len);
+ msg.data[0] = targetNodeId;
+ msg.data[1] = (char) cs;
+ //msg.data[2] =
+ //msg.data[3] =
+ //msg.data[4] =
+ //msg.data[5] =
+ //msg.data[6] =
+ //msg.data[7] =
+
+ PostMessage(0, &msg);
+}
+
+
+/* ============================================================================
+ * Private Methods
+ * ============================================================================
*/
-void ServiceProvider::PostMessage (CanOpenMessage * msg)
+
+/* CanMessage Reading -------------------------------------------------------*/
+
+void ServiceProvider::ReadIT(void)
+{
+
+ //printf("S::ReadIT()\r\n");
+
+ CanOpenMessage msg;
+ if(CanOpenApiRead (&msg)) {
+ if (bInboxUnlocked) {
+ ReadIT_clearBuffer();
+ inbox.push(msg);
+ } else {
+ inboxBuffer.push(msg);
+ printf("!!!!!!!!!!!!!!!!!!!!!!!INBOX LOCKED!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");
+ }
+ }
+}
+
+void ServiceProvider::ReadIT_clearBuffer(void)
+{
+ while (! inboxBuffer.empty()) {
+ inbox.push (inboxBuffer.front());
+ inboxBuffer.pop();
+ }
+}
+
+void ServiceProvider::ReadIT_CWrapper(void *pServiceObject)
{
- outbox.push(*msg);
+ ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
+ pTempProvider->ReadIT();
+}
+
+
+/* Elapsed Time -------------------------------------------------------------*/
+
+uint32_t ServiceProvider::GetTime(void)
+{
+ return (CanOpenApiGetHardwareTime() + elapsedTimeOffset) & 0x0FFFFFFF;
}
+void ServiceProvider::HandleTimeMessage(CanOpenMessage *canOpenMsg)
+{
+ if (6 == canOpenMsg->dataCount) {
+
+ uint32_t timeStamp_ms = (uint32_t)canOpenMsg->data[0] +
+ (uint32_t)canOpenMsg->data[1] << 8 +
+ (uint32_t)canOpenMsg->data[2] << 16 +
+ (uint32_t)canOpenMsg->data[3] << 24;
+
+ // Nothing uses Days yet, going to ignore for now.
+ // uint32_t timeStamp_days = (uint32_t)canOpenMsg->data[4] +
+ // (uint32_t)canOpenMsg->data[5] << 8;
+
+ uint32_t hwTime = CanOpenApiGetHardwareTime() & 0x0FFFFFFF;
+
+ elapsedTimeOffset = ((int32_t)timeStamp_ms) - ((int32_t)hwTime);
+
+ // Else TODO: send error
+ }
+}
+
+
+
} /* namspace ppCANOpen */
-
-
-
-
-
-