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

Last change on this file since 10083 was 9132, checked in by tbretz, 16 years ago
*** empty log message ***
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 << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " ";
430 if (zombie)
431 {
432 gLog << "--> ZOMBIE! " << endl;
433 SetZombie();
434 }
435
436 gLog << MTime(-1) << endl;
437
438 return false;
439}
440
441/*
442void NodeDrv::WaitForSdos()
443{
444 while (fNetwork->WaitingForSdo(fId))
445 usleep(1);
446}
447*/
448
449// --------------------------------------------------------------------------
450//
451// Waits until the next Pdo1 from this device has been received
452//
453void NodeDrv::WaitForNextPdo1()
454{
455 fNetwork->WaitForNextPdo1(fId);
456}
457
458// --------------------------------------------------------------------------
459//
460// Waits until the next Pdo2 from this device has been received
461//
462void NodeDrv::WaitForNextPdo2()
463{
464 fNetwork->WaitForNextPdo2(fId);
465}
466
467// --------------------------------------------------------------------------
468//
469// Waits until the next Pdo3 from this device has been received
470//
471void NodeDrv::WaitForNextPdo3()
472{
473 fNetwork->WaitForNextPdo3(fId);
474}
475
476// --------------------------------------------------------------------------
477//
478// Waits until the next Pdo4 from this device has been received
479//
480void NodeDrv::WaitForNextPdo4()
481{
482 fNetwork->WaitForNextPdo4(fId);
483}
484
485// --------------------------------------------------------------------------
486//
487// Start the standard CANopen guarding of the device.
488// While ms is the guard time in millisec. This is the time between
489// two requests for a Nodeguard message.
490// ltf is the LifeTimeFactor. This means how often it is checked, that at
491// least one Nodeguard message was answered.
492//
493class NodeGuard : public MThread
494{
495 Double_t fTimeoutTime; //[s]
496 Double_t fGuardTime; //[s]
497 Int_t fLifeTimeFactor;
498
499 Bool_t fIsCanOpen;
500
501 NodeDrv *fDrv;
502
503public:
504 NodeGuard(NodeDrv *drv, Int_t guard, Int_t ltf, Bool_t canopen)
505 : MThread("NodeGuard"), fGuardTime(guard/1000.), fLifeTimeFactor(ltf), fIsCanOpen(canopen), fDrv(drv) { }
506
507 void Reset(const timeval_t *tv=NULL)
508 {
509 MTime t;
510 if (tv)
511 t.Set(*tv);
512 else
513 t.Now();
514
515 fTimeoutTime = t + (fGuardTime*fLifeTimeFactor);
516 }
517
518 Int_t Thread()
519 {
520 Reset();
521
522 while (!IsThreadCanceled())
523 {
524 // Sending nodeguards seems to result in
525 // loosing CANbus messages or CANbus answers...
526 // strange. Also protecting VmodIcan::SendCanFrame
527 // by a Mutex doesn't help.
528 if (fIsCanOpen)
529 fDrv->SendNodeguard();
530
531 MTime t;
532 t.Now();
533
534 const Double_t t0 = t+fGuardTime;
535
536 while ((double)t<t0 && (double)t<fTimeoutTime)
537 {
538 Sleep(100);
539 t.Now();
540 }
541
542 //cout << "-d-> " << (Long_t)((fTimeoutTime-t)*1000) << endl;
543 //cout << "-o-> " << (ULong_t)((fTimeoutTime)*1000)<< " " << (ULong_t)((t)*1000) << endl;
544 //cout << "-g-> " << (Long_t)((t-t0)*1000)<< endl;
545
546 if ((double)t<fTimeoutTime)
547 continue;
548
549 fDrv->SetZombie(false);
550 return 0;
551 }
552 return 0;
553 }
554};
555
556void NodeDrv::StartGuarding(Bool_t real)
557{
558 if (fGuard)
559 return;
560
561 fGuard = new NodeGuard(this, fGuardTime, fLifeTimeFactor, real);
562 fGuard->RunThread();
563
564 gLog << inf << "- " << GetNodeName() << ": Guarding (" << dec;
565 gLog << fLifeTimeFactor << "*" << fGuardTime << "ms) started." << endl;
566}
567
568void NodeDrv::StartGuarding(Int_t ms, Int_t ltf, Bool_t real)
569{
570 if (fGuard)
571 {
572 gLog << err << "- " << GetNodeName() << ": ERROR - Guarding already started." << endl;
573 return;
574 }
575
576 fGuardTime = ms;
577 fLifeTimeFactor = ltf;
578
579 StartGuarding(real);
580}
581
582void NodeDrv::StopGuarding()
583{
584 if (!fGuard)
585 return;
586
587 delete fGuard;
588 fGuard=NULL;
589
590 gLog << inf << "- " << GetNodeName() << ": Guarding stopped." << endl;
591}
592
593// --------------------------------------------------------------------------
594//
595// Set the timeout timer to the time the event was received plus the
596// guard time times lifetimefactor.
597//
598void NodeDrv::HandleNodeguard(const timeval_t &tv)
599{
600 if (fGuard)
601 fGuard->Reset(&tv);
602}
603
604void NodeDrv::SetZombie(bool stopguard)
605{
606 fIsZombie = true;
607 if (stopguard)
608 StopGuarding();
609 else
610 gLog << warn << " - " << GetNodeName() << ": Zombie set due to timeout." << endl;
611}
Note: See TracBrowser for help on using the repository browser.