Paul Paterson / ppCANOpen

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

source/ServiceProvider.cpp@5:22a337cdc0e3, 2016-02-13 (annotated)

Committer:
ptpaterson
Date:
Sat Feb 13 20:22:59 2016 +0000
Revision:
5:22a337cdc0e3
Parent:
4:2034b04c86d2 
PDO receive complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ptpaterson 2:c724ff3a4e4d 1 /**
ptpaterson 2:c724ff3a4e4d 2 ******************************************************************************
ptpaterson 2:c724ff3a4e4d 3 * @file
ptpaterson 2:c724ff3a4e4d 4 * @author Paul Paterson
ptpaterson 2:c724ff3a4e4d 5 * @version
ptpaterson 2:c724ff3a4e4d 6 * @date 2015-12-22
ptpaterson 2:c724ff3a4e4d 7 * @brief CANOpen implementation library
ptpaterson 2:c724ff3a4e4d 8 ******************************************************************************
ptpaterson 2:c724ff3a4e4d 9 * @attention
ptpaterson 2:c724ff3a4e4d 10 *
ptpaterson 2:c724ff3a4e4d 11 * <h2><center>&copy; COPYRIGHT(c) 2015 Paul Paterson
ptpaterson 2:c724ff3a4e4d 12 *
ptpaterson 2:c724ff3a4e4d 13 * All rights reserved.
ptpaterson 2:c724ff3a4e4d 14
ptpaterson 2:c724ff3a4e4d 15 This program is free software: you can redistribute it and/or modify
ptpaterson 2:c724ff3a4e4d 16 it under the terms of the GNU General Public License as published by
ptpaterson 2:c724ff3a4e4d 17 the Free Software Foundation, either version 3 of the License, or
ptpaterson 2:c724ff3a4e4d 18 (at your option) any later version.
ptpaterson 2:c724ff3a4e4d 19
ptpaterson 2:c724ff3a4e4d 20 This program is distributed in the hope that it will be useful,
ptpaterson 2:c724ff3a4e4d 21 but WITHOUT ANY WARRANTY; without even the implied warranty of
ptpaterson 2:c724ff3a4e4d 22 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
ptpaterson 2:c724ff3a4e4d 23 GNU General Public License for more details.
ptpaterson 2:c724ff3a4e4d 24
ptpaterson 2:c724ff3a4e4d 25 You should have received a copy of the GNU General Public License
ptpaterson 2:c724ff3a4e4d 26 along with this program. If not, see <http://www.gnu.org/licenses/>.
ptpaterson 2:c724ff3a4e4d 27 */
ptpaterson 5:22a337cdc0e3 28 #include "mbed.h"
ptpaterson 2:c724ff3a4e4d 29
ptpaterson 2:c724ff3a4e4d 30 #include "ServiceProvider.h"
ptpaterson 2:c724ff3a4e4d 31 #include "Node.h"
ptpaterson 2:c724ff3a4e4d 32
ptpaterson 2:c724ff3a4e4d 33 #include "canopen_api.h"
ptpaterson 2:c724ff3a4e4d 34
ptpaterson 2:c724ff3a4e4d 35 #include <string.h>
ptpaterson 3:12b3c25bdeba 36 #include <stdio.h>
ptpaterson 2:c724ff3a4e4d 37
ptpaterson 2:c724ff3a4e4d 38 namespace ppCANOpen
ptpaterson 2:c724ff3a4e4d 39 {
ptpaterson 5:22a337cdc0e3 40
ptpaterson 5:22a337cdc0e3 41
ptpaterson 4:2034b04c86d2 42
ptpaterson 5:22a337cdc0e3 43 ServiceProvider::ServiceProvider (void) : nodeCount(0)
ptpaterson 2:c724ff3a4e4d 44 {
ptpaterson 5:22a337cdc0e3 45
ptpaterson 5:22a337cdc0e3 46 bInboxUnlocked = 1;
ptpaterson 5:22a337cdc0e3 47
ptpaterson 3:12b3c25bdeba 48 //memset (nodes, 0, sizeof(Node) * SERVICE_MAX_NODES ); // TODO: fix incorrect <--
ptpaterson 3:12b3c25bdeba 49 for (int i = 0; i < SERVICE_MAX_NODES; i++) {
ptpaterson 5:22a337cdc0e3 50 nodes[i] = 0;
ptpaterson 3:12b3c25bdeba 51 }
ptpaterson 5:22a337cdc0e3 52
ptpaterson 5:22a337cdc0e3 53 CanOpenApiInit((void *)this, &ReadIT_CWrapper, &UpdateNodes_CWrapper);
ptpaterson 3:12b3c25bdeba 54 }
ptpaterson 3:12b3c25bdeba 55
ptpaterson 3:12b3c25bdeba 56 ServiceProvider::~ServiceProvider (void)
ptpaterson 3:12b3c25bdeba 57 {
ptpaterson 2:c724ff3a4e4d 58 }
ptpaterson 2:c724ff3a4e4d 59
ptpaterson 5:22a337cdc0e3 60
ptpaterson 5:22a337cdc0e3 61 /* ============================================================================
ptpaterson 5:22a337cdc0e3 62 * Public Methods
ptpaterson 5:22a337cdc0e3 63 * ============================================================================
ptpaterson 5:22a337cdc0e3 64 */
ptpaterson 5:22a337cdc0e3 65
ptpaterson 5:22a337cdc0e3 66
ptpaterson 5:22a337cdc0e3 67 /* Main application ---------------------------------------------------------*/
ptpaterson 2:c724ff3a4e4d 68
ptpaterson 2:c724ff3a4e4d 69 void ServiceProvider::Run (void)
ptpaterson 2:c724ff3a4e4d 70 {
ptpaterson 5:22a337cdc0e3 71 //while (1) {
ptpaterson 5:22a337cdc0e3 72
ptpaterson 5:22a337cdc0e3 73
ptpaterson 5:22a337cdc0e3 74 /* Check for new messages.
ptpaterson 5:22a337cdc0e3 75 * Dispatch one at a time in order for nodes to handle incoming data
ptpaterson 5:22a337cdc0e3 76 */
ptpaterson 5:22a337cdc0e3 77 bInboxUnlocked = 0;
ptpaterson 5:22a337cdc0e3 78 if (! inbox.empty()) {
ptpaterson 5:22a337cdc0e3 79
ptpaterson 5:22a337cdc0e3 80 /* Handle things inside of Service Provider first if necessary */
ptpaterson 5:22a337cdc0e3 81 if (CANOPEN_FUNCTION_CODE_TIME == MESSAGE_GET_COMMAND(inbox.front().id)) {
ptpaterson 5:22a337cdc0e3 82
ptpaterson 5:22a337cdc0e3 83 HandleTimeMessage(&inbox.front());
ptpaterson 5:22a337cdc0e3 84
ptpaterson 5:22a337cdc0e3 85 } else {
ptpaterson 5:22a337cdc0e3 86 /* Send out remaining messages to the Nodes */
ptpaterson 3:12b3c25bdeba 87
ptpaterson 5:22a337cdc0e3 88 // TODO if time stamp message, then the SP can sync the clock for all nodes
ptpaterson 5:22a337cdc0e3 89
ptpaterson 5:22a337cdc0e3 90 /* if new message exists, give it to the nodes*/
ptpaterson 5:22a337cdc0e3 91 for (int i=0; i < nodeCount; i++) {
ptpaterson 5:22a337cdc0e3 92 nodes[i]->DispatchMessage(&inbox.front());
ptpaterson 4:2034b04c86d2 93 }
ptpaterson 4:2034b04c86d2 94 }
ptpaterson 4:2034b04c86d2 95
ptpaterson 5:22a337cdc0e3 96 inbox.pop();
ptpaterson 5:22a337cdc0e3 97 }
ptpaterson 5:22a337cdc0e3 98 bInboxUnlocked = 1;
ptpaterson 5:22a337cdc0e3 99
ptpaterson 5:22a337cdc0e3 100 /* Update all of the nodes */
ptpaterson 5:22a337cdc0e3 101 for (int i=0; i < nodeCount; i++) {
ptpaterson 5:22a337cdc0e3 102 nodes[i]->Update();
ptpaterson 5:22a337cdc0e3 103 }
ptpaterson 5:22a337cdc0e3 104
ptpaterson 5:22a337cdc0e3 105 /* send out the responses */
ptpaterson 5:22a337cdc0e3 106 while (! outbox.empty()) {
ptpaterson 5:22a337cdc0e3 107 if (CanOpenApiWrite(&outbox.front().message)) {
ptpaterson 5:22a337cdc0e3 108 /* send message externally first.
ptpaterson 5:22a337cdc0e3 109 * Since we want to keep the message alive and try to resend, we
ptpaterson 5:22a337cdc0e3 110 * do not want to keep resending the message internally over and
ptpaterson 5:22a337cdc0e3 111 * over again.*/
ptpaterson 5:22a337cdc0e3 112
ptpaterson 5:22a337cdc0e3 113 /* dispatch to the internal nodes */
ptpaterson 5:22a337cdc0e3 114 for (int i=0; i < nodeCount; i++) {
ptpaterson 5:22a337cdc0e3 115 if ((nodes[i]->bLoopbackOn) ||
ptpaterson 5:22a337cdc0e3 116 (nodes[i]->nodeId != outbox.front().nodeId)) {
ptpaterson 5:22a337cdc0e3 117
ptpaterson 5:22a337cdc0e3 118 nodes[i]->DispatchMessage(&outbox.front().message);
ptpaterson 5:22a337cdc0e3 119 }
ptpaterson 5:22a337cdc0e3 120 }
ptpaterson 5:22a337cdc0e3 121
ptpaterson 5:22a337cdc0e3 122 outbox.pop();
ptpaterson 5:22a337cdc0e3 123 } else {
ptpaterson 5:22a337cdc0e3 124 printf("Could not send last message. Trying again\r\n");
ptpaterson 5:22a337cdc0e3 125 }
ptpaterson 5:22a337cdc0e3 126 }
ptpaterson 5:22a337cdc0e3 127
ptpaterson 5:22a337cdc0e3 128 wait (.005);
ptpaterson 5:22a337cdc0e3 129
ptpaterson 5:22a337cdc0e3 130 //}
ptpaterson 2:c724ff3a4e4d 131 }
ptpaterson 2:c724ff3a4e4d 132
ptpaterson 5:22a337cdc0e3 133 void ServiceProvider::UpdateNodes(void)
ptpaterson 5:22a337cdc0e3 134 {
ptpaterson 5:22a337cdc0e3 135 uint32_t time = GetTime();
ptpaterson 5:22a337cdc0e3 136
ptpaterson 5:22a337cdc0e3 137 //printf("S::Run() update nodes\r\n");
ptpaterson 5:22a337cdc0e3 138 /* update all of the nodes */
ptpaterson 5:22a337cdc0e3 139 for (int i=0; i < nodeCount; i++) {
ptpaterson 5:22a337cdc0e3 140 nodes[i]->FixedUpdate(time);
ptpaterson 5:22a337cdc0e3 141 }
ptpaterson 5:22a337cdc0e3 142 }
ptpaterson 2:c724ff3a4e4d 143
ptpaterson 5:22a337cdc0e3 144 void ServiceProvider::UpdateNodes_CWrapper(void *pServiceObject)
ptpaterson 5:22a337cdc0e3 145 {
ptpaterson 5:22a337cdc0e3 146 ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
ptpaterson 5:22a337cdc0e3 147 pTempProvider->UpdateNodes();
ptpaterson 5:22a337cdc0e3 148 }
ptpaterson 5:22a337cdc0e3 149
ptpaterson 5:22a337cdc0e3 150
ptpaterson 5:22a337cdc0e3 151 /* Node Management ----------------------------------------------------------*/
ptpaterson 5:22a337cdc0e3 152
ptpaterson 5:22a337cdc0e3 153 int ServiceProvider::AddNode (Node * node)
ptpaterson 2:c724ff3a4e4d 154 {
ptpaterson 2:c724ff3a4e4d 155 int result = 1;
ptpaterson 5:22a337cdc0e3 156
ptpaterson 2:c724ff3a4e4d 157 if (nodeCount < SERVICE_MAX_NODES) {
ptpaterson 2:c724ff3a4e4d 158 nodes[nodeCount++] = node;
ptpaterson 2:c724ff3a4e4d 159 } else {
ptpaterson 2:c724ff3a4e4d 160 result = 0;
ptpaterson 2:c724ff3a4e4d 161 }
ptpaterson 5:22a337cdc0e3 162
ptpaterson 2:c724ff3a4e4d 163 return result;
ptpaterson 2:c724ff3a4e4d 164 }
ptpaterson 2:c724ff3a4e4d 165
ptpaterson 5:22a337cdc0e3 166
ptpaterson 5:22a337cdc0e3 167 /* CanMessage Transmission --------------------------------------------------*/
ptpaterson 5:22a337cdc0e3 168
ptpaterson 5:22a337cdc0e3 169 void ServiceProvider::PostMessage (int nodeId, CanOpenMessage * msg)
ptpaterson 5:22a337cdc0e3 170 {
ptpaterson 5:22a337cdc0e3 171 outbox.push(InternalMessage(nodeId, *msg));
ptpaterson 5:22a337cdc0e3 172 }
ptpaterson 5:22a337cdc0e3 173
ptpaterson 5:22a337cdc0e3 174 void ServiceProvider::PostNmtControl (char targetNodeId, NmtCommandSpecifier cs)
ptpaterson 5:22a337cdc0e3 175 {
ptpaterson 5:22a337cdc0e3 176 CanOpenMessage msg;
ptpaterson 5:22a337cdc0e3 177
ptpaterson 5:22a337cdc0e3 178 msg.id = 0;
ptpaterson 5:22a337cdc0e3 179 msg.dataCount = 2;
ptpaterson 5:22a337cdc0e3 180 msg.type = CANOPEN_TYPE_DATA;
ptpaterson 5:22a337cdc0e3 181 msg.format = CANOPEN_FORMAT_STANDARD;
ptpaterson 5:22a337cdc0e3 182
ptpaterson 5:22a337cdc0e3 183 // NOTE: memcpy is freezing execution
ptpaterson 5:22a337cdc0e3 184 //memcpy(msg.data, canMsg.data, canMsg.len);
ptpaterson 5:22a337cdc0e3 185 msg.data[0] = targetNodeId;
ptpaterson 5:22a337cdc0e3 186 msg.data[1] = (char) cs;
ptpaterson 5:22a337cdc0e3 187 //msg.data[2] =
ptpaterson 5:22a337cdc0e3 188 //msg.data[3] =
ptpaterson 5:22a337cdc0e3 189 //msg.data[4] =
ptpaterson 5:22a337cdc0e3 190 //msg.data[5] =
ptpaterson 5:22a337cdc0e3 191 //msg.data[6] =
ptpaterson 5:22a337cdc0e3 192 //msg.data[7] =
ptpaterson 5:22a337cdc0e3 193
ptpaterson 5:22a337cdc0e3 194 PostMessage(0, &msg);
ptpaterson 5:22a337cdc0e3 195 }
ptpaterson 5:22a337cdc0e3 196
ptpaterson 5:22a337cdc0e3 197
ptpaterson 5:22a337cdc0e3 198 /* ============================================================================
ptpaterson 5:22a337cdc0e3 199 * Private Methods
ptpaterson 5:22a337cdc0e3 200 * ============================================================================
ptpaterson 2:c724ff3a4e4d 201 */
ptpaterson 2:c724ff3a4e4d 202
ptpaterson 5:22a337cdc0e3 203
ptpaterson 5:22a337cdc0e3 204 /* CanMessage Reading -------------------------------------------------------*/
ptpaterson 5:22a337cdc0e3 205
ptpaterson 5:22a337cdc0e3 206 void ServiceProvider::ReadIT(void)
ptpaterson 5:22a337cdc0e3 207 {
ptpaterson 5:22a337cdc0e3 208
ptpaterson 5:22a337cdc0e3 209 //printf("S::ReadIT()\r\n");
ptpaterson 5:22a337cdc0e3 210
ptpaterson 5:22a337cdc0e3 211 CanOpenMessage msg;
ptpaterson 5:22a337cdc0e3 212 if(CanOpenApiRead (&msg)) {
ptpaterson 5:22a337cdc0e3 213 if (bInboxUnlocked) {
ptpaterson 5:22a337cdc0e3 214 ReadIT_clearBuffer();
ptpaterson 5:22a337cdc0e3 215 inbox.push(msg);
ptpaterson 5:22a337cdc0e3 216 } else {
ptpaterson 5:22a337cdc0e3 217 inboxBuffer.push(msg);
ptpaterson 5:22a337cdc0e3 218 printf("!!!!!!!!!!!!!!!!!!!!!!!INBOX LOCKED!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");
ptpaterson 5:22a337cdc0e3 219 }
ptpaterson 5:22a337cdc0e3 220 }
ptpaterson 5:22a337cdc0e3 221 }
ptpaterson 5:22a337cdc0e3 222
ptpaterson 5:22a337cdc0e3 223 void ServiceProvider::ReadIT_clearBuffer(void)
ptpaterson 5:22a337cdc0e3 224 {
ptpaterson 5:22a337cdc0e3 225 while (! inboxBuffer.empty()) {
ptpaterson 5:22a337cdc0e3 226 inbox.push (inboxBuffer.front());
ptpaterson 5:22a337cdc0e3 227 inboxBuffer.pop();
ptpaterson 5:22a337cdc0e3 228 }
ptpaterson 5:22a337cdc0e3 229 }
ptpaterson 5:22a337cdc0e3 230
ptpaterson 5:22a337cdc0e3 231 void ServiceProvider::ReadIT_CWrapper(void *pServiceObject)
ptpaterson 2:c724ff3a4e4d 232 {
ptpaterson 5:22a337cdc0e3 233 ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
ptpaterson 5:22a337cdc0e3 234 pTempProvider->ReadIT();
ptpaterson 5:22a337cdc0e3 235 }
ptpaterson 5:22a337cdc0e3 236
ptpaterson 5:22a337cdc0e3 237
ptpaterson 5:22a337cdc0e3 238 /* Elapsed Time -------------------------------------------------------------*/
ptpaterson 5:22a337cdc0e3 239
ptpaterson 5:22a337cdc0e3 240 uint32_t ServiceProvider::GetTime(void)
ptpaterson 5:22a337cdc0e3 241 {
ptpaterson 5:22a337cdc0e3 242 return (CanOpenApiGetHardwareTime() + elapsedTimeOffset) & 0x0FFFFFFF;
ptpaterson 2:c724ff3a4e4d 243 }
ptpaterson 2:c724ff3a4e4d 244
ptpaterson 5:22a337cdc0e3 245 void ServiceProvider::HandleTimeMessage(CanOpenMessage *canOpenMsg)
ptpaterson 5:22a337cdc0e3 246 {
ptpaterson 5:22a337cdc0e3 247 if (6 == canOpenMsg->dataCount) {
ptpaterson 5:22a337cdc0e3 248
ptpaterson 5:22a337cdc0e3 249 uint32_t timeStamp_ms = (uint32_t)canOpenMsg->data[0] +
ptpaterson 5:22a337cdc0e3 250 (uint32_t)canOpenMsg->data[1] << 8 +
ptpaterson 5:22a337cdc0e3 251 (uint32_t)canOpenMsg->data[2] << 16 +
ptpaterson 5:22a337cdc0e3 252 (uint32_t)canOpenMsg->data[3] << 24;
ptpaterson 5:22a337cdc0e3 253
ptpaterson 5:22a337cdc0e3 254 // Nothing uses Days yet, going to ignore for now.
ptpaterson 5:22a337cdc0e3 255 // uint32_t timeStamp_days = (uint32_t)canOpenMsg->data[4] +
ptpaterson 5:22a337cdc0e3 256 // (uint32_t)canOpenMsg->data[5] << 8;
ptpaterson 5:22a337cdc0e3 257
ptpaterson 5:22a337cdc0e3 258 uint32_t hwTime = CanOpenApiGetHardwareTime() & 0x0FFFFFFF;
ptpaterson 5:22a337cdc0e3 259
ptpaterson 5:22a337cdc0e3 260 elapsedTimeOffset = ((int32_t)timeStamp_ms) - ((int32_t)hwTime);
ptpaterson 5:22a337cdc0e3 261
ptpaterson 5:22a337cdc0e3 262 // Else TODO: send error
ptpaterson 5:22a337cdc0e3 263 }
ptpaterson 5:22a337cdc0e3 264 }
ptpaterson 5:22a337cdc0e3 265
ptpaterson 5:22a337cdc0e3 266
ptpaterson 5:22a337cdc0e3 267
ptpaterson 2:c724ff3a4e4d 268 } /* namspace ppCANOpen */
ptpaterson 2:c724ff3a4e4d 269