/
canopen.c
408 lines (387 loc) · 12.9 KB
/
canopen.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* 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 <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usefull_macros.h>
#include "canopen.h"
static const abortcodes AC[] = {
//while read l; do N=$(echo $l|awk '{print $1 $2}'); R=$(echo $l|awk '{$1=$2=""; print substr($0,3)}'|sed 's/\.//'); echo -e "{0x$N, \"$R\"},"; done < codes.b
{0x05030000, "Toggle bit not alternated"},
{0x05040000, "SDO protocol timed out"},
{0x05040001, "Client/server command specifier not valid or unknown"},
{0x05040002, "Invalid block size (block mode only)"},
{0x05040003, "Invalid sequence number (block mode only)"},
{0x05040004, "CRC error (block mode only)"},
{0x05040005, "Out of memory"},
{0x06010000, "Unsupported access to an object"},
{0x06010001, "Attempt to read a write only object"},
{0x06010002, "Attempt to write a read only object"},
{0x06020000, "Object does not exist in the object dictionary"},
{0x06040041, "Object cannot be mapped to the PDO"},
{0x06040042, "The number and length of the objects to be mapped would exceed PDO length"},
{0x06040043, "General parameter incompatibility reason"},
{0x06040047, "General internal incompatibility in the device"},
{0x06060000, "Access failed due to a hardware error"},
{0x06070010, "Data type does not match; length of service parameter does not match"},
{0x06070012, "Data type does not match; length of service parameter too high"},
{0x06070013, "Data type does not match; length of service parameter too low"},
{0x06090011, "Sub-index does not exist"},
{0x06090030, "Value range of parameter exceeded (only for write access)"},
{0x06090031, "Value of parameter written too high"},
{0x06090032, "Value of parameter written too low"},
{0x06090036, "Maximum value is less than minimum value"},
{0x08000000, "General error"},
{0x08000020, "Data cannot be transferred or stored to the application"},
{0x08000021, "Data cannot be transferred or stored to the application because of local control"},
{0x08000022, "Data cannot be transferred or stored to the application because of the present device state"},
{0x08000023, "Object dictionary dynamic generation fails or no object dictionary is present"},
};
static const int ACmax = sizeof(AC)/sizeof(abortcodes) - 1;
// used
/**
* @brief abortcode_search - explanation of abort code
* @param abortcode - code
* @return abortcode found or NULL
*/
static const abortcodes *abortcode_search(uint32_t abortcode){ //, int *n){
int idx = ACmax/2, min_ = 0, max_ = ACmax, newidx = 0, iter=0;
do{
++iter;
uint32_t c = AC[idx].code;
if(c == abortcode){
return &AC[idx];
}else if(c > abortcode){
newidx = (idx + min_)/2;
max_ = idx;
}else{
newidx = (idx + max_ + 1)/2;
min_ = idx;
}
if(newidx == idx || min_ < 0 || max_ > ACmax){
return NULL;
}
idx = newidx;
}while(1);
}
// used
/**
* @brief mkMesg - make CAN message from sdo object
* @param sdo (i) - sdo object to transform
* @param mesg (o) - output CANmesg
* @return pointer to mesg
*/
CANmesg *mkMesg(SDO *sdo, CANmesg *mesg){
if(!sdo || !mesg) return NULL;
/*DBG("Got SDO NID=%d, idx=0x%x, subidx=%d, len=%d, data={%x %x %x %x}",
sdo->NID, sdo->index, sdo->subindex, sdo->datalen,
sdo->data[0], sdo->data[1], sdo->data[2], sdo->data[3]);*/
memset(mesg, 0, sizeof(CANmesg));
mesg->ID = RSDO_COBID + sdo->NID;
mesg->len = 8;
memset(mesg->data, 0, 8);
mesg->data[0] = SDO_CCS(sdo->ccs);
if(sdo->datalen){ // send N bytes of data
mesg->data[0] |= SDO_N(sdo->datalen) | SDO_E | SDO_S;
for(uint8_t i = 0; i < sdo->datalen; ++i) mesg->data[4+i] = sdo->data[i];
}
mesg->data[1] = sdo->index & 0xff; // l
mesg->data[2] = (sdo->index >> 8) & 0xff; // h
mesg->data[3] = sdo->subindex;
#if 0
DBG("MESG: ID=0x%X, data=", mesg->ID);
for(int x = 0; x < 8; ++x) fprintf(stderr, "0x%02X ", mesg->data[x]);
fprintf(stderr, "\n");
#endif
return mesg;
}
// used
/**
* @brief parseSDO - transform CAN-message to SDO
* @param mesg (i) - message
* @param sdo (o) - SDO
* @return sdo or NULL depending on result
*/
SDO *parseSDO(const CANmesg *mesg, SDO *sdo){
if(mesg->len != 8){
WARNX("Wrong SDO data length");
return NULL;
}
uint16_t cobid = mesg->ID & COBID_MASK;
if(cobid != TSDO_COBID){
DBG("cobid=0x%X, not a TSDO!", cobid);
return NULL; // not a transmit SDO
}
sdo->NID = mesg->ID & NODEID_MASK;
uint8_t spec = mesg->data[0];
sdo->ccs = GET_CCS(spec);
sdo->index = (uint16_t)mesg->data[1] | ((uint16_t)mesg->data[2] << 8);
sdo->subindex = mesg->data[3];
if((spec & SDO_E) && (spec & SDO_S)) sdo->datalen = SDO_datalen(spec);
else if(sdo->ccs == CCS_ABORT_TRANSFER) sdo->datalen = 4; // error code
else sdo->datalen = 0; // no data in message
for(uint8_t i = 0; i < 4; ++i) sdo->data[i] = mesg->data[4+i];
DBG("Got TSDO from NID=%d, ccs=%u, index=0x%X, subindex=0x%X, datalen=%d, data=[%x %x %x %x]",
sdo->NID, sdo->ccs, sdo->index, sdo->subindex, sdo->datalen,
sdo->data[0], sdo->data[1], sdo->data[2], sdo->data[3]);
return sdo;
}
#if 0
// send request to read SDO
static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
SDO sdo = {
.NID = NID,
.ccs = CCS_INIT_UPLOAD,
.datalen = 0,
.index = idx,
.subindex = subidx
};
CANmesg mesg;
return canbus_write(mkMesg(&sdo, &mesg));
}
static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){
FNAME();
int found = 0;
CANmesg mesg;
double t0 = dtime();
while(dtime() - t0 < SDO_ANS_TIMEOUT){
mesg.ID = TSDO_COBID | NID; // read only from given ID
if(canbus_read(&mesg)){
continue;
}
if(!parseSDO(&mesg, sdo)) continue;
if(sdo->index == idx && sdo->subindex == subidx){
found = 1;
break;
}
}
if(!found){
WARNX("No answer from SDO 0x%X/0x%X", idx, subidx);
return NULL;
}
return sdo;
}
/**
* @brief readSDOvalue - send request to SDO read
* @param idx - SDO index
* @param subidx - SDO subindex
* @param NID - target node ID
* @param sdo (i)- SDO to fit
* @return SDO received or NULL if error
*/
SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){
FNAME();
if(ask2read(idx, subidx, NID)){
WARNX("readSDOvalue(): Can't initiate upload");
return NULL;
}
return getSDOans(idx, subidx, NID, sdo);
}
#endif
// mk... are all used
static inline uint32_t mku32(const uint8_t data[4]){
return (uint32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24));
}
static inline uint16_t mku16(const uint8_t data[4]){
return (uint16_t)(data[0] | (data[1]<<8));
}
static inline uint8_t mku8(const uint8_t data[4]){
return data[0];
}
static inline int32_t mki32(const uint8_t data[4]){
return (int32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24));
}
static inline int16_t mki16(const uint8_t data[4]){
return (int16_t)(data[0] | (data[1]<<8));
}
static inline int8_t mki8(const uint8_t data[4]){
return (int8_t)data[0];
}
// used
/**
* @brief getSDOval - get value from SDO
* @param sdo (i) - SDO
* @param e (i) - its dictionary entry
* @param ac (o) - pointer for abort code (in case of error) or NULL
* @return value, INT64_MIN in case of error or INT64_MAX in case of zero-length
*/
int64_t getSDOval(const SDO *sdo, const SDO_dic_entry *e, const abortcodes **ac){
if(sdo->ccs == CCS_ABORT_TRANSFER){ // error
WARNX("Got error for SDO 0x%X", e->index);
uint32_t c = mku32(sdo->data);
const abortcodes *abc = abortcode_search(c);
if(abc) WARNX("Abort code 0x%X: %s", ac, abc->errmsg);
if(ac) *ac = abc;
return INT64_MIN;
}
if(sdo->datalen == 0) return INT_MAX;
if(sdo->datalen != e->datasize){
WARNX("Got SDO with length %d instead of %d (as in dictionary)", sdo->datalen, e->datasize);
}
int64_t ans = 0;
if(e->issigned){
switch(sdo->datalen){
case 1:
ans = mki8(sdo->data);
break;
case 4:
ans = mki32(sdo->data);
break;
default: // can't be 3! 3->2
ans = mki16(sdo->data);
}
}else{
switch(sdo->datalen){
case 1:
ans = mku8(sdo->data);
break;
case 4:
ans = mku32(sdo->data);
break;
default: // can't be 3! 3->2
ans = mku16(sdo->data);
}
}
return ans;
}
// used
/**
* @brief SDO_read - form CANmesg to read SDO entry `e`
* @param e (i) - SDO dictionary entry to read
* @param NID - target node ID
* @param cm (o) - pointer to CANmesg which to modify
* @return `cm` or NULL if failed
*/
CANmesg *SDO_read(const SDO_dic_entry *e, uint8_t NID, CANmesg *cm){
if(!e || !cm) return NULL;
SDO sdo = {
.NID = NID,
.ccs = CCS_INIT_UPLOAD,
.index = e->index,
.subindex = e->subindex
};
return mkMesg(&sdo, cm);
}
// used
/**
* @brief SDO_write - form CANmesg to write `data` to SDO entry `e`
* @param e
* @param NID
* @param cm
* @return
*/
CANmesg *SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data, CANmesg *cm){
if(!e || !cm) return NULL;
uint32_t U;
int32_t I;
uint16_t U16;
int16_t I16;
SDO sdo ={
.NID = NID,
.ccs = CCS_INIT_DOWNLOAD,
.datalen = e->datasize,
.index = e->index,
.subindex = e->subindex
};
DBG("datalen=%d, signed=%d", e->datasize, e->issigned);
if(e->issigned){
switch(e->datasize){
case 1:
sdo.data[0] = (uint8_t) data;
break;
case 4:
I = (int32_t) data;
sdo.data[0] = I&0xff;
sdo.data[1] = (I>>8)&0xff;
sdo.data[2] = (I>>16)&0xff;
sdo.data[3] = (I>>24)&0xff;
break;
default: // can't be 3! 3->2
I16 = (int16_t) data;
sdo.data[0] = I16&0xff;
sdo.data[1] = (I16>>8)&0xff;
}
}else{
switch(e->datasize){
case 1:
sdo.data[0] = (uint8_t) data;
break;
case 4:
U = (uint32_t) data;
sdo.data[0] = U&0xff;
sdo.data[1] = (U>>8)&0xff;
sdo.data[2] = (U>>16)&0xff;
sdo.data[3] = (U>>24)&0xff;
break;
default: // can't be 3! 3->2
U16 = (uint16_t) data;
sdo.data[0] = U16&0xff;
sdo.data[1] = (U16>>8)&0xff;
}
}
mkMesg(&sdo, cm);
return cm;
}
#if 0
// write SDO data, return 0 if all OK
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
FNAME();
if(!e || !data || e->datasize < 1 || e->datasize > 4){
WARNX("SDO_write(): bad datalen");
return 1;
}
SDO sdo = {
.NID = NID,
.ccs = CCS_INIT_DOWNLOAD,
.datalen = e->datasize,
.index = e->index,
.subindex = e->subindex
};
for(uint8_t i = 0; i < e->datasize; ++i) sdo.data[i] = data[i];
CANmesg mesgp;
mkMesg(&sdo, &mesgp);
DBG("Canbus write..");
if(canbus_write(&mesgp)){
WARNX("SDO_write(): Can't initiate download");
return 2;
}
DBG("get answer");
SDO sdop;
if(!getSDOans(e->index, e->subindex, NID, &sdop)){
WARNX("SDO_write(): SDO read error");
return 3;
}
if(sdop.ccs == CCS_ABORT_TRANSFER){ // error
WARNX("SDO_write(): Got error for SDO 0x%X", e->index);
uint32_t ac = mku32(sdop.data);
const abortcodes *e = abortcode_search(ac);
if(e) WARNX("Abort code 0x%X: %s", ac, e->errmsg);
return 4;
}
if(sdop.datalen != 0){
WARNX("SDO_write(): got answer with non-zero length");
return 5;
}
if(sdop.ccs != CCS_SEG_UPLOAD){
WARNX("SDO_write(): got wrong answer");
return 6;
}
return 0;
}
#endif