15#include "embObjAnalogSensor.h"
18#include "EoProtocol.h"
20#include <yarp/os/Time.h>
25#include "EOYtheSystem.h"
160 return(eo_prot_BRDdummy);
163 return(fid_boardnum-1);
171#ifdef _SETPOINT_TEST_
177void check_received_debug_data(
FEAT_ID *
id,
int jointNum, setpoint_test_data_t *test_data_ptr)
179 static int old_diff_packets[10][8] = {0};
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"
189 if(!((
tmp->getFeat_id().boardNum == 6) || (
tmp->getFeat_id().boardNum == 8)))
195 printf(
"error in heck_received_debug_data\n");
215 if((jointNum == 0) && ((
tmp->getFeat_id().boardNum == 6) || (
tmp->getFeat_id().boardNum == 8)))
221 if(test_data_ptr->diff_packets != old_diff_packets[
tmp->getFeat_id().boardNum][jointNum])
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;
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)
245 if((test_data_ptr->numofrecpkt != test_data_ptr->numofprocesspkt) || (test_data_ptr->numofrecpkt > 1))
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;
255 if((test_data_ptr->exit_cond != 0) && (test_data_ptr->exit_cond != -5))
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;
263 tmp->j_debug_data[jointNum].mutex.wait();
265 if(test_data_ptr->setpoint ==
tmp->j_debug_data[jointNum].pos)
267 if(
tmp->j_debug_data[jointNum].gotIt)
269 tmp->j_debug_data[jointNum].mutex.post();
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);
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 <<
"]";
285 else if(test_data_ptr->setpoint ==
tmp->j_debug_data[jointNum].last_pos)
287 tmp->j_debug_data[jointNum].count_old++;
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;
294 if(!
tmp->j_debug_data[jointNum].wtf)
296 struct timeval err_time;
297 gettimeofday(&err_time, NULL);
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;
302 tmp->j_debug_data[jointNum].wtf =
true;
305 tmp->j_debug_data[jointNum].mutex.post();
embObjMotionControl : driver for iCub motor control boards EMS on a ETH bus.
static eth::TheEthManager * _interface2ethManager