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

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