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

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