source: trunk/Cosy/candrv/canopen.cc@ 14402

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