source: trunk/MagicSoft/Cosy/devdrv/dkc.cc@ 9194

Last change on this file since 9194 was 9132, checked in by tbretz, 16 years ago
*** empty log message ***
File size: 24.1 KB
Line 
1#include "dkc.h"
2
3#include <sys/time.h> // timeval->tv_sec
4
5#include <TGLabel.h>
6
7#include "network.h"
8
9#include "MLogManip.h"
10
11#include "MString.h"
12
13ClassImp(Dkc);
14
15using namespace std;
16
17#define EXPERT
18
19Dkc::Dkc(const BYTE_t nodeid, const char *name)
20 : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
21 fPdoPos1(0), fPdoPos2(0), fPosActive(0), fRpmActive(0),
22 fStatusDKC(0), fStatusPdo3(0xff), fArmed(false),
23 fReport(NULL),fLabel(NULL)
24{
25}
26
27Dkc::~Dkc()
28{
29}
30
31TString Dkc::EvalStatusDKC(UInt_t stat) const
32{
33 switch (stat)
34 {
35 case 0: return "offline";
36 case 0xa000:
37 case 0xa001:
38 case 0xa002:
39 case 0xa003: return MString::Format("Communication phase %d", stat&0xf);
40 case 0xa010: return "Drive HALT";
41 case 0xa012: return "Control and power section ready for operation";
42 case 0xa013: return "Ready for power on";
43 case 0xa100: return "Drive in Torque mode";
44 case 0xa101: return "Drive in Velocity mode";
45 case 0xa102: return "Position control mode with encoder 1";
46 case 0xa103: return "Position control mode with encoder 2";
47 case 0xa104: return "Position control mode with encoder 1, lagless";
48 case 0xa105: return "Position control mode with encoder 2, lagless";
49 case 0xa106: return "Drive controlled interpolated positioning with encoder 1";
50 case 0xa107: return "Drive controlled interpolated positioning with encoder 2";
51 case 0xa108: return "Drive controlled interpolated positioning with encoder 1, lagless";
52 case 0xa109: return "Drive controlled interpolated positioning with encoder 2, lagless";
53 //case 0xa146: return "Drive controlled interpolated relative positioning with encoder 1";
54 //case 0xa147: return "Drive controlled interpolated relative positioning with encoder 2";
55 //case 0xa148: return "Drive controlled interpolated relative positioning lagless with encoder 1";
56 //case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2";
57 case 0xa150: return "Drive controlled positioning with encoder 1";
58 case 0xa151: return "Drive controlled positioning with encoder 1, lagless";
59 case 0xa152: return "Drive controlled positioning with encoder 2";
60 case 0xa153: return "Drive controlled positioning with encoder 2, lagless";
61 case 0xa208: return "Jog mode positive";
62 case 0xa218: return "Jog mode negative";
63 case 0xa400: return "Automatic drive check and adjustment";
64 case 0xa401: return "Drive decelerating to standstill";
65 case 0xa800: return "Unknown operation mode";
66 case 0xc217: return "Motor encoder reading error";
67 case 0xc218: return "Shaft encoder reading error";
68 case 0xc220: return "Motor encoder initialization error";
69 case 0xc221: return "Shaft encoder initialization error";
70 case 0xc300: return "Command: set absolute measure";
71 case 0xc400: return "Switching to parameter mode";
72 case 0xc401: return "Drive active, switching mode not allowed";
73 case 0xc500: return "Error reset";
74 case 0xc600: return "Drive controlled homi*ng procedure ";
75 case 0xe225: return "Motor overload";
76 case 0xe249: return "Positioning command velocity exceeds limit bipolar";
77 case 0xe250: return "Drive overtemp warning";
78 case 0xe251: return "Motor overtemp warning";
79 case 0xe252: return "Bleeder overtemp warning";
80 case 0xe257: return "Continous current limit active";
81 case 0xe264: return "Target position out of numerical range";
82 case 0xe829: return "Positive position limit exceeded";
83 case 0xe830: return "Negative position limit exceeded";
84 case 0xe831: return "Position limit reached during jog";
85 case 0xe834: return "Emergency-Stop";
86 case 0xe842: return "Both end-switches activated";
87 case 0xe843: return "Positive end-switch activated";
88 case 0xe844: return "Negative end-switch activated";
89 case 0xf218: return "Amplifier overtemp shutdown";
90 case 0xf219: return "Motor overtemp shutdown";
91 case 0xf220: return "Bleeder overload shutdown";
92 case 0xf221: return "Motor temperature surveillance defective";
93 case 0xf224: return "Maximum breaking time exceeded";
94 case 0xf228: return "Excessive control deviation";
95 case 0xf250: return "Overflow of target position preset memory";
96 case 0xf269: return "Error during release of the motor holding brake";
97 case 0xf276: return "Absolute encoder out of allowed window";
98 case 0xf409: return "Bus error on Profibus interface";
99 case 0xf434: return "Emergency-Stop";
100 case 0xf629: return "Positive position limit exceeded";
101 case 0xf630: return "Negative position limit exceeded";
102 case 0xf634: return "Emergency-Stop";
103 case 0xf643: return "Positive end-switch activated";
104 case 0xf644: return "Negative end-switch activated";
105 case 0xf870: return "24V DC error";
106 case 0xf878: return "Velocity loop error";
107 }
108 return "unknown";
109}
110
111TString Dkc::GetStatus(LWORD_t val) const
112{
113 const Int_t errnum = val&0xffff;
114 const Int_t errinf = val>>16;
115
116 if (errnum!=0xff)
117 return "";
118
119 // DKC offline. This is a fatal error
120 if (errinf==0)
121 return "offline.";
122
123 TString str;
124
125 const Int_t type = errinf&0xf000;
126 switch (type)
127 {
128 case 0xf000: str += "ERROR"; break;
129 case 0xe000: str += "WARNING"; break;
130 case 0xa000: str += "Status"; break;
131 case 0xc000:
132 case 0xd000: str += "Message"; break;
133 default: str += "Unknown"; break;
134 }
135
136 str += " (";
137 str += MString::Format("%X", errinf);
138 str += "): ";
139 str += EvalStatusDKC(errinf);
140 str += (type==0xf000 || type==0xe000 ? "!" : ".");
141
142 return str;
143}
144
145Bool_t Dkc::PrintStatus(LWORD_t val) const
146{
147 const Int_t errnum = val&0xffff;
148 const Int_t errinf = val>>16;
149
150 if (errnum!=0xff)
151 return errnum==0;
152
153 gLog << all << MTime(-1) << ": " << GetNodeName() << " " << GetStatus(val) << endl;
154
155 // errinf==0: DKC offline. This is a fatal error
156 return errinf==0 ? kFALSE : (errinf&0xf000)!=0xf000;
157}
158
159void Dkc::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv)
160{
161 // cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx;
162 // cout << ", val=0x" << val << endl;
163 switch (idx)
164 {
165 case 0x1000:
166 if (subidx==1)
167 {
168 gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl;
169 fArmed = val==1;
170 }
171 return;
172
173 case 0x1003:
174 // FIXME, see Init
175 if (subidx!=2)
176 return;
177 gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << hex << val << dec << endl;
178 CheckErrorDKC(val);
179 return;
180
181 case 0x100a:
182 gLog << inf2 << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
183 fSoftVersion = val;
184 return;
185
186 case 0x100b:
187 // Do not display, this is used for CheckConnection
188 // lout << "Node ID: " << dec << val << endl;
189 return;
190
191 case 0x100c:
192 gLog << inf2 << "- " << GetNodeName() << ": Guard time:" << dec << val << endl;
193 return;
194
195 case 0x100d:
196 gLog << inf2 << "- " << GetNodeName() << ": Life time factor:" << dec << val << endl;
197 return;
198
199 case 0x2002:
200 gLog << inf2 << GetNodeName() << ": Current velocity: " << dec << val << endl;
201 fVel = val;
202 return;
203
204 case 0x6004:
205 if (subidx==0)
206 {
207 fPdoPos1 = (LWORDS_t)val;
208 fPdoTime1.Set(tv);
209 fHasChangedPos1 = true;
210 }
211 if (subidx==1)
212 {
213 fPdoPos2 = (LWORDS_t)val;
214 fPdoTime2.Set(tv);
215 fHasChangedPos2 = true;
216 }
217 return;
218
219 case 0x6002:
220 gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " (1rpm)" << endl;
221 fVelRes = val;
222 return;
223
224 case 0x6003:
225 gLog << inf2 << "- " << GetNodeName() << ": Maximum velocity = " << dec << val << " (100%)" << endl;
226 fVelMax = val;
227 return;
228
229 case 0x6501:
230 gLog << inf2 << "- " << GetNodeName() << ": Position resolution = " << dec << val << " ticks/revolution" << endl;
231 fPosRes = val;
232 return;
233 }
234
235 NodeDrv::HandleSDO(idx, subidx, val, tv);
236
237 // cout << "Dkc: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
238 // cout << ", val=0x"<<val<<endl;
239}
240
241void Dkc::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
242{
243 // cout << "Node #" << dec << (int)GetId() << ": Sdo=" << hex << idx << "/" << (int)subidx << " set." << endl;
244
245 // If a real drive operation is requested from the MACS and
246 // the MACS is not correctly initialized the operation is
247 // rejected. (This is expecially harmfull if the NoWait state
248 // is set incorrectly)
249 if (data)
250 SetZombie();
251
252 switch (idx)
253 {
254 case 0x1000:
255 switch (subidx)
256 {
257 case 1:
258 //lout << ddev(MLog::eGui);
259 gLog << inf2 << "- " << GetNodeName() << ": State of node set." << endl;
260 //lout << edev(MLog::eGui);
261 return;
262 }
263 break;
264
265 case 0x100c:
266 switch (subidx)
267 {
268 case 0:
269 //lout << ddev(MLog::eGui);
270 gLog << inf2 << "- " << GetNodeName() << ": Guard time set." << endl;
271 //lout << edev(MLog::eGui);
272 return;
273 }
274 break;
275
276 case 0x100d:
277 switch (subidx)
278 {
279 case 0:
280 //lout << ddev(MLog::eGui);
281 gLog << inf2 << "- " << GetNodeName() << ": Life time factor set." << endl;
282 //lout << edev(MLog::eGui);
283 return;
284 }
285 break;
286
287 case 0x1800:
288 switch (subidx)
289 {
290 case 1:
291 //lout << ddev(MLog::eGui);
292 gLog << inf2 << "- " << GetNodeName() << ": Status of PDO1 set." << endl;
293 //lout << edev(MLog::eGui);
294 return;
295 }
296 break;
297
298 case 0x2002:
299 switch (subidx)
300 {
301 case 0:
302 //lout << ddev(MLog::eGui);
303 gLog << inf2 << "- " << GetNodeName() << ": Velocity set." << endl;
304 //lout << edev(MLog::eGui);
305 return;
306 }
307 break;
308
309 case 0x2003:
310 switch (subidx)
311 {
312 case 0:
313 //lout << ddev(MLog::eGui);
314 gLog << inf2 << "- " << GetNodeName() << ": Acceleration set." << endl;
315 //lout << edev(MLog::eGui);
316 return;
317 case 1:
318 //lout << ddev(MLog::eGui);
319 gLog << inf2 << "- " << GetNodeName() << ": Deceleration set." << endl;
320 //lout << edev(MLog::eGui);
321 return;
322 }
323 break;
324
325 case 0x3006:
326 switch (subidx)
327 {
328 case 0:
329 //lout << ddev(MLog::eGui);
330 gLog << inf2 << "- " << GetNodeName() << ": RPM mode switched." << endl;
331 //lout << edev(MLog::eGui);
332 return;
333
334 case 1:
335 /*
336 lout << ddev(MLog::eGui);
337 lout << "- Velocity set (" << GetNodeName() << ")" << endl;
338 lout << edev(MLog::eGui);
339 */
340 return;
341 }
342 break;
343
344 case 0x4000:
345 HandleNodeguard(tv);
346 return;
347
348 case 0x6000:
349 //lout << ddev(MLog::eGui);
350 gLog << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl;
351 //lout << edev(MLog::eGui);
352 return;
353
354 case 0x6002:
355 //lout << ddev(MLog::eGui);
356 gLog << inf2 << "- " << GetNodeName() << ": Velocitz resolution set." << endl;
357 //lout << edev(MLog::eGui);
358 return;
359
360 case 0x6003:
361 switch (subidx)
362 {
363 case 0:
364 //lout << ddev(MLog::eGui);
365 gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
366 //lout << edev(MLog::eGui);
367 return;
368
369 case 1:
370 //lout << ddev(MLog::eGui);
371 gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
372 //lout << edev(MLog::eGui);
373 return;
374 }
375 break;
376
377 case 0x6004:
378 switch (subidx)
379 {
380 case 0:
381 //lout << ddev(MLog::eGui);
382 gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
383 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
384 //lout << edev(MLog::eGui);
385 return;
386
387 case 1:
388 //lout << ddev(MLog::eGui);
389 gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
390 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
391 //lout << edev(MLog::eGui);
392 return;
393 }
394 break;
395
396
397 }
398 NodeDrv::HandleSDOOK(idx, subidx, data, tv);
399}
400
401void Dkc::ReqVelRes()
402{
403 gLog << inf2 << "- " << GetNodeName() << ": Requesting velocity resolution (velres, 0x6002)." << endl;
404 RequestSDO(0x6002);
405 WaitForSdo(0x6002);
406}
407
408void Dkc::ReqVelMax()
409{
410 gLog << inf2 << "- " << GetNodeName() << ": Requesting maximum velocity (velmax, 0x6003)." << endl;
411 RequestSDO(0x6003);
412 WaitForSdo(0x6003);
413}
414
415void Dkc::ReqPosRes()
416{
417 gLog << inf2 << "- " << GetNodeName() << ": Requesting position resolution (posres, 0x6501)." << endl;
418 RequestSDO(0x6501);
419 WaitForSdo(0x6501);
420}
421
422void Dkc::SetPDO1On(BYTE_t flag)
423{
424 gLog << inf2 << "- " << GetNodeName() << ": " << (flag?"Enable":"Disable") << " PDO1." << endl;
425 SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
426 WaitForSdo(0x1800, 1);
427}
428
429void Dkc::StartNode()
430{
431 //
432 // Switch node from pre-operational state to operational state
433 // (This is not CANOpen compatible)
434 // After this the MACS will react on real movement commands.
435 //
436 gLog << inf2 << "- " << GetNodeName() << ": Starting Node." << endl;
437 SendSDO(0x1000, 1, (LWORD_t)1);
438 WaitForSdo(0x1000, 1);
439}
440
441void Dkc::Arm()
442{
443 StartNode();
444}
445
446void Dkc::Disarm()
447{
448 gLog << inf2 << "- " << GetNodeName() << ": Stopping Node." << endl;
449 SendSDO(0x1000, 1, (LWORD_t)0);
450 WaitForSdo(0x1000, 1);
451}
452
453void Dkc::CheckConnection()
454{
455 RequestSDO(0x100b);
456 WaitForSdo(0x100b);
457}
458
459void Dkc::Init()
460{
461 //
462 // Request current error status (FIXME: is the first entry in the
463 // error list)
464 //
465 gLog << inf2 << "- " << GetNodeName() << ": Requesting Error[0]." << endl;
466 RequestSDO(0x1003, 2);
467 WaitForSdo(0x1003, 2);
468
469 SetPDO1On(TRUE);
470 /*
471 if (HasError())
472 {
473 gLog << err << "Dkc::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
474 SetZombie();
475 }
476 if (IsZombieNode())
477 return;
478 */
479
480 SetRpmMode(FALSE);
481
482 ReqPosRes(); // Init fVelRes
483 ReqVelRes(); // Init fVelResVelRes
484 ReqVelMax(); // Init fVelMax
485
486#ifdef EXPERT
487 StartNode();
488#endif
489
490 gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl;
491 RequestSDO(0x1000, 1);
492 WaitForSdo(0x1000, 1);
493}
494
495void Dkc::StopDevice()
496{
497 //
498 // FIXME: This isn't called if the initialization isn't done completely!
499 //
500
501 SetRpmMode(FALSE);
502}
503
504void Dkc::ReqPos1()
505{
506 gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 1." << endl;
507 RequestSDO(0x6004, 0);
508 WaitForSdo(0x6004, 0);
509}
510
511void Dkc::ReqPos2()
512{
513 gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 2." << endl;
514 RequestSDO(0x6004, 1);
515 WaitForSdo(0x6004, 1);
516}
517
518void Dkc::ReqVel()
519{
520 gLog << inf2 << "- " << GetNodeName() << ": Requesting Velocity." << endl;
521 RequestSDO(0x2002);
522 WaitForSdo(0x2002);
523}
524
525void Dkc::SetVelocity(LWORD_t vel)
526{
527 gLog << dbg << "- Setting velocity to: " << vel << endl;
528 SendSDO(0x2002, vel); // velocity
529 WaitForSdo(0x2002, 0);
530}
531
532void Dkc::SetVelocityRel(Double_t vel)
533{
534 gLog << dbg << "- Setting velocity to: " << vel*100 << "%" << endl;
535 SendSDO(0x2002, (LWORD_t)(vel*fVelMax+0.5)); // velocity
536 WaitForSdo(0x2002, 0);
537}
538
539void Dkc::SetAcceleration(LWORD_t acc)
540{
541 gLog << dbg << "- Setting acceleration to: " << acc << endl;
542 SendSDO(0x2003, 0, acc); // acceleration
543 WaitForSdo(0x2003, 0);
544}
545
546void Dkc::SetDeceleration(LWORD_t dec)
547{
548 gLog << dbg << "- Setting deceleration to: " << dec << endl;
549 SendSDO(0x2003, 1, dec);
550 WaitForSdo(0x2003, 1);
551}
552
553void Dkc::SetRpmMode(BYTE_t mode)
554{
555 //
556 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
557 //
558 SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
559 WaitForSdo(0x3006, 0);
560}
561
562void Dkc::SetRpmVelocity(LWORDS_t cvel)
563{
564 SendSDO(0x3006, 1, (LWORD_t)cvel);
565 WaitForSdo(0x3006, 1);
566}
567
568void Dkc::StartRelPos(LWORDS_t pos)
569{
570 if (!fArmed)
571 {
572 gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
573 SetZombie();
574 return;
575 }
576
577 gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl;
578 SendSDO(0x6004, 1, (LWORD_t)pos);
579 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
580}
581
582void Dkc::StartAbsPos(LWORDS_t pos)
583{
584 if (!fArmed)
585 {
586 gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
587 SetZombie();
588 return;
589 }
590
591 gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (LWORDS_t)pos << " ticks." << endl;
592 SendSDO(0x6004, 0, (LWORD_t)pos);
593 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
594}
595
596void Dkc::StartAbsPosRev(Double_t pos)
597{
598 if (!fArmed)
599 {
600 gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
601 SetZombie();
602 return;
603 }
604
605 const LWORD_t p = (LWORD_t)(pos*fPosRes+.5);
606
607 gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << p << " ticks." << endl;
608 SendSDO(0x6004, 0, p);
609 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
610}
611
612void Dkc::StartVelSync()
613{
614 //
615 // The syncronization mode is disabled by a 'MOTOR STOP'
616 // or by a positioning command (POSA, ...)
617 //
618 gLog << inf2 << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl;
619 SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
620 WaitForSdo(0x3007, 0);
621}
622
623void Dkc::StartPosSync()
624{
625 //
626 // The syncronization mode is disabled by a 'MOTOR STOP'
627 // or by a positioning command (POSA, ...)
628 //
629 gLog << inf2 << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl;
630 SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
631 WaitForSdo(0x3007, 1);
632}
633
634void Dkc::SendMsg(BYTE_t data[6])
635{
636 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
637}
638
639void Dkc::SendMsg(BYTE_t d0, BYTE_t d1, BYTE_t d2,
640 BYTE_t d3, BYTE_t d4, BYTE_t d5)
641{
642 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
643}
644
645void Dkc::HandlePDO1(const BYTE_t *data, const timeval_t &tv)
646{
647 // FIXME!!!! Only 0x4000 should do this to be
648 // CanOpen conform
649 //HandleNodeguard(tv);
650
651 fPdoPos1 = (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0];
652 fPdoTime1.Set(tv);
653 fHasChangedPos1 = true;
654
655 fPdoPos2 = (data[7]<<24) | (data[6]<<16) | (data[5]<<8) | data[4];
656 fPdoTime2.Set(tv);
657 fHasChangedPos2 = true;
658
659 if (fReport)
660 {
661 fReport->Lock("ShaftEncoder::HandlePDOType0");
662 *fReport << "FEEDBACK " << (int)GetId() << " " << fPdoTime1 << " PDO0 " << GetNodeName() << " " << fPdoPos1 << " " << fPdoPos2 << endl;
663 fReport->UnLock("ShaftEncoder::HandlePDOType0");
664 }
665}
666
667void Dkc::HandlePDO3(const BYTE_t *data, const timeval_t &tv)
668{
669 fArmed = data[2]&0x01;
670
671 if (fStatus!=data[3])
672 {
673 // data[3]&0x01; // motor not moving
674 fPosActive = data[3]&0x02; // positioning active
675 fRpmActive = data[3]&0x04; // RPM mode switched on
676 // data[3]&0x08; // - unused -
677 // data[3]&0x10; // - unused -
678 // data[3]&0x20; // - unused -
679 //fInControl = data[3]&0x40; // motor uncontrolled
680 // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
681
682 fStatus = data[3];
683 }
684
685 if (fStatusPdo3!=data[0])
686 {
687 gLog << inf << MTime(-1) << ": " << GetNodeName() << " - PDO1 = ";
688 const Bool_t ready = data[0]&0x01;
689 const Bool_t fuse = data[0]&0x02;
690 const Bool_t emcy = data[0]&0x04;
691 const Bool_t vltg = data[0]&0x08;
692 const Bool_t mode = data[0]&0x10;
693 const Bool_t rf = data[0]&0x20;
694 const Bool_t brake = data[0]&0x40;
695 const Bool_t power = data[0]&0x80;
696 if (ready) gLog << "DKC-Ready ";
697 if (fuse) gLog << "FuseOk ";
698 if (emcy) gLog << "EmcyOk ";
699 if (vltg) gLog << "OvervoltOk ";
700 if (mode) gLog << "SwitchToManualMode ";
701 if (rf) gLog << "RF ";
702 if (brake) gLog << "BrakeOpen ";
703 if (power) gLog << "PowerOn ";
704 gLog << endl;
705
706 fStatusPdo3 = data[0];
707 }
708}
709
710void Dkc::CheckErrorDKC(LWORD_t val)
711{
712 fStatusDKC = val;
713
714 const Bool_t rc = PrintStatus(val);
715 SetError(rc ? 0 : val);
716 if (!rc)
717 SetZombie();
718}
719
720void Dkc::HandlePDO2(const BYTE_t *data, const timeval_t &tv)
721{
722 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
723 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
724
725 // Check if the DKC changed its status message
726 if (errnum==0xff && (errinf&0xf000)<=0xe000)
727 {
728 CheckErrorDKC(errnum, errinf);
729 return;
730 }
731
732 // Check if MACS report error occursion.
733 // errnum==0 gives a sudden information that something happened. Now the
734 // microcontroller is running inside its interrupt procedure which
735 // stopped the normal program. The interrupt procedure should try to clear
736 // the error state of the hardware. This should never create a new error!
737 //
738 if (!errnum)
739 {
740 gLog << err << "- " << GetNodeName() << ": reports Error occursion." << endl;
741 gLog << "Dkc::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
742 SetZombie();
743 SetError(-1);
744 return;
745 }
746
747 //
748 // Now the error is handled by the hardware now it is the software part
749 // to react on it. The Error flag now is set to the correct value.
750 //
751 if (GetError()>0)
752 {
753 gLog << warn << GetNodeName() << ": WARNING! Previous error #" << GetError() << " unhandled (not cleared) by software." << endl;
754
755 //
756 // If the error is unhadled and/or not cleared, don't try it again.
757 //
758 if (GetError()==errnum)
759 return;
760 }
761
762 SetError(errnum);
763
764 gLog << err << GetNodeName() << " reports: ";
765 switch (errnum)
766 {
767 case 0xff:
768 EvalStatus(errnum, errinf);
769 return;
770
771 default:
772 gLog << "Error Nr. " << dec << errnum << ", " << errinf << endl;
773 }
774}
775
776// FIXME? Handling of fIsZombie?
777void Dkc::HandleError()
778{
779 //
780 // If there is no error we must not handle anything
781 //
782 if (!HasError())
783 return;
784
785 //
786 // If the program got into the: HandleError state before the hardware
787 // has finished handeling the error we have to wait for the hardware
788 // handeling the error
789 //
790 // FIXME: Timeout???
791 //
792// while (GetError()<0)
793// usleep(1);
794
795 //
796 // After this software and hardware should be in a state so that
797 // we can go on working 'as usual' Eg. Initialize a Display Update
798 //
799 gLog << inf << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
800 switch (GetError())
801 {
802 case 0xff:
803 gLog << err << "DKC error! Go and check what is going on!" << endl;
804 return;
805
806 default:
807 gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
808
809 }
810}
811
812void Dkc::DisplayVal()
813{
814 const LWORDS_t pos = GetPdoPos2();
815 if (IsZombieNode())
816 {
817 if (fLabel)
818 fLabel->SetText(new TGString(""));
819 fUpdPos = ~pos;
820 return;
821 }
822
823 char text[21]="";
824 if (pos!=fUpdPos && fLabel)
825 {
826 sprintf(text, "%ld", pos);
827 fLabel->SetText(new TGString(text));
828 fUpdPos = pos;
829 }
830}
Note: See TracBrowser for help on using the repository browser.