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

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