source: trunk/MagicSoft/Cosy/candrv/canopen.cc@ 8816

Last change on this file since 8816 was 8813, checked in by tbretz, 18 years ago
*** empty log message ***
File size: 17.7 KB
Line 
1/* ======================================================================== *\
2!
3! *
4! * This file is part of Stesy, the MAGIC Steering System
5! * Software. It is distributed to you in the hope that it can be a useful
6! * and timesaving tool in analysing Data of imaging Cerenkov telescopes.
7! * It is distributed WITHOUT ANY WARRANTY.
8! *
9! * Permission to use, copy, modify and distribute this software and its
10! * documentation for any purpose is hereby granted without fee,
11! * provided that the above copyright notice appear in all copies and
12! * that both that copyright notice and this permission notice appear
13! * in supporting documentation. It is provided "as is" without express
14! * or implied warranty.
15! *
16!
17!
18! Author(s): Thomas Bretz <mailto:tbretz@astro.uni-wuerzburg.de>, 2003
19!
20! Copyright: MAGIC Software Development, 2000-2008
21!
22!
23\* ======================================================================== */
24
25///////////////////////////////////////////////////////////////////////
26//
27// CanOpen
28//
29// implements the canopen layer over the raw device driver
30//
31///////////////////////////////////////////////////////////////////////
32#include "canopen.h"
33
34#include "MLog.h"
35#include "MLogManip.h"
36
37#include "interface.h"
38
39ClassImp(CanOpen);
40
41using namespace std;
42
43// --------------------------------------------------------------------------
44//
45// Initializes a conditional and a mutex semaphore for all possible
46// PDO combinations
47//
48CanOpen::CanOpen() : fInterface(0)
49{
50 gLog << inf << "- CanOpen initialized." << endl;
51}
52
53// --------------------------------------------------------------------------
54//
55// Destroys all conditional and mutex semaphores
56//
57CanOpen::~CanOpen()
58{
59 gLog << inf << "- CanOpen destroyed." << endl;
60}
61
62// --------------------------------------------------------------------------
63//
64// Start the interface
65//
66void CanOpen::Start()
67{
68 if (fInterface)
69 fInterface->Start();
70}
71
72// --------------------------------------------------------------------------
73//
74// Stop the interface
75//
76void CanOpen::Stop()
77{
78 if (fInterface)
79 fInterface->Stop();
80}
81
82// --------------------------------------------------------------------------
83//
84// This overloads Interface::HandleCanMessage. It is called if a can
85// message was received with all message relevant data (COBId, data, time)
86// It distributes the data depending on its function code to several
87// functions (to be obverloaded)
88// In case of a PDO the conditional semaphore corresponding to this PDO
89// is raised (WaitForNextPDO)
90// HandleSDO: handles a SDO message
91// HandlePDO1/2/3/4:handles received PDOs
92//
93void CanOpen::HandleCanMessage(WORD_t cobid, const BYTE_t *data, const timeval_t &tv)
94{
95 const WORD_t fcode = cobid >> 7;
96 const BYTE_t node = cobid & 0x1f;
97
98 switch (fcode)
99 {
100 case kNMT:
101 cout << "NMT: " << hex ;
102 cout << "CobId: 0x" << cobid << " ";
103 cout << "cmd=0x" << (int)data[0] << " ";
104 cout << "node=" << dec << (int)data[1] << endl;
105 return;
106
107 case kEMERGENCY: // also case kSYNC:
108 if (cobid==0)
109 cout << "Sync" << endl;
110 else
111 {
112 cout << "EMERGENCY Node #" << dec << (int)data[1] << endl;
113 HandleEmergency(node, tv);
114 }
115 return;
116
117 case kNodeguard:
118 //cout << "Nodeguard Node #" << dec << (int)node << endl;
119 HandleNodeguard(node, tv);
120 return;
121
122 case kSDO_RX:
123 {
124 const BYTE_t cmd = data[0];
125 const LWORD_t dat = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
126 const WORD_t idx = data[1] | (data[2]<<8);
127 const WORD_t subidx = data[3];
128
129 cout << "SND NODE: " << (int)node << " " << flush;
130 HandleSDO(node, cmd, idx, subidx, dat, tv);
131
132 fSdoList.Del(node, idx, subidx);
133 }
134 return;
135
136 case kPDO1_TX:
137 {
138 HandlePDO1(node, data, tv);
139 fPdoCond[node-1][0].Broadcast();
140 }
141 return;
142
143 case kPDO2_TX:
144 {
145 HandlePDO2(node, data, tv);
146 fPdoCond[node-1][1].Broadcast();
147 }
148 return;
149
150 case kPDO3_TX:
151 {
152 HandlePDO3(node, data, tv);
153 fPdoCond[node-1][2].Broadcast();
154 }
155 return;
156
157 case kPDO4_TX:
158 {
159 HandlePDO4(node, data, tv);
160 fPdoCond[node-1][3].Broadcast();
161 }
162 return;
163 }
164
165 cout << "CanOpen::HandleCanMessage - Unhandled Message: Function Code=0x" << hex << fcode << " Node #" << dec << (int)node << endl;
166}
167
168// --------------------------------------------------------------------------
169//
170// Does a basic message processing and hadles the so called command (cmd)
171// stamp of the message. This is some kind of message type which is send
172// by the can interface
173//
174void CanOpen::HandleMessage(const Message &msg, const timeval_t &tv)
175{
176 //
177 // Decode message
178 //
179 const WORD_t desc = msg.data[2]<<8 | msg.data[3];
180 const BYTE_t rtr = (desc>>4)&1;
181 const BYTE_t len = desc&0xf;
182 const WORD_t cobid = desc>>5;
183
184 switch (msg.cmd) // FROM mican.h
185 {
186 case M_MSG_LOST:
187 cout << "Interface reports: " << dec << (int)msg.data[0] << " msg(s) lost!" << endl;
188 return;
189
190 case M_BCAN_TX_con: /* confirm (+/-) transmission */
191 cout << "Interface reports: CTXcon=0x35" << endl;
192 cout << "This normally means, that the transmission of the following CAN frame failed:" << hex << endl;
193 cout << "Descr: 0x" << cobid << dec;
194 cout << " Rtr: " << (rtr?"Yes":"No");
195 cout << " Len: " << (int)len << endl;
196 return;
197
198 case M_BCAN_EVENT_ind:
199 cout << "Interface reports: CEVTind=0x37: " << hex;
200 switch (msg.data[0]) // error indicator
201 {
202 case 0x01:
203 cout << "Error interrup occured" << endl;
204 cout << "This means noisy network normally. Please check the bus termination." << endl;
205 switch (msg.data[1]) // msg type (board depending)
206 {
207 case 2: // SJA1000
208 cout << dec;
209 cout << "ModeReg=" << (int)msg.data[2] << ", ";
210 cout << "StatReg=" << (int)msg.data[3] << ", ";
211 cout << "RxErrCnt=" << (int)msg.data[4] << ", ";
212 cout << "TxErrCnt=" << (int)msg.data[5] << endl;
213 }
214 //FIXME? TerminateApp();
215 return;
216 case 0x02:
217 cout << "Overrun interrup occured" << endl;
218 return;
219 case 0x04:
220 cout << "Interrupts lost" << endl;
221 return;
222 case 0x08:
223 cout << "Send queue full" << endl;
224 return;
225 case 0x10:
226 cout << "CANbus bus-error" << endl;
227 return;
228 }
229 return;
230 case M_BCAN_RX_ind:
231 //
232 // Message is a message from the Can bus
233 //
234 //cout << "HandleCanMessage " << cobid << endl;
235 HandleCanMessage(cobid, &msg.data[4], tv);
236 return;
237 }
238
239 //
240 // Nothing of the above happened
241 //
242 cout << hex;
243 cout << "Cmd=0x" << (int)msg.cmd << ": ";
244 cout << "Descr: 0x" << cobid << dec;
245 cout << " Rtr: " << (rtr?"Yes":"No");
246 cout << " Len: " << (int)len << endl;
247
248 cout << "As Raw Data:" << hex << setfill('0');
249 for (int i=0; i<msg.len; i++)
250 cout << " " << setw(2) << (int)(msg.data[i]) << flush;
251 cout << endl;
252}
253
254bool CanOpen::WaitForSdos(WORDS_t ms)
255{
256 // 0: unlimited
257 // <0: don't do a real wait.
258 if (ms<0)
259 {
260 fSdoList.DelAll();
261 return true;
262 }
263
264 MTimeout t(ms);
265 while (fSdoList.IsPending() &&
266 !StopWaitingForSDO() &&
267 !t.HasTimedOut())
268 usleep(1);
269
270 bool rc = true;
271 if (ms && t.HasTimedOut())
272 {
273 cout << "WaitForSdos timed out." << endl;
274 rc = false;
275 }
276 /*
277 if (StopWaitingForSDO())
278 {
279 cout << "WaitForSdos stopped." << endl;
280 rc = false;
281 }
282 */
283 if ((ms && t.HasTimedOut()) || StopWaitingForSDO())
284 fSdoList.DelAll();
285
286 return rc;
287}
288
289bool CanOpen::WaitForSdo(BYTE_t node, WORD_t idx, BYTE_t subidx, WORDS_t ms)
290{
291 // 0: unlimited
292 // <0: don't do a real wait.
293
294 if (ms<0)
295 {
296 fSdoList.Del(node, idx, subidx);
297 return true;
298 }
299
300 MTimeout t(ms);
301 while (fSdoList.IsPending(node, idx, subidx) &&
302 !StopWaitingForSDO() &&
303 !t.HasTimedOut())
304 usleep(1);
305
306 bool rc = true;
307 if (ms && t.HasTimedOut())
308 {
309 cout << "WaitForSdo Node #" << (int)node << " " << idx << "/" << (int)subidx << " timed out." << endl;
310 rc = false;
311 }
312 /*
313 if (StopWaitingForSDO())
314 {
315 cout << "WaitForSdo Node #" << (int)node << " " << idx << "/" << (int)subidx << " stopped." << endl;
316 rc = false;
317 }
318 */
319 if ((ms && t.HasTimedOut()) || StopWaitingForSDO())
320 fSdoList.Del(node, idx, subidx);
321
322 return rc;
323}
324
325// --------------------------------------------------------------------------
326//
327// Enables can messaged for a given node ID and function code.
328//
329void CanOpen::EnableCanMsg(BYTE_t node, BYTE_t fcode, int flag)
330{
331 if (node>=0x20)
332 return;
333
334 if (fInterface)
335 fInterface->EnableCobId(CobId(node, fcode), flag);
336}
337
338// --------------------------------------------------------------------------
339//
340// Enables Emergency messages for a given node
341//
342void CanOpen::EnableEmcy(BYTE_t node)
343{
344 EnableCanMsg(node, kEMERGENCY);
345}
346
347// --------------------------------------------------------------------------
348//
349// Enables Nodeguard messages for a given node
350//
351void CanOpen::EnableNodeguard(BYTE_t node)
352{
353 EnableCanMsg(node, kNodeguard);
354}
355
356
357// --------------------------------------------------------------------------
358//
359// Enables SDO rx messages for a given node
360//
361void CanOpen::EnableSdoRx(BYTE_t node)
362{
363 EnableCanMsg(node, kSDO_RX);
364}
365
366// --------------------------------------------------------------------------
367//
368// Enables PDO1 tx messages for a given node
369//
370void CanOpen::EnablePdo1Rx(BYTE_t node)
371{
372 EnableCanMsg(node, kPDO1_TX);
373}
374
375// --------------------------------------------------------------------------
376//
377// Enables PDO2 tx messages for a given node
378//
379void CanOpen::EnablePdo2Rx(BYTE_t node)
380{
381 EnableCanMsg(node, kPDO2_TX);
382}
383
384// --------------------------------------------------------------------------
385//
386// Enables PDO3 rx messages for a given node
387//
388void CanOpen::EnablePdo3Rx(BYTE_t node)
389{
390 EnableCanMsg(node, kPDO1_TX);
391}
392
393// --------------------------------------------------------------------------
394//
395// Enables PDO4 rx messages for a given node
396//
397void CanOpen::EnablePdo4Rx(BYTE_t node)
398{
399 EnableCanMsg(node, kPDO2_TX);
400}
401
402// --------------------------------------------------------------------------
403//
404// Sends a PDO1 message with the given data to the given node
405//
406void CanOpen::SendPDO1(BYTE_t node, BYTE_t data[8])
407{
408 SendCanFrame(CobId(node, kPDO1_TX), data);
409}
410
411// --------------------------------------------------------------------------
412//
413// Sends a PDO2 message with the given data to the given node
414//
415void CanOpen::SendPDO2(BYTE_t node, BYTE_t data[8])
416{
417 SendCanFrame(CobId(node, kPDO2_TX), data);
418}
419
420// --------------------------------------------------------------------------
421//
422// Sends a PDO3 message with the given data to the given node
423//
424void CanOpen::SendPDO3(BYTE_t node, BYTE_t data[8])
425{
426 SendCanFrame(CobId(node, kPDO3_TX), data);
427}
428
429// --------------------------------------------------------------------------
430//
431// Sends a PDO4 message with the given data to the given node
432//
433void CanOpen::SendPDO4(BYTE_t node, BYTE_t data[8])
434{
435 SendCanFrame(CobId(node, kPDO4_TX), data);
436}
437
438// --------------------------------------------------------------------------
439//
440// Sends a PDO1 message with the given data to the given node
441//
442void CanOpen::SendPDO1(BYTE_t node,
443 BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
444 BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
445{
446 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
447 SendCanFrame(CobId(node, kPDO2_TX), msg);
448}
449
450// --------------------------------------------------------------------------
451//
452// Sends a PDO2 message with the given data to the given node
453//
454void CanOpen::SendPDO2(BYTE_t node,
455 BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
456 BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
457{
458 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
459 SendCanFrame(CobId(node, kPDO2_TX), msg);
460}
461
462// --------------------------------------------------------------------------
463//
464// Sends a PDO3 message with the given data to the given node
465//
466void CanOpen::SendPDO3(BYTE_t node,
467 BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
468 BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
469{
470 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
471 SendCanFrame(CobId(node, kPDO3_TX), msg);
472}
473
474// --------------------------------------------------------------------------
475//
476// Sends a PDO4 message with the given data to the given node
477//
478void CanOpen::SendPDO4(BYTE_t node,
479 BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
480 BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
481{
482 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
483 SendCanFrame(CobId(node, kPDO4_TX), msg);
484}
485
486// --------------------------------------------------------------------------
487//
488// Sends a SDO message with the given data to the given node:
489// - index describing the dictionary index to set
490// - subindex describing the dictionary subindex of theindex to set
491// - val describing the value to set.
492// - store describes whether the sdo should be stored in a list to
493// be able to wait for an answer
494//
495void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, BYTE_t val, bool store)
496{
497 if (store)
498 fSdoList.Add(node, idx, subidx);
499
500 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX1,
501 word_to_lsb(idx), word_to_msb(idx), subidx, val);
502}
503
504// --------------------------------------------------------------------------
505//
506// Sends a SDO message with the given data to the given node:
507// - index describing the dictionary index to set
508// - subindex describing the dictionary subindex of theindex to set
509// - val describing the value to set.
510// - store describes whether the sdo should be stored in a list to
511// be able to wait for an answer
512//
513void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, WORD_t val, bool store)
514{
515 if (store)
516 fSdoList.Add(node, idx, subidx);
517
518 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX2,
519 word_to_lsb(idx), word_to_msb(idx), subidx,
520 word_to_lsb(val), word_to_msb(val));
521}
522
523// --------------------------------------------------------------------------
524//
525// Sends a SDO message with the given data to the given node:
526// - index describing the dictionary index to set
527// - subindex describing the dictionary subindex of theindex to set
528// - val describing the value to set.
529// - store describes whether the sdo should be stored in a list to
530// be able to wait for an answer
531//
532void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, LWORD_t val, bool store)
533{
534 if (store)
535 fSdoList.Add(node, idx, subidx);
536
537 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX4,
538 word_to_lsb(idx), word_to_msb(idx), subidx,
539 word_to_lsb(val&0xffff), word_to_msb(val&0xffff),
540 word_to_lsb(val>>16), word_to_msb(val>>16));
541}
542
543// --------------------------------------------------------------------------
544//
545// Request a SDO message from the given node:
546// - index describing the dictionary index to request
547// - subindex describing the dictionary subindex of the index to request
548//
549void CanOpen::RequestSDO(BYTE_t node, WORD_t idx, BYTE_t subidx)
550{
551 fSdoList.Add(node, idx, subidx);
552
553 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX_DATA, word_to_lsb(idx), word_to_msb(idx), subidx);
554}
555
556// --------------------------------------------------------------------------
557//
558// Send a NMT Message to the given node with command cmd
559//
560void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd)
561{
562 SendCanFrame(CobId(0, kNMT), cmd, node);
563}
564
565// --------------------------------------------------------------------------
566//
567// Send a Nodeguard Message to the given node with command cmd
568//
569void CanOpen::SendNodeguard(BYTE_t node)
570{
571 BYTE_t msg[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
572 SendCanFrame(CobId(node, kNodeguard), msg, 1);
573}
574
575// --------------------------------------------------------------------------
576//
577// This is IcSendReqBCAN from the Janz software
578//
579// /*
580// * IcSendReqBCAN - Send a CANbus message
581// *
582// * Issue request to send a CAN message. <Spec> controls whether to send with
583// * or without spec/confirmation.
584// * .CS
585// * spec action
586// * 0 send only
587// * 1 send with confirmation to the host.
588// * 2 send and echo message to the host.
589// * 3 send and generate both echo and confirmation.
590// * .CE
591// *
592// * SERVICE: CTXreq, CTXCreq, CTXEreq, CTXCEreq
593// *
594// * NOTE:
595// * Raw ICANOS version of the firmware only.
596// */
597//
598void CanOpen::SendCanFrame(WORD_t cobid, BYTE_t m[8], BYTE_t rtr)
599{
600 if (fInterface)
601 fInterface->SendCanFrame(cobid, m, rtr);
602}
603
604// --------------------------------------------------------------------------
605//
606// Sends a can frame with the given cobid and the given eight bytes
607// through the can network
608//
609void CanOpen::SendCanFrame(WORD_t cobid,
610 BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
611 BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
612{
613 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
614 SendCanFrame(cobid, msg);
615}
616
617// --------------------------------------------------------------------------
618//
619// Decodes node and function code into a CobId
620//
621WORD_t CanOpen::CobId(BYTE_t node, BYTE_t fcode) const
622{
623 return (fcode<<7) | node&0x1f;
624}
Note: See TracBrowser for help on using the repository browser.