iCub-main
Loading...
Searching...
No Matches
FeatureInterface.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2014 iCub Facility, Istituto Italiano di Tecnologia
3 * Authors: Alberto Cardellino
4 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
5 *
6 */
7
8
9
10#include "FeatureInterface.h"
12
13#include <ethManager.h>
14#include "embObjMotionControl.h"
15#include "embObjAnalogSensor.h"
16#include "embObjSkin.h"
17
18#include "EoProtocol.h"
19
20#include <yarp/os/Time.h>
21
22#ifdef _SETPOINT_TEST_
23#include <time.h>
24#include <sys/time.h>
25#include "EOYtheSystem.h"
26#endif
27
28
29
30//static TheEthManager *_interface2ethManager = NULL;
31
32//void initCallback(void *p)
33//{
34// if(_interface2ethManager == NULL )
35// {
36// _interface2ethManager = (TheEthManager*) p;
37// }
38//}
39
40//fakestdbool_t addEncoderTimeStamp(FEAT_ID *id, int jointNum)
41//{
42// embObjMotionControl *tmp = (embObjMotionControl *)(_interface2ethManager->getHandle(id->boardNum, id->ep));
43// if(tmp != NULL)
44// {
45// tmp->refreshEncoderTimeStamp(jointNum);
46// return fakestdbool_true;
47// }
48
49// return fakestdbool_false;
50//}
51
52//fakestdbool_t findAndFill(FEAT_ID *id, void *sk_array, eOprotID32_t id32)
53//{
54// // new with table, data stored in eoSkin;
55// // specie di view grezza, usare dynamic cast?
56// static int error = 0;
57// static int notYetInitted = 0;
58// IiCubFeature *skin;
59// EmbObjSkin *tmp = (EmbObjSkin *)(_interface2ethManager->getHandle(id->boardNum, id->ep));
60
61// if(NULL == tmp)
62// {
63// if(0 == (error%1000) )
64// yError() << "Got a SKIN message from EMS with EP " << id->ep << "board number " << id->boardNum << "but no class was instatiated for it";
65
66// error++;
67// return fakestdbool_false;
68// }
69// else
70// {
71// skin = dynamic_cast<IiCubFeature *>(tmp);
72// if(NULL != skin)
73// {
74// skin->fillData((void *)sk_array, id32);
75// }
76// else
77// {
78// if(0 == (notYetInitted%1000))
79// yWarning() << "Got a SKIN message with EP "<< id->ep << "board number " << id->boardNum << " not yet initialized";
80
81// notYetInitted++;
82// return fakestdbool_false;
83// }
84// }
85// return fakestdbool_true;
86//}
87
88//void *get_MChandler_fromEP(uint8_t boardnum, eOprotEndpoint_t ep)
89//{
90// void *h = NULL;
91// h = _interface2ethManager->getHandle(boardnum, ep);
92// return h;
93//}
94
95//fakestdbool_t handle_AS_data(FEAT_ID *id, void *as_array, eOprotID32_t id32)
96//{
97// IiCubFeature *iAnalog;
98
99// // if the following is required ... include "EoAnalogSensors.h"
100// // eOas_arrayofupto12bytes_t *debug_tmp = (eOas_arrayofupto12bytes_t *) as_array;
101
102// // specie di view grezza, usare dynamic cast?
103// embObjAnalogSensor *tmp = (embObjAnalogSensor *)(_interface2ethManager->getHandle(id->boardNum, id->ep));
104
105// if(NULL == tmp)
106// {
110// return fakestdbool_false;
111// }
112// else
113// {
114// iAnalog = dynamic_cast<IiCubFeature *>(tmp);
115// iAnalog->fillData(as_array, id32);
116// }
117
118// return fakestdbool_true;
119//}
120
121//fakestdbool_t MCmutex_post(void *p, uint32_t prognum)
122//{
123// eoThreadEntry *th = NULL;
124// embObjMotionControl *handler = (embObjMotionControl *) p;
125// int threadId;
126// eoThreadFifo *fuffy = handler->requestQueue->getFifo(prognum);
127
128// if( (threadId = fuffy->pop()) < 0)
129// {
130// yError() << "Received an answer message nobody is waiting for (MCmutex_post)";
131// return fakestdbool_false;
132// }
133// else
134// {
135// th = handler->requestQueue->threadPool->getThreadTable(threadId);
136// if(NULL == th)
137// yError() << "MCmutex_post error at line " << __LINE__;
138// th->push();
139// return fakestdbool_true;
140// }
141
142// return fakestdbool_false;
143//}
144
145
146//FEAT_boardnumber_t nvBoardNum2FeatIdBoardNum(eOprotBRD_t nvboardnum)
147//{
148// if(eo_prot_BRDdummy == nvboardnum)
149// {
150// return(FEAT_boardnumber_dummy);
151// }
152
153// return(nvboardnum+1);
154//}
155
157{
158 if(FEAT_boardnumber_dummy == fid_boardnum)
159 {
160 return(eo_prot_BRDdummy);
161 }
162
163 return(fid_boardnum-1);
164}
165
166//double feat_yarp_time_now(void)
167//{
168// return(yarp::os::Time::now());
169//}
170
171#ifdef _SETPOINT_TEST_
172//static void s_print_test_data(int jointNum, setpoint_test_data_t *test_data_ptr, uint64_t diff_ms)
173//{
174// printf("\n\n ******* IN CBK J_STATUS n=%d, diff=(ms)%llu, rec_pkt=%u , process_pkt=%u, exit_rx_phase_cond=%d\n\n",jointNum, diff_ms, test_data_ptr->numofrecpkt, test_data_ptr->numofprocesspkt, (int)test_data_ptr->exit_rx_phase_cond);
175//}
176
177void check_received_debug_data(FEAT_ID *id, int jointNum, setpoint_test_data_t *test_data_ptr)
178{
179 static int old_diff_packets[10][8] = {0}; // scheda/joint
180 struct timespec curr_time;
181 static int count = 0;
182 static bool ep_printed_6 = false;
183 static bool ep_printed_8 = false;
184 uint64_t curr_time_us;
185 uint64_t diff_ms = 0;
187#warning "check_received_debug_data"
188
189 if(!((tmp->getFeat_id().boardNum == 6) || (tmp->getFeat_id().boardNum == 8)))
190 return;
191
192
193 if(tmp == 0)
194 {
195 printf("error in heck_received_debug_data\n");
196 }
197
198// //prendo il numero di endpoint per le schede 6 e 7
199//
200// if( (!ep_printed_6) && (tmp->getFeat_id().boardNum == 6))
201// {
202// yError() << "++++++++++++++++++++++++ board num "<< tmp->getFeat_id().boardNum << "ep = " << tmp->getFeat_id().ep;
203// ep_printed_6 = true;
204// }
205//
206// if( (!ep_printed_8) && (tmp->getFeat_id().boardNum == 8))
207// {
208// yError() << "++++++++++++++++++++++++ board num "<< tmp->getFeat_id().boardNum << "ep = " << tmp->getFeat_id().ep;
209// ep_printed_8 = true;
210// }
211
212
213
214 //only for joint num 0 verify loop time
215 if((jointNum == 0) && ((tmp->getFeat_id().boardNum == 6) || (tmp->getFeat_id().boardNum == 8)))
216 {
217// if(test_data_ptr->looptime >10)
218// {
219// printf("--------- LOOP TIME----- %s, %u\n", tmp->getFeat_id().name, test_data_ptr->looptime);
220// }
221 if(test_data_ptr->diff_packets != old_diff_packets[tmp->getFeat_id().boardNum][jointNum])
222 {
223 yError() << "board" << tmp->getFeat_id().name << "joint" << jointNum << "--------- diff_packets ----- " << test_data_ptr->diff_packets;
224 old_diff_packets[tmp->getFeat_id().boardNum][jointNum] = test_data_ptr->diff_packets;
225 }
226 }
227
228 //check if received values are default
229 if((test_data_ptr->time == 0xABABABABABABABAB) &&
230 (test_data_ptr->setpoint == 0xCCCCCCCC) &&
231 (test_data_ptr->numofrecpkt == 0xFF) &&
232 (test_data_ptr->numofprocesspkt == 0xEE)
233// (test_data_ptr->exit_rx_phase_cond == 0xDD)
234 )
235 {
236// tmp->j_debug_data[jointNum].last_time = test_data_ptr->time;
237 return; //no setpoint are received!!
238 }
239
240// if(test_data_ptr->numofsepoint > 1)
241// yError() << "*** for EMS" << tmp->getFeat_id().boardNum << "joint " << jointNum << "setpoint num =" << test_data_ptr->numofsepoint;
242
243 if(jointNum == 0)
244 {
245 if((test_data_ptr->numofrecpkt != test_data_ptr->numofprocesspkt) || (test_data_ptr->numofrecpkt > 1))
246 {
247 struct timeval err_time;
248 gettimeofday(&err_time, NULL);
249 yError() << "[" << err_time.tv_sec <<"." <<err_time.tv_usec << "] QUEUEUEUEUE" << tmp->getFeat_id().boardNum << "joint " << jointNum << "recv " << test_data_ptr->numofrecpkt << "processed " << test_data_ptr->numofprocesspkt;
250 }
251 }
252
253 if(jointNum == 0)
254 {
255 if((test_data_ptr->exit_cond != 0) && (test_data_ptr->exit_cond != -5))
256 {
257 struct timeval err_time;
258 gettimeofday(&err_time, NULL);
259 yError() << "[" << err_time.tv_sec <<"." <<err_time.tv_usec << "] board " << tmp->getFeat_id().boardNum << "exit_cond" << test_data_ptr->exit_cond;
260 }
261 }
262
263 tmp->j_debug_data[jointNum].mutex.wait();
264
265 if(test_data_ptr->setpoint == tmp->j_debug_data[jointNum].pos)
266 {
267 if(tmp->j_debug_data[jointNum].gotIt)
268 {
269 tmp->j_debug_data[jointNum].mutex.post();
270 return; // siamo felici
271 }
272
273 else
274 {
275 tmp->j_debug_data[jointNum].gotIt = true;
276 curr_time_us = eoy_sys_abstime_get(eoy_sys_GetHandle());
277 diff_ms = (curr_time_us - test_data_ptr->time) /1000;
278 struct timeval err_time;
279 gettimeofday(&err_time, NULL);
280
281 yWarning() << "Got Ack for EMS" << tmp->getFeat_id().boardNum << "joint " << jointNum << "with time " << diff_ms << "and count_old " << tmp->j_debug_data[jointNum].count_old << "[" << err_time.tv_sec <<"." <<err_time.tv_usec << "]";
282// tmp->j_debug_data[jointNum].count_old = 0;
283 }
284 }
285 else if(test_data_ptr->setpoint == tmp->j_debug_data[jointNum].last_pos)
286 {
287 tmp->j_debug_data[jointNum].count_old++;
288
289 if(tmp->j_debug_data[jointNum].count_old == 10)
290 yWarning() << "*** for EMS" << tmp->getFeat_id().boardNum << "joint " << jointNum << "count_old =" << tmp->j_debug_data[jointNum].count_old;
291 }
292 else
293 {
294 if(!tmp->j_debug_data[jointNum].wtf)
295 {
296 struct timeval err_time;
297 gettimeofday(&err_time, NULL);
298
299 yError() << "[" << err_time.tv_sec <<"." <<err_time.tv_usec << "] LOST PACKET for EMS" << tmp->getFeat_id().boardNum << "joint " << jointNum << "and count_old " << tmp->j_debug_data[jointNum].count_old;
300 }
301
302 tmp->j_debug_data[jointNum].wtf = true;
303 }
304
305 tmp->j_debug_data[jointNum].mutex.post();
306
307// if(test_data_ptr->time == tmp->j_debug_data[jointNum].last_time)
308// {
309// //verifico se ho perso un setpoint in direzione pc104-->ems
314//
315// return; //la scheda non ha piu' riceviuto set point
316// }
317// else
318// {
319// tmp->j_debug_data[jointNum].last_time = test_data_ptr->time;
320// }
321//
322// curr_time_us = eoy_sys_abstime_get(eoy_sys_GetHandle());
323// diff_ms = (curr_time_us - test_data_ptr->time) /1000;
324//
325//
326// if( (diff_ms >3) || (test_data_ptr->numofrecpkt != test_data_ptr->numofprocesspkt) || (test_data_ptr->exit_rx_phase_cond != proccessed_all_rec_pkt) )
327// {
328// if( (tmp->getFeat_id().boardNum == 6) || (tmp->getFeat_id().boardNum == 8) )
329// s_print_test_data(jointNum, test_data_ptr, diff_ms);
330// }
331// else
332// {
333// printf("| ");
334// fflush(stdout);
335// }
336//
337//
338// if( (tmp->j_debug_data[jointNum].pos != test_data_ptr->setpoint) && ((tmp->getFeat_id().boardNum == 6) || (tmp->getFeat_id().boardNum == 8)) )
339// {
340// printf("******* IN CBK J_STATUS %s, n=%d received pos %d instead of %d\n", tmp->getFeat_id().name, jointNum, test_data_ptr->setpoint, tmp->j_debug_data[jointNum].pos);
341// }
342
343}
344#endif
345
346// eof
347
348
349
350
embObjMotionControl : driver for iCub motor control boards EMS on a ETH bus.
static eth::TheEthManager * _interface2ethManager
eOprotEndpoint_t ep
eOprotBRD_t featIdBoardNum2nvBoardNum(FEAT_boardnumber_t fid_boardnum)
uint8_t FEAT_boardnumber_t
#define FEAT_boardnumber_dummy