source: trunk/Cosy/candrv/nodedrv.cc@ 14751

Last change on this file since 14751 was 14319, checked in by tbretz, 12 years ago
Improved logging output.
File size: 17.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@uni-sw.gwdg.de>, 2001
19!
20! Copyright: MAGIC Software Development, 2000-2001
21!
22!
23\* ======================================================================== */
24
25///////////////////////////////////////////////////////////////////////
26//
27// NodeDrv
28//
29// Base class for a class describing the interface for the CAN nodes.
30//
31// to be overloaded:
32// virtual void Init()
33// virtual void StopDevice()
34// virtual void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
35// virtual void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
36// virtual void HandleSDOError(LWORD_t data)
37// virtual void HandlePDO1(BYTE_t *data, timeval_t *tv)
38// virtual void HandlePDO2(BYTE_t *data, timeval_t *tv)
39// virtual void HandlePDO3(BYTE_t *data, timeval_t *tv)
40// virtual void HandlePDO4(BYTE_t *data, timeval_t *tv)
41// virtual bool Reboot();
42// virtual void CheckConnection();
43//
44///////////////////////////////////////////////////////////////////////
45#include "nodedrv.h"
46
47#include <iomanip>
48#include <iostream>
49
50//#include <TTimer.h>
51
52#include "MTime.h"
53#include "network.h"
54
55#include "MLogManip.h"
56
57#include "MThread.h"
58
59ClassImp(NodeDrv);
60
61using namespace std;
62
63// --------------------------------------------------------------------------
64//
65// Constructor for one node. Sets the Node Id (<32) the logging stream
66// and the node name. The name is a name for debug output.
67//
68NodeDrv::NodeDrv(BYTE_t nodeid, const char *name) : fNetwork(NULL), fId(32), fError(0), fIsZombie(kTRUE), fGuard(NULL)
69{
70 if (nodeid>0x1f)
71 {
72 gLog << err << "ERROR - NodeDrv::NodeDrv: Only node Numbers < 32 are allowed"<< endl;
73 return;
74 }
75
76 fId = nodeid;
77
78 if (name)
79 fName = name;
80 else
81 {
82 fName = "Node#";
83 fName += (int)nodeid;
84 }
85
86 gLog << inf2 << "- Node #" << (int)nodeid << " (" << name << ") initialized." << endl;
87
88}
89
90// --------------------------------------------------------------------------
91//
92// destructor
93//
94NodeDrv::~NodeDrv()
95{
96}
97
98// --------------------------------------------------------------------------
99//
100// This should be called from a master or main thread to get a node out
101// of the Zombie-Status. Overload it by your needs.
102//
103bool NodeDrv::Reboot()
104{
105 fIsZombie = false;
106
107 Init();
108
109 return !fIsZombie;
110}
111
112// --------------------------------------------------------------------------
113//
114// Init device, sets the pointer to the whole network and enables
115// the Can messages to be passed through the interface:
116// PDO1 tx
117// PDO2 tx
118// PDO3 tx
119// PDO4 tx
120// SDO rx
121// SDO tx
122//
123bool NodeDrv::InitDevice(Network *net)
124{
125 fNetwork = net;
126
127 EnableCanMsg(kPDO1_TX);
128 EnableCanMsg(kPDO2_TX);
129 EnableCanMsg(kPDO3_TX);
130 EnableCanMsg(kPDO4_TX);
131 EnableCanMsg(kSDO_RX);
132 EnableCanMsg(kSDO_TX);
133 EnableCanMsg(kNodeguard);
134 EnableCanMsg(kEMERGENCY);
135
136 fIsZombie = kFALSE;
137
138 Init();
139
140 return !fIsZombie;
141}
142
143// --------------------------------------------------------------------------
144//
145// Print an "SDO idx/subidx set." from this device message.
146// This output is never redirected to the GUI.
147// In standard CANOpen operation data is meaningless (we are using
148// it in the 'non-standard' CANOpen communication with the MACS)
149//
150void NodeDrv::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
151{
152 const Bool_t gui = gLog.IsOutputDeviceEnabled(MLog::eGui);
153
154 if (gui)
155 gLog << ddev(MLog::eGui);
156
157 gLog << warn << setfill('0') << "WARNING - Nodedrv::HandleSDOOK: ";
158 gLog << "Node #" << dec << (int)fId << ": Sdo=" << hex << idx << "/" << (int)subidx << " set.";
159 gLog << endl;
160
161 if (gui)
162 gLog << edev(MLog::eGui);
163}
164
165// --------------------------------------------------------------------------
166//
167// Print an error message with the corresponding data from this device.
168//
169void NodeDrv::HandleSDOError(WORD_t idx, BYTE_t subidx)
170{
171 gLog << warn << "WARNING - Nodedrv::HandleSDOError: Node #" << dec << (int)fId << ": Entry not found in dictionary (idx=0x";
172 gLog << hex << setfill('0') << setw(4) << idx << "/" << (int)subidx << dec << ")";
173 gLog << endl;
174}
175
176// --------------------------------------------------------------------------
177//
178// Prints the received SDo from this device
179//
180void NodeDrv::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv)
181{
182 gLog << warn << "WARNING - Nodedrv::HandleSDO: Idx=0x"<< hex << idx << "/" << (int)subidx;
183 gLog << ", val=0x" << val << endl;
184}
185
186// --------------------------------------------------------------------------
187//
188// Sends the given PDO1 through the network to this device
189// A PDO is carrying up to eight bytes of information.
190//
191// The message is not send if the node has the status Zombie.
192// In this case false is returned, otherwise true
193//
194bool NodeDrv::SendPDO1(BYTE_t data[8])
195{
196 if (!fIsZombie)
197 fNetwork->SendPDO1(fId, data);
198 return !fIsZombie;
199}
200
201// --------------------------------------------------------------------------
202//
203// Sends the given PDO2 through the network to this device
204// A PDO is carrying up to eight bytes of information.
205//
206// The message is not send if the node has the status Zombie.
207// In this case false is returned, otherwise true
208//
209bool NodeDrv::SendPDO2(BYTE_t data[8])
210{
211 if (!fIsZombie)
212 fNetwork->SendPDO2(fId, data);
213 return !fIsZombie;
214}
215
216// --------------------------------------------------------------------------
217//
218// Sends the given PDO1 through the network to this device
219// A PDO is carrying up to eight bytes of information.
220//
221// The message is not send if the node has the status Zombie.
222// In this case false is returned, otherwise true
223//
224bool NodeDrv::SendPDO1(BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
225 BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
226{
227 if (!fIsZombie)
228 fNetwork->SendPDO1(fId, m0, m1, m2, m3, m4, m5, m6, m7);
229 return !fIsZombie;
230}
231
232// --------------------------------------------------------------------------
233//
234// Sends the given PDO2 through the network to this device
235// A PDO is carrying up to eight bytes of information.
236//
237// The message is not send if the node has the status Zombie.
238// In this case false is returned, otherwise true
239//
240bool NodeDrv::SendPDO2(BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
241 BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
242{
243 if (!fIsZombie)
244 fNetwork->SendPDO2(fId, m0, m1, m2, m3, m4, m5, m6, m7);
245 return !fIsZombie;
246}
247
248// --------------------------------------------------------------------------
249//
250// Sends the given SDO through the network to this device
251// An SDO message contains
252// an address (this device)
253// an index of the dictionary entry to address
254// a subindex of this dictionary entry to access
255// and a value to set for this dictionary entry
256//
257// The message is not send if the node has the status Zombie.
258// In this case false is returned, otherwise true
259//
260bool NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val, bool store)
261{
262 if (!fIsZombie)
263 fNetwork->SendSDO(fId, idx, subidx, val, store);
264 return !fIsZombie;
265}
266
267// --------------------------------------------------------------------------
268//
269// Sends the given SDO through the network to this device
270// An SDO message contains
271// an address (this device)
272// an index of the dictionary entry to address
273// a subindex of this dictionary entry to access
274// and a value to set for this dictionary entry
275//
276// The message is not send if the node has the status Zombie.
277// In this case false is returned, otherwise true
278//
279bool NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val, bool store)
280{
281 if (!fIsZombie)
282 fNetwork->SendSDO(fId, idx, subidx, val, store);
283 return !fIsZombie;
284}
285
286// --------------------------------------------------------------------------
287//
288// Sends the given SDO through the network to this device
289// An SDO message contains
290// an address (this device)
291// an index of the dictionary entry to address
292// a subindex of this dictionary entry to access
293// and a value to set for this dictionary entry
294//
295// The message is not send if the node has the status Zombie.
296// In this case false is returned, otherwise true
297//
298bool NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, bool store)
299{
300 if (!fIsZombie)
301 fNetwork->SendSDO(fId, idx, subidx, val, store);
302 return !fIsZombie;
303}
304
305// --------------------------------------------------------------------------
306//
307// Sends the given SDO through the network to this device
308// An SDO message contains
309// an address (this device)
310// an index of the dictionary entry to address
311// a subindex of this dictionary entry to access
312// and a value to set for this dictionary entry
313//
314// The message is not send if the node has the status Zombie.
315// In this case false is returned, otherwise true
316//
317bool NodeDrv::SendSDO(WORD_t idx, BYTE_t val)
318{
319 if (!fIsZombie)
320 fNetwork->SendSDO(fId, idx, val, true);
321 return !fIsZombie;
322}
323
324// --------------------------------------------------------------------------
325//
326// Sends the given SDO through the network to this device
327// An SDO message contains
328// an address (this device)
329// an index of the dictionary entry to address
330// a subindex of this dictionary entry to access
331// and a value to set for this dictionary entry
332//
333// The message is not send if the node has the status Zombie.
334// In this case false is returned, otherwise true
335//
336bool NodeDrv::SendSDO(WORD_t idx, WORD_t val)
337{
338 if (!fIsZombie)
339 fNetwork->SendSDO(fId, idx, val, true);
340 return !fIsZombie;
341}
342
343// --------------------------------------------------------------------------
344//
345// Sends the given SDO through the network to this device
346// An SDO message contains
347// an address (this device)
348// an index of the dictionary entry to address
349// a subindex of this dictionary entry to access
350// and a value to set for this dictionary entry
351//
352// The message is not send if the node has the status Zombie.
353// In this case false is returned, otherwise true
354//
355bool NodeDrv::SendSDO(WORD_t idx, LWORD_t val)
356{
357 if (!fIsZombie)
358 fNetwork->SendSDO(fId, idx, val, true);
359 return !fIsZombie;
360}
361
362// --------------------------------------------------------------------------
363//
364// Request a SDO for a given idx/subidx
365// An SDO message contains
366// an address (this device)
367// an index of the dictionary entry to read
368// a subindex of this dictionary entry to access
369//
370// The message is not send if the node has the status Zombie.
371// In this case false is returned, otherwise true
372//
373bool NodeDrv::RequestSDO(WORD_t idx, BYTE_t subidx)
374{
375 if (!fIsZombie)
376 fNetwork->RequestSDO(fId, idx, subidx);
377 return !fIsZombie;
378}
379
380// --------------------------------------------------------------------------
381//
382// Send a NMT message (command) to this device
383//
384// The message is not send if the node has the status Zombie.
385// In this case false is returned, otherwise true
386//
387bool NodeDrv::SendNMT(BYTE_t cmd)
388{
389 if (!fIsZombie)
390 fNetwork->SendNMT(fId, cmd);
391 return !fIsZombie;
392}
393
394// --------------------------------------------------------------------------
395//
396// Send a Nodeguard message (command) to this device
397//
398void NodeDrv::SendNodeguard()
399{
400 fNetwork->SendNodeguard(fId);
401}
402
403// --------------------------------------------------------------------------
404//
405// Enable passthrough for the given functioncode of this device
406//
407void NodeDrv::EnableCanMsg(BYTE_t fcode)
408{
409 fNetwork->EnableCanMsg(fId, fcode, TRUE);
410}
411
412// --------------------------------------------------------------------------
413//
414// Wait a given timeout until the SDO with the given idx/subidx from
415// this device has been received.
416// You can stop waiting by StopWaitingForSDO.
417// Return false if waiting timed out.
418// If waiting timed out the node is set to status Zombie.
419//
420// If the node is already a zombie node, the message is deleted from the
421// queue and no waiting is done, false is returned..
422//
423bool NodeDrv::WaitForSdo(WORD_t idx, BYTE_t subidx, WORDS_t timeout, bool zombie)
424{
425 bool rc = fNetwork->WaitForSdo(fId, idx, subidx, fIsZombie?-1:timeout);
426 if (rc)
427 return true;
428
429 gLog << inf2;
430 if (zombie)
431 {
432 SetZombie();
433 gLog << warn;
434 }
435
436 gLog << warn << MTime(-1);
437 gLog << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " (dt=" << timeout <<")" << endl;
438
439 return false;
440}
441
442/*
443void NodeDrv::WaitForSdos()
444{
445 while (fNetwork->WaitingForSdo(fId))
446 usleep(1);
447}
448*/
449
450// --------------------------------------------------------------------------
451//
452// Waits until the next Pdo1 from this device has been received
453//
454void NodeDrv::WaitForNextPdo1()
455{
456 fNetwork->WaitForNextPdo1(fId);
457}
458
459// --------------------------------------------------------------------------
460//
461// Waits until the next Pdo2 from this device has been received
462//
463void NodeDrv::WaitForNextPdo2()
464{
465 fNetwork->WaitForNextPdo2(fId);
466}
467
468// --------------------------------------------------------------------------
469//
470// Waits until the next Pdo3 from this device has been received
471//
472void NodeDrv::WaitForNextPdo3()
473{
474 fNetwork->WaitForNextPdo3(fId);
475}
476
477// --------------------------------------------------------------------------
478//
479// Waits until the next Pdo4 from this device has been received
480//
481void NodeDrv::WaitForNextPdo4()
482{
483 fNetwork->WaitForNextPdo4(fId);
484}
485
486// --------------------------------------------------------------------------
487//
488// Start the standard CANopen guarding of the device.
489// While ms is the guard time in millisec. This is the time between
490// two requests for a Nodeguard message.
491// ltf is the LifeTimeFactor. This means how often it is checked, that at
492// least one Nodeguard message was answered.
493//
494class NodeGuard : public MThread
495{
496 Double_t fTimeoutTime; //[s]
497 Double_t fGuardTime; //[s]
498 Int_t fLifeTimeFactor;
499
500 Bool_t fIsCanOpen;
501
502 NodeDrv *fDrv;
503
504public:
505 NodeGuard(NodeDrv *drv, Int_t guard, Int_t ltf, Bool_t canopen)
506 : MThread("NodeGuard"), fGuardTime(guard/1000.), fLifeTimeFactor(ltf), fIsCanOpen(canopen), fDrv(drv) { }
507
508 void Reset(const timeval_t *tv=NULL)
509 {
510 MTime t;
511 if (tv)
512 t.Set(*tv);
513 else
514 t.Now();
515
516 fTimeoutTime = t + (fGuardTime*fLifeTimeFactor);
517 }
518
519 Int_t Thread()
520 {
521 Reset();
522
523 while (!IsThreadCanceled())
524 {
525 // Sending nodeguards seems to result in
526 // loosing CANbus messages or CANbus answers...
527 // strange. Also protecting VmodIcan::SendCanFrame
528 // by a Mutex doesn't help.
529 if (fIsCanOpen)
530 fDrv->SendNodeguard();
531
532 MTime t;
533 t.Now();
534
535 const Double_t t0 = t+fGuardTime;
536
537 while ((double)t<t0 && (double)t<fTimeoutTime)
538 {
539 Sleep(100);
540 t.Now();
541 }
542
543 //cout << "-d-> " << (Long_t)((fTimeoutTime-t)*1000) << endl;
544 //cout << "-o-> " << (ULong_t)((fTimeoutTime)*1000)<< " " << (ULong_t)((t)*1000) << endl;
545 //cout << "-g-> " << (Long_t)((t-t0)*1000)<< endl;
546
547 if ((double)t<fTimeoutTime)
548 continue;
549
550 fDrv->SetZombie(false);
551 return 0;
552 }
553 return 0;
554 }
555};
556
557void NodeDrv::StartGuarding(Bool_t real)
558{
559 if (fGuard)
560 return;
561
562 fGuard = new NodeGuard(this, fGuardTime, fLifeTimeFactor, real);
563 fGuard->RunThread();
564
565 gLog << inf << "- " << GetNodeName() << ": Guarding (" << dec;
566 gLog << fLifeTimeFactor << "*" << fGuardTime << "ms) started." << endl;
567}
568
569void NodeDrv::StartGuarding(Int_t ms, Int_t ltf, Bool_t real)
570{
571 if (fGuard)
572 {
573 gLog << err << "- " << GetNodeName() << ": ERROR - Guarding already started." << endl;
574 return;
575 }
576
577 fGuardTime = ms;
578 fLifeTimeFactor = ltf;
579
580 StartGuarding(real);
581}
582
583void NodeDrv::StopGuarding()
584{
585 if (!fGuard)
586 return;
587
588 delete fGuard;
589 fGuard=NULL;
590
591 gLog << inf << "- " << GetNodeName() << ": Guarding stopped." << endl;
592}
593
594// --------------------------------------------------------------------------
595//
596// Set the timeout timer to the time the event was received plus the
597// guard time times lifetimefactor.
598//
599void NodeDrv::HandleNodeguard(const timeval_t &tv)
600{
601 if (fGuard)
602 fGuard->Reset(&tv);
603}
604
605void NodeDrv::SetZombie(bool stopguard)
606{
607 fIsZombie = true;
608 if (stopguard)
609 StopGuarding();
610 else
611 gLog << warn << " - " << GetNodeName() << ": Zombie set due to timeout." << endl;
612}
Note: See TracBrowser for help on using the repository browser.