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

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