iCub-main
Loading...
Searching...
No Matches
fakeCan.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008 RobotCub Consortium
3 * Author: Lorenzo Natale
4 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5 *
6 */
7
8#include "fakeCan.h"
9#include <iostream>
10
11#include <yarp/os/Bottle.h>
12#include <yarp/os/Value.h>
13
14using namespace std;
15using namespace yarp::dev;
16using namespace yarp::os;
17
20
23
24/*ICan*/
25bool FakeCan::canSetBaudRate(unsigned int rate)
26{
27 return true;
28}
29
30bool FakeCan::canGetBaudRate(unsigned int *rate)
31{
32 return true;
33}
34
35bool FakeCan::canIdAdd(unsigned int id)
36{
37 return true;
38}
39
40bool FakeCan::canIdDelete(unsigned int id)
41{
42 return true;
43}
44
45bool FakeCan::canRead(CanBuffer &msgs,
46 unsigned int size,
47 unsigned int *read,
48 bool wait)
49{
50 replies.lock();
51 unsigned int l=replies.size();
52
53 if (size<l)
54 l=size;
55
56 *read=l;
57
58 MsgIt it=replies.begin();
59 unsigned int k=0;
60 while(it!=replies.end() && k<l)
61 {
62 FCMSG *r=reinterpret_cast<FCMSG *>(msgs[k].getPointer());
63 *r=*it;
64 it++;
65 k++;
66 }
67
68 replies.clear();
69 replies.unlock();
70 return true;
71}
72
73bool FakeCan::canWrite(const CanBuffer &msgs,
74 unsigned int size,
75 unsigned int *sent,
76 bool wait)
77{
78 BoardsIt it=boardList.begin();
79
80 while(it!=boardList.end())
81 {
82 FakeBoard *tmp=(*it);
83 for(unsigned int k=0;k<size;k++)
84 {
85 CanMessage &m=const_cast<CanBuffer &>(msgs)[k];
86 FCMSG *canM=reinterpret_cast<FCMSG *>(m.getPointer());
87 tmp->pushMessage(*canM);
88 }
89 it++;
90 }
91
92 *sent=size;
93
94 return true;
95}
96
97/*Device Driver*/
98bool FakeCan::open(yarp::os::Searchable &par)
99{
100 cerr<<"Opening FakeCan network" << endl;
101
102 //fprintf(stderr, "%s", par.toString().c_str());
103
104 int njoints=par.findGroup("GENERAL").find("Joints").asInt32();
105 Bottle &can = par.findGroup("CAN");
106 Bottle ids=can.findGroup("CanAddresses");
107
108 if (ids.size()<njoints/2)
109 {
110 fprintf(stderr, "Check ini file, wrong number of board ids or joints\n");
111 return false;
112 }
113
114 for(int i=1;i<=njoints/2;i++)
115 {
116 FakeBoard *tmp=new FakeBoard;
117 int id=ids.get(i).asInt32();
118 tmp->setId(id); //just as a test
119 tmp->setReplyFifo(&replies);
120 tmp->start();
121 boardList.push_back(tmp);
122 }
123
124 return true;
125}
126
128{
129 cerr<<"Closing FakeCan network" << endl;
130
131 BoardsIt it=boardList.begin();
132
133 while(it!=boardList.end())
134 {
135 FakeBoard *tmp=(*it);
136 tmp->stop();
137 delete tmp;
138 it++;
139 }
140
141 boardList.clear();
142
143 return true;
144}
void lock()
Definition msgList.h:24
void unlock()
Definition msgList.h:25
virtual bool open(yarp::os::Searchable &par)
Definition fakeCan.cpp:98
virtual bool close()
Definition fakeCan.cpp:127
virtual bool canSetBaudRate(unsigned int rate)
Definition fakeCan.cpp:25
virtual bool canWrite(const CanBuffer &msgs, unsigned int size, unsigned int *sent, bool wait=false)
Definition fakeCan.cpp:73
virtual bool canRead(CanBuffer &msgs, unsigned int size, unsigned int *read, bool wait=false)
Definition fakeCan.cpp:45
virtual bool canIdAdd(unsigned int id)
Definition fakeCan.cpp:35
virtual bool canGetBaudRate(unsigned int *rate)
Definition fakeCan.cpp:30
virtual bool canIdDelete(unsigned int id)
Definition fakeCan.cpp:40
std::list< FakeBoard * >::iterator BoardsIt
Definition fakeCan.h:86
std::list< FCMSG >::iterator MsgIt
Definition msgList.h:17
fprintf(fid,'\n')