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

Last change on this file since 5277 was 2518, checked in by tbretz, 21 years ago
*** empty log message ***
File size: 11.8 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-2003
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 <iostream> // cout
35#include <iomanip> // setw, setfill
36
37ClassImp(CanOpen);
38
39using namespace std;
40
41// --------------------------------------------------------------------------
42//
43// Initializes a conditional and a mutex semaphore for all possible
44// PDO combinations
45//
46CanOpen::CanOpen(const char *dev, const int baud, MLog &out) : VmodIcan(dev, baud, out)
47{
48 lout << "- CanOpen initialized." << endl;
49}
50
51// --------------------------------------------------------------------------
52//
53// Destroys all conditional and mutex semaphores
54//
55CanOpen::~CanOpen()
56{
57 lout << "- CanOpen destroyed." << endl;
58}
59
60// --------------------------------------------------------------------------
61//
62// This overloads VmodIcan::HandleCanMessage. It is called if a can
63// message was received with all message relevant data (COBId, data, time)
64// It distributes the data depending on its function code to several
65// functions (to be obverloaded)
66// In case of a PDO the conditional semaphore corresponding to this PDO
67// is raised (WaitForNextPDO)
68// HandleSDO: handles a SDO message
69// HandlePDO1/2/3/4:handles received PDOs
70//
71void CanOpen::HandleCanMessage(WORD_t cobid, BYTE_t *data, timeval_t *tv)
72{
73 const WORD_t fcode = cobid >> 7;
74 const BYTE_t node = cobid & 0x1f;
75
76 switch (fcode)
77 {
78 case kNMT:
79 cout << "NMT: " << hex ;
80 cout << "CobId: 0x" << cobid << " ";
81 cout << "cmd=0x" << (int)data[0] << " ";
82 cout << "node=" << dec << (int)data[1] << endl;
83 return;
84
85 case kEMERGENCY: // also case kSYNC:
86 if (cobid==0)
87 cout << "Sync" << endl;
88 else
89 {
90 cout << "EMERGENCY Node #" << dec << (int)data[1] << endl;
91 HandleEmergency(node, tv);
92 }
93 return;
94
95 case kNodeguard:
96 //cout << "Nodeguard Node #" << dec << (int)node << endl;
97 HandleNodeguard(node, tv);
98 return;
99
100 case kSDO_RX:
101 {
102 const BYTE_t cmd = data[0];
103 const LWORD_t dat = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
104 const WORD_t idx = data[1] | (data[2]<<8);
105 const WORD_t subidx = data[3];
106
107 HandleSDO(node, cmd, idx, subidx, dat, tv);
108
109 fSdoList.Del(node, idx, subidx);
110 }
111 return;
112
113 case kPDO1_TX:
114 {
115 HandlePDO1(node, data, tv);
116 fPdoCond[node-1][0].Broadcast();
117 }
118 return;
119
120 case kPDO2_TX:
121 {
122 HandlePDO2(node, data, tv);
123 fPdoCond[node-1][1].Broadcast();
124 }
125 return;
126
127 case kPDO3_TX:
128 {
129 HandlePDO3(node, data, tv);
130 fPdoCond[node-1][2].Broadcast();
131 }
132 return;
133
134 case kPDO4_TX:
135 {
136 HandlePDO4(node, data, tv);
137 fPdoCond[node-1][3].Broadcast();
138 }
139 return;
140 }
141
142 cout << "CanOpen::HandleCanMessage - Unhandled Message: Function Code=0x" << hex << fcode << " Node #" << dec << (int)node << endl;
143}
144
145// --------------------------------------------------------------------------
146//
147// Enables can messaged for a given node ID and function code.
148//
149void CanOpen::EnableCanMsg(BYTE_t node, BYTE_t fcode, int flag)
150{
151 if (node>=0x20)
152 return;
153
154 EnableCobId(CobId(node, fcode), flag);
155}
156
157// --------------------------------------------------------------------------
158//
159// Enables Emergency messages for a given node
160//
161void CanOpen::EnableEmcy(BYTE_t node)
162{
163 EnableCanMsg(node, kEMERGENCY);
164}
165
166// --------------------------------------------------------------------------
167//
168// Enables Nodeguard messages for a given node
169//
170void CanOpen::EnableNodeguard(BYTE_t node)
171{
172 EnableCanMsg(node, kNodeguard);
173}
174
175
176// --------------------------------------------------------------------------
177//
178// Enables SDO rx messages for a given node
179//
180void CanOpen::EnableSdoRx(BYTE_t node)
181{
182 EnableCanMsg(node, kSDO_RX);
183}
184
185// --------------------------------------------------------------------------
186//
187// Enables PDO1 tx messages for a given node
188//
189void CanOpen::EnablePdo1Rx(BYTE_t node)
190{
191 EnableCanMsg(node, kPDO1_TX);
192}
193
194// --------------------------------------------------------------------------
195//
196// Enables PDO2 tx messages for a given node
197//
198void CanOpen::EnablePdo2Rx(BYTE_t node)
199{
200 EnableCanMsg(node, kPDO2_TX);
201}
202
203// --------------------------------------------------------------------------
204//
205// Enables PDO3 rx messages for a given node
206//
207void CanOpen::EnablePdo3Rx(BYTE_t node)
208{
209 EnableCanMsg(node, kPDO1_TX);
210}
211
212// --------------------------------------------------------------------------
213//
214// Enables PDO4 rx messages for a given node
215//
216void CanOpen::EnablePdo4Rx(BYTE_t node)
217{
218 EnableCanMsg(node, kPDO2_TX);
219}
220
221// --------------------------------------------------------------------------
222//
223// Sends a PDO1 message with the given data to the given node
224//
225void CanOpen::SendPDO1(BYTE_t node, BYTE_t data[8])
226{
227 SendCanFrame(CobId(node, kPDO1_TX), data);
228}
229
230// --------------------------------------------------------------------------
231//
232// Sends a PDO2 message with the given data to the given node
233//
234void CanOpen::SendPDO2(BYTE_t node, BYTE_t data[8])
235{
236 SendCanFrame(CobId(node, kPDO2_TX), data);
237}
238
239// --------------------------------------------------------------------------
240//
241// Sends a PDO3 message with the given data to the given node
242//
243void CanOpen::SendPDO3(BYTE_t node, BYTE_t data[8])
244{
245 SendCanFrame(CobId(node, kPDO3_TX), data);
246}
247
248// --------------------------------------------------------------------------
249//
250// Sends a PDO4 message with the given data to the given node
251//
252void CanOpen::SendPDO4(BYTE_t node, BYTE_t data[8])
253{
254 SendCanFrame(CobId(node, kPDO4_TX), data);
255}
256
257// --------------------------------------------------------------------------
258//
259// Sends a PDO1 message with the given data to the given node
260//
261void CanOpen::SendPDO1(BYTE_t node,
262 BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
263 BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
264{
265 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
266 SendCanFrame(CobId(node, kPDO2_TX), msg);
267}
268
269// --------------------------------------------------------------------------
270//
271// Sends a PDO2 message with the given data to the given node
272//
273void CanOpen::SendPDO2(BYTE_t node,
274 BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
275 BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
276{
277 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
278 SendCanFrame(CobId(node, kPDO2_TX), msg);
279}
280
281// --------------------------------------------------------------------------
282//
283// Sends a PDO3 message with the given data to the given node
284//
285void CanOpen::SendPDO3(BYTE_t node,
286 BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
287 BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
288{
289 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
290 SendCanFrame(CobId(node, kPDO3_TX), msg);
291}
292
293// --------------------------------------------------------------------------
294//
295// Sends a PDO4 message with the given data to the given node
296//
297void CanOpen::SendPDO4(BYTE_t node,
298 BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
299 BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
300{
301 BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
302 SendCanFrame(CobId(node, kPDO4_TX), msg);
303}
304
305// --------------------------------------------------------------------------
306//
307// Sends a SDO message with the given data to the given node:
308// - index describing the dictionary index to set
309// - subindex describing the dictionary subindex of theindex to set
310// - val describing the value to set.
311// - store describes whether the sdo should be stored in a list to
312// be able to wait for an answer
313//
314void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, BYTE_t val, bool store)
315{
316 if (store)
317 fSdoList.Add(node, idx, subidx);
318
319 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX1,
320 word_to_lsb(idx), word_to_msb(idx), subidx, val);
321}
322
323// --------------------------------------------------------------------------
324//
325// Sends a SDO message with the given data to the given node:
326// - index describing the dictionary index to set
327// - subindex describing the dictionary subindex of theindex to set
328// - val describing the value to set.
329// - store describes whether the sdo should be stored in a list to
330// be able to wait for an answer
331//
332void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, WORD_t val, bool store)
333{
334 if (store)
335 fSdoList.Add(node, idx, subidx);
336
337 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX2,
338 word_to_lsb(idx), word_to_msb(idx), subidx,
339 word_to_lsb(val), word_to_msb(val));
340}
341
342// --------------------------------------------------------------------------
343//
344// Sends a SDO message with the given data to the given node:
345// - index describing the dictionary index to set
346// - subindex describing the dictionary subindex of theindex to set
347// - val describing the value to set.
348// - store describes whether the sdo should be stored in a list to
349// be able to wait for an answer
350//
351void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, LWORD_t val, bool store)
352{
353 if (store)
354 fSdoList.Add(node, idx, subidx);
355
356 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX4,
357 word_to_lsb(idx), word_to_msb(idx), subidx,
358 word_to_lsb(val&0xffff), word_to_msb(val&0xffff),
359 word_to_lsb(val>>16), word_to_msb(val>>16));
360}
361
362// --------------------------------------------------------------------------
363//
364// Request a SDO message from the given node:
365// - index describing the dictionary index to request
366// - subindex describing the dictionary subindex of the index to request
367//
368void CanOpen::RequestSDO(BYTE_t node, WORD_t idx, BYTE_t subidx)
369{
370 fSdoList.Add(node, idx, subidx);
371
372 SendCanFrame(CobId(node, kSDO_TX), kSDO_RX_DATA, word_to_lsb(idx), word_to_msb(idx), subidx);
373}
374
375// --------------------------------------------------------------------------
376//
377// Send a NMT Message to the given node with command cmd
378//
379void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd)
380{
381 SendCanFrame(CobId(0, kNMT), cmd, node);
382}
383
384// --------------------------------------------------------------------------
385//
386// Send a Nodeguard Message to the given node with command cmd
387//
388void CanOpen::SendNodeguard(BYTE_t node)
389{
390 BYTE_t msg[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
391 SendCanFrame(CobId(node, kNodeguard), msg, 1);
392}
393
394// --------------------------------------------------------------------------
395//
396// Decodes node and function code into a CobId
397//
398WORD_t CanOpen::CobId(BYTE_t node, BYTE_t fcode) const
399{
400 return (fcode<<7) | node&0x1f;
401}
Note: See TracBrowser for help on using the repository browser.