iCub-main
downloader.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2008 RobotCub Consortium
5  * Author: Marco Maggiali, Marco Randazzo, Alessandro Scalzo, Marco Accame
6  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
7  *
8  */
9 
10 #include "driver.h"
11 #include "downloader.h"
12 #include <yarp/os/Time.h>
13 #include <yarp/os/Log.h>
14 #include <stdlib.h> //added for abs
15 #include <string.h>
16 
17 #include <iCubCanProtocol.h>
18 #include "strain.h"
19 
20 using namespace yarp::dev;
21 using namespace yarp::os;
22 using namespace std;
23 
24 
25 //*****************************************************************/
26 void drv_sleep (double time)
27 {
28  yarp::os::Time::delay(time/1000);
29 }
30 
31 //*****************************************************************/
32 //utility function for conversion of hex string to int
33 int axtoi(char *hexStg)
34 {
35  int n = 0; // position in string
36  int m = 0; // position in digit[] to shift
37  int count; // loop index
38  int intValue = 0; // integer value of hex string
39  int digit[5]; // hold values to convert
40  while (n < 4) {
41  if (hexStg[n]=='\0')
42  break;
43  if (hexStg[n] > 0x29 && hexStg[n] < 0x40 ) //if 0 to 9
44  digit[n] = hexStg[n] & 0x0f; //convert to int
45  else if (hexStg[n] >='a' && hexStg[n] <= 'f') //if a to f
46  digit[n] = (hexStg[n] & 0x0f) + 9; //convert to int
47  else if (hexStg[n] >='A' && hexStg[n] <= 'F') //if A to F
48  digit[n] = (hexStg[n] & 0x0f) + 9; //convert to int
49  else break;
50  n++;
51  }
52  count = n;
53  m = n - 1;
54  n = 0;
55  while(n < count) {
56  // digit[n] is value of hex digit at position n
57  // (m << 2) is the number of positions to shift
58  // OR the bits into return value
59  intValue = intValue | (digit[n] << (m << 2));
60  m--; // adjust the position to set
61  n++; // next digit to process
62  }
63  return (intValue);
64 }
65 
66 //*****************************************************************/
67 
68 int cDownloader::build_id(int source, int dest)
69 {
70  return (ICUBCANPROTO_CLASS_BOOTLOADER << 8) + ( source << 4) + dest;
71 }
72 
73 int cDownloader::get_src_from_id (int id)
74 {
75  // 000 1111 0000
76  return ((id >> 4) & 0x0F);
77 }
78 
79 int cDownloader::get_dst_from_id (int id)
80 {
81  // 000 0000 1111
82  return (id & 0x0F);
83 }
84 
85 //*****************************************************************/
86 
87 cDownloader::cDownloader(bool verbose)
88 {
89  _verbose = verbose;
90  board_list = NULL;
91  board_list_size = 0;
92  connected = false;
93  m_idriver=NULL;
94  sprsPage=0;
95  set_canbus_id(-1);
96 }
97 
98 void cDownloader::set_verbose(bool verbose)
99 {
100  _verbose = verbose;
101 }
102 
103 void cDownloader::set_external_logger(void *caller, void (*logger)(void *, const std::string &))
104 {
105  _externalLoggerFptr = logger;
106  _externalLoggerCaller = caller;
107 }
108 
109 void cDownloader::Log(const std::string &msg)
110 {
111  yDebug() << "$" << msg;
112 
113  if(NULL != _externalLoggerFptr)
114  {
115  _externalLoggerFptr(_externalLoggerCaller, "$ " + msg);
116  }
117 }
118 
119 
120 //*****************************************************************/
121 
122 int cDownloader::stopdriver()
123 {
124  if (m_idriver !=NULL)
125  {
126 #if defined(DOWNLOADER_USE_IDRIVER2)
127  txBuffer.resize(0);
128  rxBuffer.resize(0);
129 #else
130  m_idriver->destroyBuffer(txBuffer);
131  m_idriver->destroyBuffer(rxBuffer);
132 #endif
133  delete m_idriver;
134  m_idriver=NULL;
135  connected = false;
136  }
137  return 0;
138 }
139 
140 //*****************************************************************/
141 
142 int cDownloader::initdriver(Searchable &config, bool verbose)
143 {
144  _verbose = verbose;
145 
146  set_external_logger(NULL, NULL);
147 
148  int ret = 0; // 0 is ok, -1 is failure, -2 is retry ...
149  if (m_idriver !=NULL)
150  {
151  delete m_idriver;
152  m_idriver=NULL;
153  connected = false;
154  }
155 
156  int tmp = 0;
157 
158  if (config.find("device").asString()=="ETH")
159  {
160 #if defined(DOWNLOADER_USE_IDRIVER2)
161  m_idriver = new eDriver2;
162 #else
163  m_idriver = new eDriver;
164 #endif
165  tmp = config.check("canid")?config.find("canid").asInt32():CanPacket::everyCANbus;
166  if((1 != tmp) && (2 != tmp))
167  {
168  tmp = CanPacket::everyCANbus;
169  }
170  }
171  else
172  {
173 #if defined(DOWNLOADER_USE_IDRIVER2)
174  m_idriver = new cDriver2;
175 #else
176  m_idriver = new cDriver;
177 #endif
178  tmp = config.check("canDeviceNum")?config.find("canDeviceNum").asInt32():99;
179  }
180 
181  if (0 != (ret = m_idriver->init(config, _verbose)))
182  {
183  if (m_idriver)
184  {
185  delete m_idriver;
186  m_idriver=NULL;
187  connected = false;
188  }
189  return ret;
190  }
191 
192  set_canbus_id(tmp);
193 
194 
195 #if defined(DOWNLOADER_USE_IDRIVER2)
196  txBuffer.resize(1);
197  txBuffer[0].setCanBus(tmp);
198  rxBuffer.resize(MAX_READ_MSG);
199  for(int i=0; i<MAX_READ_MSG; i++)
200  {
201  rxBuffer[i].setCanBus(tmp);
202  }
203 #else
204  txBuffer=m_idriver->createBuffer(1);
205  rxBuffer=m_idriver->createBuffer(MAX_READ_MSG);
206 #endif
207  connected = true;
208 
209 
210  return ret;
211 }
212 
213 //*****************************************************************/
214 int cDownloader::strain_save_to_eeprom (int bus, int target_id, string *errorstring)
215 {
216  // check if driver is running
217  if (m_idriver == NULL)
218  {
219  if(_verbose) yError ("Driver not ready\n");
220  return -1;
221  }
222 
223  // Send transmission command to strain board
224  txBuffer[0].setId((2 << 8) + target_id);
225  txBuffer[0].setLen(1);
226  txBuffer[0].getData()[0]= 0x09;
227  set_bus(txBuffer[0], bus);
228  int ret = m_idriver->send_message(txBuffer, 1);
229  // check if send_message was successful
230  if (ret==0)
231  {
232  if(_verbose) yError ("Unable to send message\n");
233  return -1;
234  }
235 
236  drv_sleep(5);
237 
238  int read_messages = m_idriver->receive_message(rxBuffer,1);
239  for (int i=0; i<read_messages; i++)
240  {
241  if (rxBuffer[i].getData()[0]==0x09 &&
242  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
243  {
244  if(rxBuffer[i].getData()[1]!=0)
245  {
246  if(_verbose) yInfo ("Data has been saved in EEprom correctly\n");
247  return 1;
248  }
249  else
250  {
251  if(_verbose) yError ("Error in data saving in EEprom \n");
252  return 0;
253  }
254  }
255  }
256  if(_verbose) yError ("Save_to_eeprom didn't receive answer...maybe strain firmware is obsolete \n");
257  return -1;
258 
259 }
260 
261 //*****************************************************************/
262 //int cDownloader::sg6_obsolete_get_amp_gain (int bus, int target_id, char channel, unsigned int& gain1, unsigned int& gain2 )
263 //{
264 //#if 1
265 // yError ("sg6_obsolete_get_amp_gain() is obsolete. \n");
266 // return -1;
267 //#else
268 // // check if driver is running
269 // if (m_idriver == NULL)
270 // {
271 // if(_verbose) yError ("Driver not ready\n");
272 // return -1;
273 // }
274 //#error command ID 0x1D is not available anymore
275 // // Send read gain command to strain board
276 // txBuffer[0].setId((2 << 8) + target_id);
277 // txBuffer[0].setLen(2);
278 // txBuffer[0].getData()[0]= 0x1D;
279 // txBuffer[0].getData()[1]= channel;
280 // set_bus(txBuffer[0], bus);
281 // int ret = m_idriver->send_message(txBuffer, 1);
282 // // check if send_message was successful
283 // if (ret==0)
284 // {
285 // if(_verbose) yError ("Unable to send message\n");
286 // return -1;
287 // }
288 
289 // drv_sleep(3);
290 
291 // //read gain
292 // int read_messages = m_idriver->receive_message(rxBuffer,1);
293 // for (int i=0; i<read_messages; i++)
294 // {
295 // if (rxBuffer[i].getData()[0]==0x1D &&
296 // rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
297 // {
298 // int ret_channel = rxBuffer[i].getData()[1];
299 // if (ret_channel == channel)
300 // {
301 // gain1 = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
302 // gain2 = rxBuffer[i].getData()[4]<<8 | rxBuffer[i].getData()[5];
303 // return 0;
304 // }
305 // else
306 // {
307 // if(_verbose) yError ("sg6_get_amp_gain : invalid response\n");
308 // return -1;
309 // }
310 // }
311 // }
312 
313 // return -1;
314 //#endif
315 //}
316 
317 //*****************************************************************/
318 //int cDownloader::sg6_obsolete_set_amp_gain (int bus, int target_id, char channel, unsigned int gain1, unsigned int gain2 )
319 //{
320 //#if 1
321 // yError ("sg6_obsolete_set_amp_gain() is obsolete. \n");
322 // return -1;
323 //#else
324 // // check if driver is running
325 // if (m_idriver == NULL)
326 // {
327 // if(_verbose) yError ("Driver not ready\n");
328 // return -1;
329 // }
330 //#error command ID 0x1E is not available anymore
331 // //set amp gain
332 // txBuffer[0].setId((2 << 8) + target_id);
333 // txBuffer[0].setLen(6);
334 // txBuffer[0].getData()[0]= 0x1E;
335 // txBuffer[0].getData()[1]= channel;
336 // txBuffer[0].getData()[2]= gain1 >> 8;
337 // txBuffer[0].getData()[3]= gain1 & 0xFF;
338 // txBuffer[0].getData()[4]= gain2 >> 8;
339 // txBuffer[0].getData()[5]= gain2 & 0xFF;
340 // set_bus(txBuffer[0], bus);
341 // int ret = m_idriver->send_message(txBuffer, 1);
342 
343 // return 0;
344 //#endif
345 //}
346 
347 //*****************************************************************/
348 int cDownloader::strain_get_adc(int bus, int target_id, char channel, unsigned int& adc, int type, string *errorstring)
349 {
350  // check if driver is running
351  if (m_idriver == NULL)
352  {
353  if(_verbose) yError ("Driver not ready\n");
354  return -1;
355  }
356 
357  // Send read channel command to strain board
358  txBuffer[0].setId((2 << 8) + target_id);
359  txBuffer[0].setLen(3);
360  txBuffer[0].getData()[0]= 0x0C;
361  txBuffer[0].getData()[1]= channel;
362  txBuffer[0].getData()[2]= type;
363  set_bus(txBuffer[0], bus);
364  int ret = m_idriver->send_message(txBuffer, 1);
365  // check if send_message was successful
366  if (ret==0)
367  {
368  if(_verbose) yError ("Unable to send message\n");
369  return -1;
370  }
371 
372  drv_sleep(3);
373 
374  //read adc
375  int read_messages = m_idriver->receive_message(rxBuffer,1);
376  for (int i=0; i<read_messages; i++)
377  {
378  if (rxBuffer[i].getData()[0]==0x0C &&
379  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
380  {
381  int ret_channel = rxBuffer[i].getData()[1];
382  if (ret_channel == channel)
383  {
384  adc = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
385  return 0;
386  }
387  else
388  {
389  if(_verbose) yError ("strain_get_adc : invalid response\n");
390  return -1;
391  }
392  }
393  }
394 
395  return -1;
396 }
397 
398 //*****************************************************************/
399 int cDownloader::strain_get_offset(int bus, int target_id, char channel, unsigned int& offset, int regset, string *errorstring)
400 {
401  // check if driver is running
402  if (m_idriver == NULL)
403  {
404  if(_verbose) yError ("Driver not ready\n");
405  return -1;
406  }
407 
408  //read dac
409  txBuffer[0].setId((2 << 8) + target_id);
410  txBuffer[0].setLen(2);
411  txBuffer[0].getData()[0]= 0x0B;
412  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
413 
414  clean_rx();
415  set_bus(txBuffer[0], bus);
416  int ret = m_idriver->send_message(txBuffer, 1);
417 
418  drv_sleep(3);
419 
420  int read_messages = m_idriver->receive_message(rxBuffer,1);
421  for (int i=0; i<read_messages; i++)
422  {
423  if (rxBuffer[i].getData()[0]==0x0B &&
424  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
425  {
426  int ret_channel = rxBuffer[i].getData()[1] & 0x0f;
427  if (channel==ret_channel)
428  {
429  offset = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
430  return 0;
431  }
432  else
433  {
434  if(_verbose) yError ("strain_get_offset : invalid response\n");
435  return -1;
436  }
437 
438  }
439  }
440 
441  return -1;
442 }
443 //*****************************************************************/
444 int cDownloader::strain_get_calib_bias (int bus, int target_id, char channel, signed int& bias, int regset, string *errorstring)
445 {
446  // check if driver is running
447  if (m_idriver == NULL)
448  {
449  if(_verbose) yError ("Driver not ready\n");
450  return -1;
451  }
452 
453  //read current bias
454  txBuffer[0].setId((2 << 8) + target_id);
455  txBuffer[0].setLen(2);
456  txBuffer[0].getData()[0]= 0x14;
457  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
458  set_bus(txBuffer[0], bus);
459  int ret = m_idriver->send_message(txBuffer, 1);
460 
461  int read_messages = m_idriver->receive_message(rxBuffer,1);
462  for (int i=0; i<read_messages; i++)
463  {
464  if (rxBuffer[i].getData()[0]==0x14 &&
465  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
466  {
467  bias = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
468  return 0;
469  }
470  }
471  return -1;
472 }
473 
474 // this funtion set the calib bias = to the negative of adc for every channel, hence it must be only for strain_regset_inuse
475 //*****************************************************************/
476 int cDownloader::strain_set_calib_bias (int bus, int target_id, string *errorstring)
477 {
478  // check if driver is running
479  if (m_idriver == NULL)
480  {
481  if(_verbose) yError ("Driver not ready\n");
482  return -1;
483  }
484 
485  //set calib bias
486  txBuffer[0].setId((2 << 8) + target_id);
487  txBuffer[0].setLen(2);
488  txBuffer[0].getData()[0]= 0x13;
489  txBuffer[0].getData()[1]= 1;
490  set_bus(txBuffer[0], bus);
491  int ret = m_idriver->send_message(txBuffer, 1);
492  drv_sleep(5);
493 
494  return 0;
495 }
496 //*****************************************************************/
497 int cDownloader::strain_set_calib_bias (int bus, int target_id, char channel, int bias, int regset, string *errorstring)
498 {
499  // check if driver is running
500  if (m_idriver == NULL)
501  {
502  if(_verbose) yError ("Driver not ready\n");
503  return -1;
504  }
505 
506  //set calib bias
507  txBuffer[0].setId((2 << 8) + target_id);
508  txBuffer[0].setLen(5);
509  txBuffer[0].getData()[0]= 0x13;
510  txBuffer[0].getData()[1]= 2;
511  txBuffer[0].getData()[2]= ((regset << 4) & 0xf0) | (channel & 0x0f);
512  txBuffer[0].getData()[3]= bias >> 8;
513  txBuffer[0].getData()[4]= bias & 0xFF;
514 
515  set_bus(txBuffer[0], bus);
516  int ret = m_idriver->send_message(txBuffer, 1);
517  drv_sleep(5);
518 
519  return 0;
520 }
521 
522 // this funtion set the calib bias = 0 for every channel, hence it must be only for strain_regset_inuse
523 //*****************************************************************/
524 int cDownloader::strain_reset_calib_bias (int bus, int target_id, string *errorstring)
525 {
526  // check if driver is running
527  if (m_idriver == NULL)
528  {
529  if(_verbose) yError ("Driver not ready\n");
530  return -1;
531  }
532 
533  //reset calib bias
534  txBuffer[0].setId((2 << 8) + target_id);
535  txBuffer[0].setLen(2);
536  txBuffer[0].getData()[0]= 0x13;
537  txBuffer[0].getData()[1]= 0;
538  set_bus(txBuffer[0], bus);
539  int ret = m_idriver->send_message(txBuffer, 1);
540  drv_sleep(5);
541 
542  return 0;
543 }
544 //*****************************************************************/
545 int cDownloader::strain_get_curr_bias (int bus, int target_id, char channel, signed int& bias, string *errorstring)
546 {
547  // check if driver is running
548  if (m_idriver == NULL)
549  {
550  if(_verbose) yError ("Driver not ready\n");
551  return -1;
552  }
553 
554  //read current bias
555  txBuffer[0].setId((2 << 8) + target_id);
556  txBuffer[0].setLen(2);
557  txBuffer[0].getData()[0]= 0x16;
558  txBuffer[0].getData()[1]= channel;
559  set_bus(txBuffer[0], bus);
560  int ret = m_idriver->send_message(txBuffer, 1);
561 
562  int read_messages = m_idriver->receive_message(rxBuffer,1);
563  for (int i=0; i<read_messages; i++)
564  {
565  if (rxBuffer[i].getData()[0]==0x16 &&
566  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
567  {
568  bias = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
569  return 0;
570  }
571  }
572  return -1;
573 }
574 //*****************************************************************/
575 int cDownloader::strain_set_curr_bias (int bus, int target_id, string *errorstring)
576 {
577  // check if driver is running
578  if (m_idriver == NULL)
579  {
580  if(_verbose) yError ("Driver not ready\n");
581  return -1;
582  }
583 
584  //set curr bias
585  txBuffer[0].setId((2 << 8) + target_id);
586  txBuffer[0].setLen(2);
587  txBuffer[0].getData()[0]= 0x15;
588  txBuffer[0].getData()[1]= 1;
589  set_bus(txBuffer[0], bus);
590  int ret = m_idriver->send_message(txBuffer, 1);
591  drv_sleep(5);
592 
593  return 0;
594 }
595 
596 //*****************************************************************/
597 int cDownloader::strain_set_curr_bias (int bus, int target_id, char channel, int bias, string *errorstring)
598 {
599  // check if driver is running
600  if (m_idriver == NULL)
601  {
602  if(_verbose) yError ("Driver not ready\n");
603  return -1;
604  }
605 
606  //set calib bias
607  txBuffer[0].setId((2 << 8) + target_id);
608  txBuffer[0].setLen(5);
609  txBuffer[0].getData()[0]= 0x15;
610  txBuffer[0].getData()[1]= 2;
611  txBuffer[0].getData()[3]= channel;
612  txBuffer[0].getData()[4]= bias >> 8;
613  txBuffer[0].getData()[5]= bias & 0xFF;
614 
615  set_bus(txBuffer[0], bus);
616  int ret = m_idriver->send_message(txBuffer, 1);
617  drv_sleep(5);
618 
619  return 0;
620 }
621 //*****************************************************************/
622 int cDownloader::strain_reset_curr_bias (int bus, int target_id, string *errorstring)
623 {
624  // check if driver is running
625  if (m_idriver == NULL)
626  {
627  if(_verbose) yError ("Driver not ready\n");
628  return -1;
629  }
630 
631  //reset curr bias
632  txBuffer[0].setId((2 << 8) + target_id);
633  txBuffer[0].setLen(2);
634  txBuffer[0].getData()[0]= 0x15;
635  txBuffer[0].getData()[1]= 0;
636  set_bus(txBuffer[0], bus);
637  int ret = m_idriver->send_message(txBuffer, 1);
638  drv_sleep(5);
639 
640  return 0;
641 }
642 
643 //*****************************************************************/
644 int cDownloader::strain_set_serial_number (int bus, int target_id, const char* serial_number, string *errorstring)
645 {
646  // check if driver is running
647  if (m_idriver == NULL)
648  {
649  if(_verbose) yError ("Driver not ready\n");
650  return -1;
651  }
652 
653  //set dac
654  txBuffer[0].setId((2 << 8) + target_id);
655  txBuffer[0].setLen(8);
656  txBuffer[0].getData()[0]= 0x19;
657  txBuffer[0].getData()[1]= serial_number[0];
658  txBuffer[0].getData()[2]= serial_number[1];
659  txBuffer[0].getData()[3]= serial_number[2];
660  txBuffer[0].getData()[4]= serial_number[3];
661  txBuffer[0].getData()[5]= serial_number[4];
662  txBuffer[0].getData()[6]= serial_number[5];
663  txBuffer[0].getData()[7]= serial_number[6];
664  set_bus(txBuffer[0], bus);
665  int ret = m_idriver->send_message(txBuffer, 1);
666  drv_sleep(5);
667 
668  return 0;
669 }
670 
671 //*****************************************************************/
672 int cDownloader::strain_get_serial_number (int bus, int target_id, char* serial_number, string *errorstring)
673 {
674  // check if driver is running
675  if (m_idriver == NULL)
676  {
677  if(_verbose) yError ("Driver not ready\n");
678  return -1;
679  }
680 
681  //read serial number
682  txBuffer[0].setId((2 << 8) + target_id);
683  txBuffer[0].setLen(1);
684  txBuffer[0].getData()[0]= 0x1A;
685 
686  clean_rx();
687  set_bus(txBuffer[0], bus);
688  int ret = m_idriver->send_message(txBuffer, 1);
689 
690  drv_sleep(5);
691 
692  int read_messages = m_idriver->receive_message(rxBuffer,1);
693  for (int i=0; i<read_messages; i++)
694  {
695  if (rxBuffer[i].getData()[0]==0x1A &&
696  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
697  {
698  serial_number[0] = rxBuffer[i].getData()[1];
699  serial_number[1] = rxBuffer[i].getData()[2];
700  serial_number[2] = rxBuffer[i].getData()[3];
701  serial_number[3] = rxBuffer[i].getData()[4];
702  serial_number[4] = rxBuffer[i].getData()[5];
703  serial_number[5] = rxBuffer[i].getData()[6];
704  serial_number[6] = rxBuffer[i].getData()[7];
705  serial_number[7] = 0;
706  return 0;
707  }
708  }
709  return -1;
710 }
711 
712 //*****************************************************************/
713 int cDownloader::strain_get_eeprom_saved (int bus, int target_id, bool* status, string *errorstring)
714 {
715  // check if driver is running
716  if (m_idriver == NULL)
717  {
718  if(_verbose) yError ("Driver not ready\n");
719  return -1;
720  }
721 
722  //read eeprom status (saved/not saved)
723  txBuffer[0].setId((2 << 8) + target_id);
724  txBuffer[0].setLen(1);
725  txBuffer[0].getData()[0]= 0x1B;
726 
727  clean_rx();
728  set_bus(txBuffer[0], bus);
729  int ret = m_idriver->send_message(txBuffer, 1);
730 
731  drv_sleep(5);
732 
733  int read_messages = m_idriver->receive_message(rxBuffer,1);
734  for (int i=0; i<read_messages; i++)
735  {
736  if (rxBuffer[i].getData()[0]==0x1B &&
737  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
738  {
739  *status = rxBuffer[i].getData()[1]!=0;
740  return 0;
741  }
742  }
743  return -1;
744 }
745 //*****************************************************************/
746 int cDownloader::strain_get_matrix_gain (int bus, int target_id, unsigned int& gain, int regset, string *errorstring)
747 {
748  // check if driver is running
749  if (m_idriver == NULL)
750  {
751  if(_verbose) yError ("Driver not ready\n");
752  return -1;
753  }
754 
755  // read matrix gain
756  txBuffer[0].setId((2 << 8) + target_id);
757  txBuffer[0].setLen(1);
758  txBuffer[0].getData()[0]= 0x12;
759 
760  clean_rx();
761  set_bus(txBuffer[0], bus);
762  int ret = m_idriver->send_message(txBuffer, 1);
763 
764  drv_sleep(5);
765 
766  int read_messages = m_idriver->receive_message(rxBuffer,1);
767  for (int i=0; i<read_messages; i++)
768  {
769  if (rxBuffer[i].getData()[0]==0x12 &&
770  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
771  {
772  gain = rxBuffer[i].getData()[1];
773  return 0;
774  }
775  }
776  return -1;
777 }
778 
779 //*****************************************************************/
780 //int cDownloader::strain_set_matrix(int bus, int target_id, int matrix, string *errorstring)
781 //{
782 // if(0 == matrix)
783 // {
784 // return 0;
785 // }
786 
787 // if(NULL != errorstring)
788 // {
789 // *errorstring += "cDownloader::strain_set_matrix() cannot set a non zero matrix number";
790 // //*errorstring += std::to_string(matrix); // c++ 11 ...........
791 // }
792 // return -1;
793 //}
794 
795 //*****************************************************************/
796 
797 //int cDownloader::strain_get_matrix(int bus, int target_id, int &matrix, string *errorstring)
798 //{
799 // matrix = 0;
800 // return 0;
801 //}
802 
803 
804 int cDownloader::strain_set_regulationset(int bus, int target_id, int regset, int regsetmode, string *errorstring)
805 {
806 #if 0
807  return 0;
808 #else
809 
810  // check if driver is running
811  if (m_idriver == NULL)
812  {
813  if(_verbose) yError ("Driver not ready\n");
814  return -1;
815  }
816 
817  if(strain_regset_inuse == regset)
818  {
819  return -1;
820  }
821 
822  // set regulation set
823  txBuffer[0].setId((2 << 8) + target_id);
824  txBuffer[0].setLen(2);
825  txBuffer[0].getData()[0]= 0x3D;
826  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (regsetmode & 0x0f);
827 
828  set_bus(txBuffer[0], bus);
829  int ret = m_idriver->send_message(txBuffer, 1);
830  drv_sleep(5);
831 
832  return 0;
833 
834 #endif
835 }
836 
837 int cDownloader::strain_get_regulationset(int bus, int target_id, int &regset, const int regsetmode, string *errorstring)
838 {
839 #if 0
840  regset = strain_regset_one;
841  return 0;
842 #else
843  // send the command
844 
845  // check if driver is running
846  if (m_idriver == NULL)
847  {
848  if(_verbose) yError ("Driver not ready\n");
849  return -1;
850  }
851 
852  // read reg set
853  txBuffer[0].setId((2 << 8) + target_id);
854  txBuffer[0].setLen(2);
855  txBuffer[0].getData()[0]= 0x3E;
856  txBuffer[0].getData()[1]= (regsetmode & 0x0f);
857 
858  clean_rx();
859  set_bus(txBuffer[0], bus);
860  int ret = m_idriver->send_message(txBuffer, 1);
861 
862  drv_sleep(3);
863 
864  int read_messages = m_idriver->receive_message(rxBuffer,1);
865  for (int i=0; i<read_messages; i++)
866  {
867  if (rxBuffer[i].getData()[0]== 0x3E &&
868  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
869  {
870  int rregset = (rxBuffer[i].getData()[1] >> 4) & 0x0F;
871  int rregsetmode = (rxBuffer[i].getData()[1]) & 0x0F;
872  if (rregsetmode == regsetmode)
873  {
874  regset = rregset;
875  return 0;
876  }
877  else
878  {
879  if(_verbose) yError ("strain_get_regulationset : invalid response\n");
880  return -1;
881  }
882 
883  }
884  }
885 
886  return -1;
887 #endif
888 }
889 
890 //*****************************************************************/
891 int cDownloader::strain_set_matrix_gain (int bus, int target_id, unsigned int gain, int regset, string *errorstring)
892 {
893  // check if driver is running
894  if (m_idriver == NULL)
895  {
896  if(_verbose) yError ("Driver not ready\n");
897  return -1;
898  }
899 
900  // set matrix gain
901  txBuffer[0].setId((2 << 8) + target_id);
902  txBuffer[0].setLen(2);
903  txBuffer[0].getData()[0]= 0x11;
904  txBuffer[0].getData()[1]= gain;
905 
906  set_bus(txBuffer[0], bus);
907  int ret = m_idriver->send_message(txBuffer, 1);
908  drv_sleep(5);
909 
910  return 0;
911 }
912 
913 int cDownloader::strain_set_amplifier_regs(int bus, int target_id, unsigned char channel, const strain2_ampl_regs_t &ampregs, int regset, string *errorstring)
914 {
915  // check if driver is running
916  if (m_idriver == NULL)
917  {
918  if(_verbose) yError ("Driver not ready\n");
919  return -1;
920  }
921 
922  // not for normal strain
923 
924  txBuffer[0].setId((2 << 8) + target_id);
925  txBuffer[0].setLen(8);
926  txBuffer[0].getData()[0]= 0x2B;
927  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
928  txBuffer[0].getData()[2]= ampregs.data[0]; // lsb of gd
929  txBuffer[0].getData()[3]= ampregs.data[1]; // msb of gd
930  txBuffer[0].getData()[4]= ampregs.data[2];
931  txBuffer[0].getData()[5]= ampregs.data[3]; // vc0
932  txBuffer[0].getData()[6]= ampregs.data[4];
933  txBuffer[0].getData()[7]= ampregs.data[5];
934  set_bus(txBuffer[0], bus);
935  //yDebug("strain_set_amplifier_regs() is sending: [%x, %x, %x, %x, %x, %x, %x, %x]", txBuffer[0].getData()[0], txBuffer[0].getData()[1], txBuffer[0].getData()[2], txBuffer[0].getData()[3], txBuffer[0].getData()[4], txBuffer[0].getData()[5], txBuffer[0].getData()[6], txBuffer[0].getData()[7]);
936 
937  m_idriver->send_message(txBuffer, 1);
938 
939  // i wait some 10 ms
940  yarp::os::Time::delay(0.010);
941 
942 
943  return 0;
944 }
945 
946 
947 int cDownloader::strain_get_amplifier_regs(int bus, int target_id, unsigned char channel, strain2_ampl_regs_t &ampregs, int regset, string *errorstring)
948 {
949  // check if driver is running
950  if (m_idriver == NULL)
951  {
952  if(_verbose) yError ("Driver not ready\n");
953  return -1;
954  }
955 
956  // not for normal strain
957 
958  txBuffer[0].setId((2 << 8) + target_id);
959  txBuffer[0].setLen(2);
960  txBuffer[0].getData()[0]= 0x2A;
961  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);;
962  set_bus(txBuffer[0], bus);
963 
964  //yDebug("strain_get_amplifier_regs() is sending: [%x, %x, %x, %x, %x, %x, %x, %x]", txBuffer[0].getData()[0], txBuffer[0].getData()[1], txBuffer[0].getData()[2], txBuffer[0].getData()[3], txBuffer[0].getData()[4], txBuffer[0].getData()[5], txBuffer[0].getData()[6], txBuffer[0].getData()[7]);
965 
966  m_idriver->send_message(txBuffer, 1);
967 
968  // i wait some 10 ms
969  yarp::os::Time::delay(0.010);
970 
971  int rm = m_idriver->receive_message(rxBuffer, 1, 1.0);
972  for(int i=0; i<rm; i++)
973  {
974  if (rxBuffer[i].getData()[0]==0x2A)
975  {
976  ampregs.data[0] = rxBuffer[i].getData()[2];
977  ampregs.data[1] = rxBuffer[i].getData()[3];
978  ampregs.data[2] = rxBuffer[i].getData()[4];
979  ampregs.data[3] = rxBuffer[i].getData()[5];
980  ampregs.data[4] = rxBuffer[i].getData()[6];
981  ampregs.data[5] = rxBuffer[i].getData()[7];
982  //uint8_t from = rxBuffer[i].getData()[1];
983  //yDebug("from %d: [%x, %x, %x, %x, %x, %x]", from, rxBuffer[i].getData()[2], rxBuffer[i].getData()[3], rxBuffer[i].getData()[4], rxBuffer[i].getData()[5], rxBuffer[i].getData()[6], rxBuffer[i].getData()[7]);
984  }
985  break;
986  }
987 
988 
989  return 0;
990 }
991 
992 float cDownloader::strain_amplifier_discretegain2float(strain2_ampl_discretegain_t c)
993 {
994  //Luca
995  static const float mapofgains[ampl_gain_numberOf] =
996  {
997  256 , 128 , 96 , 64 , 48, 36, 24, 20, 16, 10, 8, 6, 4
998  };
999 
1000  unsigned int index = static_cast<unsigned int>(c);
1001  if(index >= ampl_gain_numberOf)
1002  {
1003  if(_verbose) yError ("strain_amplifier_discretegain2float(): cannot convert to a valid value\n");
1004  return 0.0f;
1005  }
1006  return mapofgains[index];
1007 }
1008 
1009 int cDownloader::strain_set_amplifier_discretegain(int bus, int target_id, unsigned char channel, strain2_ampl_discretegain_t ampcfg, int regset, string *errorstring)
1010 {
1011  // check if driver is running
1012  if (m_idriver == NULL)
1013  {
1014  if(_verbose) yError ("Driver not ready\n");
1015  return -1;
1016  }
1017 
1018  // not for normal strain
1019 
1020  #define _NUMofREGS 6
1021 
1022 
1023  // constant value of offset registers
1024 // const uint8_t _cfg1map[ampl_gain_numberOf][_NUMofREGS] =
1025 // {
1026 // {0x00, 0x40, 0x46, 0x25, 0x00, 0x80}, // gain = 48
1027 // {0x00, 0x10, 0x46, 0x25, 0x00, 0x80}, // gain = 36
1028 // {0x00, 0x40, 0x42, 0x25, 0x00, 0x80}, // gain = 24 [or 0x26 instead of 0x42]
1029 // {0x00, 0x20, 0x42, 0x25, 0x00, 0x80}, // gain = 20 [or 0x26 instead of 0x42]
1030 // {0x00, 0x00, 0x42, 0x25, 0x00, 0x80}, // gain = 16 [or 0x26 instead of 0x42]
1031 // {0x00, 0xC0, 0x02, 0x25, 0x00, 0x80}, // gain = 10 [or 0x10 instead of 0x02]
1032 // {0x00, 0x80, 0x02, 0x25, 0x00, 0x80}, // gain = 08 [or 0x10 instead of 0x02]
1033 // {0x00, 0x40, 0x02, 0x25, 0x00, 0x80}, // gain = 06 [or 0x10 instead of 0x02]
1034 // {0x00, 0x40, 0x00, 0x25, 0x00, 0x80} // gain = 04
1035 // };
1036 
1037  // offset all equal to 32k-1
1038  static const uint8_t _cfg1map[ampl_gain_numberOf][_NUMofREGS] =
1039  {
1040  //Luca
1041  {0x00, 0x80, 0x66, 0x06, 0xcb, 0x80}, // gain = 256
1042  {0x00, 0x80, 0x56, 0x0c, 0xcb, 0x80}, // gain = 128
1043  {0x00, 0x40, 0x56, 0x10, 0x0f, 0x81}, // gain = 96
1044  {0x00, 0x80, 0x46, 0x17, 0x6d, 0x7f}, // gain = 64
1045  {0x00, 0x40, 0x46, 0x1f, 0xb1, 0x7f}, // gain = 48
1046  {0x00, 0x10, 0x46, 0x2a, 0x80, 0x80}, // gain = 36
1047  {0x00, 0x40, 0x42, 0x3e, 0x62, 0x7f}, // gain = 24 [or 0x26 instead of 0x42]
1048  {0x00, 0x20, 0x42, 0x4b, 0x15, 0x80}, // gain = 20 [or 0x26 instead of 0x42]
1049  {0x00, 0x00, 0x42, 0x5e, 0x72, 0x80}, // gain = 16 [or 0x26 instead of 0x42]
1050  {0x00, 0xC0, 0x02, 0x64, 0xf6, 0x6e}, // gain = 10 [or 0x10 instead of 0x02]
1051  {0x00, 0x80, 0x02, 0x64, 0x29, 0x62}, // gain = 08 [or 0x10 instead of 0x02]
1052  {0x00, 0x40, 0x02, 0x64, 0xd4, 0x4c}, // gain = 06 [or 0x10 instead of 0x02]
1053  {0x00, 0x40, 0x00, 0x64, 0x29, 0x22} // gain = 04
1054  };
1055 
1056 
1057  unsigned int index = static_cast<unsigned int>(ampcfg);
1058 
1059  if(index >= ampl_gain_numberOf)
1060  {
1061  if(_verbose) yError ("strain_set_amplifier_discretegain(): cannot convert to a valid index\n");
1062  return -1;
1063  }
1064 
1065 
1066  txBuffer[0].setId((2 << 8) + target_id);
1067  txBuffer[0].setLen(8);
1068  txBuffer[0].getData()[0]= 0x2B;
1069  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
1070  txBuffer[0].getData()[2]= _cfg1map[index][0]; // lsb of gd
1071  txBuffer[0].getData()[3]= _cfg1map[index][1]; // msb of gd
1072  txBuffer[0].getData()[4]= _cfg1map[index][2];
1073  txBuffer[0].getData()[5]= _cfg1map[index][3]; // vc0
1074  txBuffer[0].getData()[6]= _cfg1map[index][4];
1075  txBuffer[0].getData()[7]= _cfg1map[index][5];
1076  set_bus(txBuffer[0], bus);
1077  if(_verbose) yDebug("strain_set_amplifier_discretegain() is sending: [%x, %x, %x, %x, %x, %x, %x, %x]", txBuffer[0].getData()[0], txBuffer[0].getData()[1], txBuffer[0].getData()[2], txBuffer[0].getData()[3], txBuffer[0].getData()[4], txBuffer[0].getData()[5], txBuffer[0].getData()[6], txBuffer[0].getData()[7]);
1078 
1079  m_idriver->send_message(txBuffer, 1);
1080 
1081  // i wait some 10 ms
1082  yarp::os::Time::delay(0.010);
1083 
1084 
1085  return 0;
1086 }
1087 
1088 int cDownloader::strain_get_amplifier_gain_offset (int bus, int target_id, unsigned char channel, float &gain, uint16_t &offset, int regset, string *errorstring)
1089 {
1090  // check if driver is running
1091  if (m_idriver == NULL)
1092  {
1093  if(_verbose) yError ("Driver not ready\n");
1094  return -1;
1095  }
1096 
1097  txBuffer[0].setId((2 << 8) + target_id);
1098  txBuffer[0].setLen(2);
1099  txBuffer[0].getData()[0]= 0x20;
1100  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
1101  set_bus(txBuffer[0], bus);
1102  m_idriver->send_message(txBuffer, 1);
1103 
1104 
1105 // for(int nr=0; nr<1; nr++)
1106  {
1107  int rm = m_idriver->receive_message(rxBuffer, 1, 1.0);
1108  for(int i=0; i<rm; i++)
1109  {
1110  if (rxBuffer[i].getData()[0]==0x20)
1111  {
1112 
1113  // uint8_t chn = rxBuffer[i].getData()[1] & 0x0f;
1114  uint16_t g16 = static_cast<uint16_t>(rxBuffer[i].getData()[2]) | static_cast<uint16_t>(rxBuffer[i].getData()[3]) << 8;
1115  float fg = static_cast<float>(g16) / 100;
1116  gain = fg;
1117  uint16_t o16 = static_cast<uint16_t>(rxBuffer[i].getData()[4]) | static_cast<uint16_t>(rxBuffer[i].getData()[5]) << 8;
1118  offset = o16;
1119  }
1120  break;
1121  }
1122  }
1123 
1124  return 0;
1125 }
1126 
1127 int cDownloader::strain_set_amplifier_gain_offset(int bus, int target_id, unsigned char channel, float gain, uint16_t offset, int regset, string *errorstring)
1128 {
1129  // check if driver is running
1130  if (m_idriver == NULL)
1131  {
1132  if(_verbose) yError ("Driver not ready\n");
1133  return -1;
1134  }
1135 
1136 #if 0
1137  txBuffer[0].setId((2 << 8) + target_id);
1138  txBuffer[0].setLen(7);
1139  txBuffer[0].getData()[0]= 0x21;
1140  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
1141  txBuffer[0].getData()[2]= 0; // mode is set both of them
1142  uint16_t gg = static_cast<uint16_t>(gain*100.0f);
1143  txBuffer[0].getData()[3] = (gg & 0x00ff); // little endian
1144  txBuffer[0].getData()[4] = (gg & 0xff00) >> 8; // little endian
1145  txBuffer[0].getData()[5] = (offset & 0x00ff); // little endian
1146  txBuffer[0].getData()[6] = (offset & 0xff00) >> 8; // little endian
1147 
1148  set_bus(txBuffer[0], bus);
1149  int ret = m_idriver->send_message(txBuffer, 1);
1150  drv_sleep(5);
1151 
1152  return 0;
1153 
1154 #else
1155 
1156  strain::amplifier::PGA308 pga;
1157 
1158  strain::amplifier::DiscreteParams dp;
1159  strain::amplifier::PGA308::Registers regs;
1160 
1161  strain::amplifier::WideParams wp;
1162 
1163  wp.load(gain, offset);
1164  if(false == convert(wp, dp))
1165  {
1166  if(_verbose) yError ("cannot load this pair gain-offset into the strain2\n");
1167  return -1;
1168  }
1169 
1170  // load dp and ...
1171  pga.import(dp, &regs);
1172  strain2_ampl_regs_t reg2tx;
1173  size_t s;
1174  regs.fill(reg2tx.memory(), s);
1175  // now in reg2tx we have what we want to tx.
1176 
1177  return strain_set_amplifier_regs(bus, target_id, channel, reg2tx, regset, errorstring);
1178 
1179 #endif
1180 }
1181 
1182 
1183 //*****************************************************************/
1184 int cDownloader::strain_get_full_scale (int bus, int target_id, unsigned char channel, unsigned int& full_scale, int regset, string *errorstring)
1185 {
1186  // check if driver is running
1187  if (m_idriver == NULL)
1188  {
1189  if(_verbose) yError ("Driver not ready\n");
1190  return -1;
1191  }
1192 
1193  //read matrix gain
1194  txBuffer[0].setId((2 << 8) + target_id);
1195  txBuffer[0].setLen(2);
1196  txBuffer[0].getData()[0]= 0x18;
1197  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
1198 
1199  set_bus(txBuffer[0], bus);
1200  int ret = m_idriver->send_message(txBuffer, 1);
1201 
1202  drv_sleep(5);
1203 
1204  int read_messages = m_idriver->receive_message(rxBuffer,1);
1205  for (int i=0; i<read_messages; i++)
1206  {
1207  std::uint8_t data1 = rxBuffer[0].getData()[1];
1208  std::uint8_t rs = (data1 >> 4) & 0x0f;
1209  std::uint8_t cc = (data1 ) & 0x0f;
1210 
1211  if (rxBuffer[i].getData()[0]==0x18 &&
1212  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
1213  {
1214  full_scale = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
1215  return 0;
1216  }
1217  }
1218  return -1;
1219 }
1220 //*****************************************************************/
1221 int cDownloader::strain_set_full_scale (int bus, int target_id, unsigned char channel, unsigned int full_scale, int regset, string *errorstring)
1222 {
1223  // check if driver is running
1224  if (m_idriver == NULL)
1225  {
1226  if(_verbose) yError ("Driver not ready\n");
1227  return -1;
1228  }
1229 
1230  // set fs
1231  txBuffer[0].setId((2 << 8) + target_id);
1232  txBuffer[0].setLen(4);
1233  txBuffer[0].getData()[0]= 0x17;
1234  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
1235  txBuffer[0].getData()[2]= full_scale >> 8;
1236  txBuffer[0].getData()[3]= full_scale & 0xFF;
1237 
1238  set_bus(txBuffer[0], bus);
1239  int ret = m_idriver->send_message(txBuffer, 1);
1240  drv_sleep(5);
1241 
1242  return 0;
1243 }
1244 //*****************************************************************/
1245 int cDownloader::strain_get_matrix_rc (int bus, int target_id, char r, char c, unsigned int& elem, int regset, string *errorstring)
1246 {
1247  // check if driver is running
1248  if (m_idriver == NULL)
1249  {
1250  if(_verbose) yError ("Driver not ready\n");
1251  return -1;
1252  }
1253 
1254  // read dac
1255  txBuffer[0].setId((2 << 8) + target_id);
1256  txBuffer[0].setLen(3);
1257  txBuffer[0].getData()[0]= 0x0A;
1258  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (r & 0x0f);
1259  txBuffer[0].getData()[2]= c;
1260 
1261  clean_rx();
1262  set_bus(txBuffer[0], bus);
1263  int ret = m_idriver->send_message(txBuffer, 1);
1264 
1265  drv_sleep(3);
1266 
1267  int read_messages = m_idriver->receive_message(rxBuffer,1);
1268  for (int i=0; i<read_messages; i++)
1269  {
1270  if (rxBuffer[i].getData()[0]==0x0A &&
1271  rxBuffer[i].getId()==(2 << 8) + (target_id<<4))
1272  {
1273  int ret_r = (rxBuffer[i].getData()[1]) & 0x0f;
1274  int ret_c = rxBuffer[i].getData()[2];
1275  if ((r==ret_r) && (c==ret_c))
1276  {
1277  elem = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
1278  return 0;
1279  }
1280  else
1281  {
1282  if(_verbose) yError ("strain_get_matrix_rc : invalid response\n");
1283  return -1;
1284  }
1285  }
1286  }
1287  return -1;
1288 }
1289 
1290 //*****************************************************************/
1291 int cDownloader::strain_set_matrix_rc (int bus, int target_id, char r, char c, unsigned int elem, int regset, string *errorstring)
1292 {
1293  // check if driver is running
1294  if (m_idriver == NULL)
1295  {
1296  if(_verbose) yError ("Driver not ready\n");
1297  return -1;
1298  }
1299 
1300  //set matrix
1301  txBuffer[0].setId((2 << 8) + target_id);
1302  txBuffer[0].setLen(5);
1303  txBuffer[0].getData()[0]= 0x03;
1304  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (r & 0x0f);
1305  txBuffer[0].getData()[2]= c;
1306  txBuffer[0].getData()[3]= elem >> 8;
1307  txBuffer[0].getData()[4]= elem & 0xFF;
1308  set_bus(txBuffer[0], bus);
1309  int ret = m_idriver->send_message(txBuffer, 1);
1310  drv_sleep(5);
1311 
1312  return 0;
1313 }
1314 
1315 //*****************************************************************/
1316 int cDownloader::strain_set_offset(int bus, int target_id, char channel, unsigned int offset, int regset, string *errorstring)
1317 {
1318  // check if driver is running
1319  if (m_idriver == NULL)
1320  {
1321  if(_verbose) yError (" Driver not ready\n");
1322  return -1;
1323  }
1324 
1325  //set dac
1326  txBuffer[0].setId((2 << 8) + target_id);
1327  txBuffer[0].setLen(4);
1328  txBuffer[0].getData()[0]= 0x04;
1329  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);;
1330  txBuffer[0].getData()[2]= offset >> 8;
1331  txBuffer[0].getData()[3]= offset & 0xFF;
1332  set_bus(txBuffer[0], bus);
1333  int ret = m_idriver->send_message(txBuffer, 1);
1334  drv_sleep(5);
1335 /*
1336  int read_messages = m_idriver->receive_message(rxBuffer);
1337  for (int i=0; i<read_messages; i++)
1338  {
1339  if (rxBuffer[i].getData()[0]==0x0B)
1340  {
1341  offset = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
1342  return 0;
1343  }
1344  }
1345  return -1;
1346 */
1347  return 0;
1348 }
1349 
1350 
1351 int cDownloader::strain_acquire_start(int bus, int target_id, uint8_t txratemilli, bool calibmode, strain_acquisition_mode_t acqmode, string *errorstring)
1352 {
1353  // check if driver is running
1354  if (m_idriver == NULL)
1355  {
1356  if(_verbose) yError ("Driver not ready\n");
1357  return -1;
1358  }
1359 
1360  int ret = 0;
1361 
1362 
1363  if (strain_acquisition_mode_polling == acqmode)
1364  {
1365  return 0;
1366  }
1367 
1368  //yDebug() << "cDownloader::strain_acquire_start() from" << bus << target_id;
1369 
1370  // Send transmission rate to strain board
1371  txBuffer[0].setId((2 << 8) + target_id);
1372  txBuffer[0].setLen(2);
1373  txBuffer[0].getData()[0]= 0x08;
1374  txBuffer[0].getData()[1]= txratemilli;
1375  set_bus(txBuffer[0], bus);
1376  ret = m_idriver->send_message(txBuffer, 1);
1377  // check if send_message was successful
1378  if (ret==0)
1379  {
1380  if(_verbose) yError ("Unable to send txrate set message\n");
1381  return -1;
1382  }
1383 
1384  // Send transmission command to strain board
1385  txBuffer[0].setId((2 << 8) + target_id);
1386  txBuffer[0].setLen(2);
1387  txBuffer[0].getData()[0]= 0x07;
1388  txBuffer[0].getData()[1]= (false == calibmode) ? (0x03) : (0x00);
1389  set_bus(txBuffer[0], bus);
1390  ret = m_idriver->send_message(txBuffer, 1);
1391  // check if send_message was successful
1392  if (ret==0)
1393  {
1394  if(_verbose) yError ("Unable to send start tx message\n");
1395  return -1;
1396  }
1397 
1398  strain_is_acquiring_in_calibratedmode = calibmode;
1399 
1400  return 0;
1401 
1402 }
1403 
1404 int cDownloader::strain_acquire_stop(int bus, int target_id, strain_acquisition_mode_t acqmode, string *errorstring)
1405 {
1406  // check if driver is running
1407  if (m_idriver == NULL)
1408  {
1409  if(_verbose) yError ("Driver not ready\n");
1410  return -1;
1411  }
1412 
1413  if (strain_acquisition_mode_polling == acqmode)
1414  {
1415  return 0;
1416  }
1417 
1418  // Send transmission command to strain board
1419  txBuffer[0].setId((2 << 8) + target_id);
1420  txBuffer[0].setLen(2);
1421  txBuffer[0].getData()[0]= 0x07;
1422  txBuffer[0].getData()[1]= 0x02;
1423  set_bus(txBuffer[0], bus);
1424  int ret = m_idriver->send_message(txBuffer, 1);
1425  // check if send_message was successful
1426  if (ret==0)
1427  {
1428  if(_verbose) yError ("Unable to send stop tx message\n");
1429  return -1;
1430  }
1431 
1432 
1433  // we should empty the rx buffer for a while...
1434 
1435  const double TOUT = 0.100;
1436  const size_t maxframes = rxBuffer.size() - 1;
1437  for(size_t n=0; n<10; n++)
1438  {
1439  int read_messages = m_idriver->receive_message(rxBuffer, maxframes, TOUT);
1440  //yDebug() << "cDownloader::strain_acquire_stop() has removed" << read_messages << "can frames from rxbuffer";
1441  if(0 == read_messages)
1442  {
1443  break;
1444  }
1445  }
1446 
1447  return 0;
1448 }
1449 
1450 int cDownloader::strain_acquire_get(int bus, int target_id, vector<strain_value_t> &values, const unsigned int howmany, void (*updateProgressBar)(void*, float), void *arg, strain_acquisition_mode_t acqmode, const unsigned int maxerrors, string *errorstring)
1451 {
1452  // i must read howmany pairs of can frames of type 0xA and 0xB. to simplify i assume that they will arrive in pairs.
1453 
1454 
1455  values.clear();
1456 
1457  if(0 == howmany)
1458  {
1459  return -1;
1460  }
1461 
1462  unsigned int errorcount = 0;
1463 
1464 
1465  if (strain_acquisition_mode_polling == acqmode)
1466  {
1467  if(_verbose)
1468  {
1469  yDebug() << "cDownloader::strain_acquire_get() is using polling";
1470  }
1471 
1472  for (unsigned int s = 0; s < howmany; s++)
1473  {
1474  cDownloader::strain_value_t sv;
1475  sv.valid = true;
1476 
1477  for (int c = 0; c < 6; c++)
1478  {
1479  //drv_sleep(3);
1480  unsigned int adc = 0;
1481  int rr = strain_get_adc(bus, target_id, c, adc, 0);
1482  if (-1 == rr)
1483  {
1484  errorcount++;
1485  yDebug() << "error in acquisition of an adc channel " << c << "incrementing error counter to" << errorcount;
1486  if (errorcount >= maxerrors)
1487  {
1488  yError() << "reached" << maxerrors << "reception errors in adc acquisition: must quit";
1489  return -1;
1490  }
1491  }
1492  sv.channel[c] = adc;
1493  }
1494  values.push_back(sv);
1495 
1496  if (NULL != updateProgressBar)
1497  {
1498  float perc = (0 != howmany) ? (static_cast<float>(s + 1) / static_cast<float>(howmany)) : (100.0);
1499  updateProgressBar(arg, perc);
1500  }
1501  }
1502 
1503  return 0;
1504  }
1505 
1506  const double TOUT = 3.0;
1507 
1508  // purge from possible acks of strain1...
1509 
1510  m_idriver->receive_message(rxBuffer, 2, TOUT);
1511  m_idriver->receive_message(rxBuffer, 2, TOUT);
1512 
1513 
1514  for(unsigned int s=0; s<howmany; s++)
1515  {
1516 
1517  if(NULL != updateProgressBar)
1518  {
1519  float perc = (0 != howmany) ? (static_cast<float>(s+1)/static_cast<float>(howmany)) : (100.0);
1520  updateProgressBar(arg, perc);
1521  }
1522 
1523  int read_messages = m_idriver->receive_message(rxBuffer, 2, TOUT);
1524  strain_value_t sv;
1525 
1526  sv.channel[0] = 0x66666;
1527  sv.channel[4] = 0x66666;
1528 
1529  if(2 == read_messages)
1530  {
1531  for(unsigned int i=0; i<2; i++)
1532  {
1533  uint32_t id = rxBuffer[i].getId();
1534  uint8_t type = id & 0xf;
1535 
1536  if((0xA != type) && (0xB != type))
1537  {
1538  yError() << "cDownloader::strain_acquire_get() has detected strange can frames of type = " << type << ".... operation aborted";
1539  char rxframe[128] = {0};
1540  snprintf(rxframe, sizeof(rxframe), "l = %d, id = 0x%x, d = 0x[%x %x %x %x %x %x %x %x]", rxBuffer[i].getLen(), rxBuffer[i].getId(),
1541  rxBuffer[i].getData()[0], rxBuffer[i].getData()[1], rxBuffer[i].getData()[2], rxBuffer[i].getData()[3],
1542  rxBuffer[i].getData()[4], rxBuffer[i].getData()[5], rxBuffer[i].getData()[6], rxBuffer[i].getData()[7]);
1543 
1544  yError() << "frame -> " << rxframe;
1545  continue;
1546  //return -1;
1547  }
1548 
1549  // values in little endian
1550  uint16_t x = static_cast<uint16_t>(rxBuffer[i].getData()[0]) | (static_cast<uint16_t>(rxBuffer[i].getData()[1]) << 8);
1551  uint16_t y = static_cast<uint16_t>(rxBuffer[i].getData()[2]) | (static_cast<uint16_t>(rxBuffer[i].getData()[3]) << 8);
1552  uint16_t z = static_cast<uint16_t>(rxBuffer[i].getData()[4]) | (static_cast<uint16_t>(rxBuffer[i].getData()[5]) << 8);
1553 
1554  sv.saturated = (6 == rxBuffer[i].getLen()) ? (true) : (false);
1555  sv.valid = true;
1556 
1557  sv.calibrated = strain_is_acquiring_in_calibratedmode;
1558 
1559  if(0xA == type)
1560  {
1561  //yDebug() << "RX 0xA";
1562 
1563  sv.channel[0] = x;
1564  sv.channel[1] = y;
1565  sv.channel[2] = z;
1566  if(false == sv.saturated)
1567  {
1568  void *tmp = &rxBuffer[i].getData()[6];
1569  icubCanProto_strain_forceSaturationInfo_t *sinfo = reinterpret_cast<icubCanProto_strain_forceSaturationInfo_t*>(tmp);
1570  if(1 == sinfo->thereIsSaturationInAtLeastOneChannel)
1571  {
1572  sv.saturationinfo[0] = static_cast<icubCanProto_strain_saturationInfo_t>(sinfo->saturationInChannel_0);
1573  sv.saturationinfo[1] = static_cast<icubCanProto_strain_saturationInfo_t>(sinfo->saturationInChannel_1);
1574  sv.saturationinfo[2] = static_cast<icubCanProto_strain_saturationInfo_t>(sinfo->saturationInChannel_2);
1575  }
1576  }
1577  }
1578  else if(0xB == type)
1579  {
1580  //yDebug() << "RX 0xB";
1581 
1582  sv.channel[3] = x;
1583  sv.channel[4] = y;
1584  sv.channel[5] = z;
1585  if(false == sv.saturated)
1586  {
1587  void *tmp = &rxBuffer[i].getData()[6];
1588  icubCanProto_strain_torqueSaturationInfo_t *sinfo = reinterpret_cast<icubCanProto_strain_torqueSaturationInfo_t*>(tmp);
1589  if(1 == sinfo->thereIsSaturationInAtLeastOneChannel)
1590  {
1591  sv.saturationinfo[3] = static_cast<icubCanProto_strain_saturationInfo_t>(sinfo->saturationInChannel_3);
1592  sv.saturationinfo[4] = static_cast<icubCanProto_strain_saturationInfo_t>(sinfo->saturationInChannel_4);
1593  sv.saturationinfo[5] = static_cast<icubCanProto_strain_saturationInfo_t>(sinfo->saturationInChannel_5);
1594  }
1595  }
1596  }
1597  else
1598  {
1599  //yDebug() << "RX ??";
1600  }
1601 
1602  }
1603 
1604  if((0x66666 == sv.channel[0]) || (0x66666 == sv.channel[4]))
1605  {
1606  yDebug() << "cDownloader::strain_acquire_get(): did not receive two consecutive 0xA and 0xB. please check!";
1607  }
1608  else
1609  {
1610  values.push_back(sv);
1611  }
1612  }
1613  else
1614  {
1615  errorcount ++;
1616  yDebug() << "in streaming mode did not received two messages but " << read_messages << "incrementing error counter to" << errorcount;
1617  if(errorcount >= maxerrors)
1618  {
1619  yError() << "reached" << maxerrors << "reception errors: must quit";
1620  return -1;
1621  }
1622  }
1623 
1624  }
1625 
1626 
1627  return 0;
1628 }
1629 
1630 
1631 
1632 //*****************************************************************/
1633 int cDownloader::strain_start_sampling (int bus, int target_id, string *errorstring)
1634 {
1635  // check if driver is running
1636  if (m_idriver == NULL)
1637  {
1638  if(_verbose) yError ("Driver not ready\n");
1639  return -1;
1640  }
1641 
1642  // Send transmission command to strain board
1643  txBuffer[0].setId((2 << 8) + target_id);
1644  txBuffer[0].setLen(2);
1645  txBuffer[0].getData()[0]= 0x07;
1646  txBuffer[0].getData()[1]= 0x01;
1647  set_bus(txBuffer[0], bus);
1648  int ret = m_idriver->send_message(txBuffer, 1);
1649  // check if send_message was successful
1650  if (ret==0)
1651  {
1652  if(_verbose) yError ("Unable to send message\n");
1653  return -1;
1654  }
1655  return 0;
1656 }
1657 
1658 //*****************************************************************/
1659 int cDownloader::strain_stop_sampling (int bus, int target_id, string *errorstring)
1660 {
1661  // check if driver is running
1662  if (m_idriver == NULL)
1663  {
1664  if(_verbose) yError ("Driver not ready\n");
1665  return -1;
1666  }
1667 
1668  // Send transmission command to strain board
1669  txBuffer[0].setId((2 << 8) + target_id);
1670  txBuffer[0].setLen(2);
1671  txBuffer[0].getData()[0]= 0x07;
1672  txBuffer[0].getData()[1]= 0x02;
1673  set_bus(txBuffer[0], bus);
1674  int ret = m_idriver->send_message(txBuffer, 1);
1675  // check if send_message was successful
1676  if (ret==0)
1677  {
1678  if(_verbose) yError ("Unable to send message\n");
1679  return -1;
1680  }
1681  return 0;
1682 }
1683 
1684 // usage:
1685 // strain: get the single value guiTarget as a [-32k, +32k) from the widget. put it inside a vector.
1686 // use an empty gains vector
1687 // std::vector<int16_t> targets;
1688 // targets.push_back(guiTarget);
1689 // std::vector<strain2_ampl_discretegain_t> gains(0);
1690 // strain_calibrate_offset2(bus, id, icubCanProto_boardType__strain, gains, targets);
1691 // strain2: get the guiTargets as [-32k, +32k) values from the widget. put them all inside a vector which must become of size 6.
1692 // std::vector<int16_t> targets;
1693 // targets.push_back(guiTargets[0]); targets.push_back(guiTargets[1]); etc....
1694 // get the 6 gains and put them into a vector
1695 // std::vector<strain2_ampl_discretegain_t> gains;
1696 // gains.push_back(g0); etc....
1697 // strain_calibrate_offset2(bus, id, icubCanProto_boardType__strain, gains, targets);
1698 
1699 int cDownloader::strain_calibrate_offset2 (int bus, int target_id, icubCanProto_boardType_t boardtype, const std::vector<strain2_ampl_discretegain_t> &gains, const std::vector<int16_t> &targets, string *errorstring)
1700 {
1701 
1702  if(icubCanProto_boardType__strain == boardtype)
1703  {
1704  int16_t i16 = (targets.empty()) ? (0) : (targets[0]);
1705 
1706  return strain_calibrate_offset2_strain1(bus, target_id, i16, errorstring);
1707  }
1708  else if(icubCanProto_boardType__strain2 == boardtype || icubCanProto_boardType__strain2c == boardtype)
1709  {
1710  return strain_calibrate_offset2_strain2(bus, target_id, gains, targets, errorstring);
1711  }
1712 
1713  return -1;
1714 }
1715 
1716 
1717 //*****************************************************************/
1718 int cDownloader::strain_calibrate_offset (int bus, int target_id, icubCanProto_boardType_t boardtype, unsigned int middle_val, string *errorstring)
1719 {
1720  // the calibration of the offset is meaningful only for the calibration set in use.
1721  const int regset = strain_regset_inuse;
1722 
1723  // check if driver is running
1724  if (m_idriver == NULL)
1725  {
1726  if(_verbose) yError ("Driver not ready\n");
1727  return -1;
1728  }
1729 
1730  int daclimit = 0x3ff;
1731  int dacstep = 1;
1732  long tolerance = 256;
1733 
1734 #define STRAIN2_USE_NEW_MODE
1735 
1736 #if !defined(STRAIN2_USE_NEW_MODE)
1737  if(icubCanProto_boardType__strain2 == boardtype || icubCanProto_boardType__strain2c == boardtype)
1738  {
1739  daclimit = 0xffff;
1740  dacstep = 16;
1741  tolerance = 64;
1742  }
1743 #else
1744  if(icubCanProto_boardType__strain2 == boardtype || icubCanProto_boardType__strain2c == boardtype)
1745  {
1746  yDebug() << "strain2-amplifier-tuning: see the various STEP-x";
1747 
1748  const uint8_t everychannel = 0x0f;
1749  tolerance = 256;
1750  uint8_t samples2average = 8; // if zero, the board uses its default (= 4)
1751 
1752 #define TESTMODE_STRAIN2
1753 #undef TESTMODE_STRAIN2_SAMEGAIN
1754 
1755 //#define DEBUGLEVEL_MAXIMUM
1756 
1757 #if defined(TESTMODE_STRAIN2)
1758 
1759  const unsigned int NUMofCHANNELS = 6;
1760 
1761 
1762 #if defined(TESTMODE_STRAIN2_SAMEGAIN)
1763 
1764  const int i2u_all = index24;
1765 
1766  yDebug() << "imposing gain =" << gainvalues[i2u_all] << "in every channel";
1767 
1768  txBuffer[0].setId((2 << 8) + target_id);
1769  txBuffer[0].setLen(8);
1770  txBuffer[0].getData()[0]= 0x2B;
1771  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (everychannel & 0x0f);
1772  txBuffer[0].getData()[2]= cfg1map[i2u_all][0]; // lsb of gd
1773  txBuffer[0].getData()[3]= cfg1map[i2u_all][1]; // msb of gd
1774  txBuffer[0].getData()[4]= cfg1map[i2u_all][2];
1775  txBuffer[0].getData()[5]= cfg1map[i2u_all][3]; // vc0
1776  txBuffer[0].getData()[6]= cfg1map[i2u_all][4];
1777  txBuffer[0].getData()[7]= cfg1map[i2u_all][5];
1778  set_bus(txBuffer[0], bus);
1779  yDebug("sending: [%x, %x, %x, %x, %x, %x, %x, %x]", txBuffer[0].getData()[0], txBuffer[0].getData()[1], txBuffer[0].getData()[2], txBuffer[0].getData()[3], txBuffer[0].getData()[4], txBuffer[0].getData()[5], txBuffer[0].getData()[6], txBuffer[0].getData()[7]);
1780 
1781  m_idriver->send_message(txBuffer, 1);
1782 
1783  // i wait some 100 ms
1784  yarp::os::Time::delay(1.0);
1785 
1786 #else
1787 
1788 
1789 #if defined(DEBUGLEVEL_MAXIMUM)
1790 
1791  yDebug() << "reading (gain, offset) of front end amplifiers";
1792 
1793  for(int c=0; c<NUMofCHANNELS; c++)
1794  {
1795  float gaain = 0;
1796  uint16_t ooffset = 0;
1797  strain_get_amplifier_gain_offset(bus, target_id, c, gaain, ooffset, regset, errorstring);
1798  yDebug("channel %d: gain = %f, offset = %d", c, gaain, ooffset);
1799  //strain_set_amplifier_gain_offset(bus, target_id, c, gaain+1.0f, ooffset+128, regset, errorstring);
1800  //yarp::os::Time::delay(1.0);
1801  //gaain = 0; ooffset = 0;
1802  //strain_get_amplifier_gain_offset(bus, target_id, c, gaain, ooffset, regset, errorstring);
1803  //yDebug("channel %d: new gain = %f, new offset = %d", c, gaain, ooffset);
1804  }
1805 
1806 
1807  yDebug() << "i get the amplifier reg config";
1808 
1809  txBuffer[0].setId((2 << 8) + target_id);
1810  txBuffer[0].setLen(2);
1811  txBuffer[0].getData()[0]= 0x2A;
1812  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (everychannel & 0x0f);
1813  set_bus(txBuffer[0], bus);
1814  m_idriver->send_message(txBuffer, 1);
1815 
1816  yDebug() << "i print the amplifier reg config";
1817 
1818  for(int nr=0; nr<NUMofCHANNELS; nr++)
1819  {
1820  int rm = m_idriver->receive_message(rxBuffer, 1, 1.0);
1821  for(int i=0; i<rm; i++)
1822  {
1823  if (rxBuffer[i].getData()[0]==0x2A)
1824  {
1825 
1826  uint8_t from = rxBuffer[i].getData()[1];
1827  yDebug("from %d: [%x, %x, %x, %x, %x, %x]", from, rxBuffer[i].getData()[2], rxBuffer[i].getData()[3], rxBuffer[i].getData()[4], rxBuffer[i].getData()[5], rxBuffer[i].getData()[6], rxBuffer[i].getData()[7]);
1828  }
1829  break;
1830  }
1831  }
1832 
1833  yarp::os::Time::delay(2.0);
1834 
1835 #endif // #if defined(DEBUGLEVEL_MAXIMUM)
1836 
1837 
1838  // step1: i set the amplifiers
1839 
1840 
1841  // the chosen gains:
1842  const strain2_ampl_discretegain_t ampsets[NUMofCHANNELS] =
1843  {
1844  ampl_gain08, ampl_gain24, ampl_gain24,
1845  ampl_gain10, ampl_gain10, ampl_gain24
1846  };
1847 
1848  yDebug() << "strain2-amplifier-tuning: STEP-1. imposing gains which are different of each channel";
1849 
1850  for(int channel=0; channel<NUMofCHANNELS; channel++)
1851  {
1852  yDebug() << "strain2-amplifier-tuning: STEP-1. on channel" << channel << "we impose gain =" << strain_amplifier_discretegain2float(ampsets[channel]);
1853 
1854  strain_set_amplifier_discretegain(bus, target_id, channel, ampsets[channel], regset, errorstring);
1855 
1856  // i wait some time
1857  yarp::os::Time::delay(1.0);
1858  }
1859 
1860 
1861 #endif // #else of #if defined(TESTMODE_STRAIN2_SAMEGAIN)
1862 
1863 
1864  // step2: i read back gains just to be sure ..
1865 
1866  yDebug() << "strain2-amplifier-tuning: STEP-2. reading (gain, offset) of front end amplifiers";
1867 
1868  for(int c=0; c<NUMofCHANNELS; c++)
1869  {
1870  float gaain = 0;
1871  uint16_t ooffset = 0;
1872  strain_get_amplifier_gain_offset(bus, target_id, c, gaain, ooffset, regset, errorstring);
1873  yDebug("strain2-amplifier-tuning: STEP-2. channel %d: gain = %f, offset = %d", c, gaain, ooffset);
1874  }
1875 
1876  yarp::os::Time::delay(2.0);
1877 
1878 
1879 #if defined(DEBUGLEVEL_MAXIMUM)
1880 
1881  yDebug() << "i get the amplifier reg config";
1882 
1883  txBuffer[0].setId((2 << 8) + target_id);
1884  txBuffer[0].setLen(2);
1885  txBuffer[0].getData()[0]= 0x2A;
1886  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (everychannel & 0x0f);
1887  set_bus(txBuffer[0], bus);
1888  m_idriver->send_message(txBuffer, 1);
1889 
1890  yDebug() << "i print the amplifier reg config";
1891 
1892  for(int nr=0; nr<NUMofCHANNELS; nr++)
1893  {
1894  int rm = m_idriver->receive_message(rxBuffer, 1, 1.0);
1895  for(int i=0; i<rm; i++)
1896  {
1897  if (rxBuffer[i].getData()[0]==0x2A)
1898  {
1899 
1900  uint8_t from = rxBuffer[i].getData()[1];
1901  yDebug("from %d: [%x, %x, %x, %x, %x, %x]", from, rxBuffer[i].getData()[2], rxBuffer[i].getData()[3], rxBuffer[i].getData()[4], rxBuffer[i].getData()[5], rxBuffer[i].getData()[6], rxBuffer[i].getData()[7]);
1902  }
1903  break;
1904  }
1905  }
1906 
1907  yarp::os::Time::delay(2.0);
1908 
1909 #endif // #if defined(DEBUGLEVEL_MAXIMUM)
1910 
1911 
1912 
1913 #endif
1914 
1915 
1916  // step3: autocalib
1917 
1918  yDebug() << "strain2-amplifier-tuning: STEP-3. regularisation of ADC to " << middle_val;
1919  yDebug() << "strain2-amplifier-tuning: STEP-3. other params: mae tolerence is" << tolerance << "and samples2average =" << samples2average;
1920 
1921 
1922  // sending an autocalib message
1923  txBuffer[0].setId((2 << 8) + target_id);
1924  txBuffer[0].setLen(8);
1925  txBuffer[0].getData()[0]= 0x22;
1926  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (everychannel & 0x0f);
1927  txBuffer[0].getData()[2]= 0; // mode oneshot, the only possible so far
1928  txBuffer[0].getData()[3]= middle_val & 0x00ff; // little endian
1929  txBuffer[0].getData()[4]= (middle_val >> 8) & 0x00ff; // little endian
1930  txBuffer[0].getData()[5]= tolerance & 0x00ff; // little endian
1931  txBuffer[0].getData()[6]= (tolerance >> 8) & 0x00ff; // little endian
1932  txBuffer[0].getData()[7]= samples2average;
1933  set_bus(txBuffer[0], bus);
1934  yDebug("strain2-amplifier-tuning: STEP-3. sent message = [%x, %x, %x, %x, %x, %x, %x, %x]", txBuffer[0].getData()[0], txBuffer[0].getData()[1], txBuffer[0].getData()[2], txBuffer[0].getData()[3], txBuffer[0].getData()[4], txBuffer[0].getData()[5], txBuffer[0].getData()[6], txBuffer[0].getData()[7]);
1935  int ret = m_idriver->send_message(txBuffer, 1);
1936  // check if send_message was successful
1937  if (ret==0)
1938  {
1939  if(_verbose) yError ("Unable to send message\n");
1940  return -1;
1941  }
1942 
1943  // now wait for a reply for most 3 seconds
1944  double TOUT = 3.0;
1945 
1946  yDebug() << "strain2-amplifier-tuning: STEP-3. results ...";
1947 
1948  int read_messages = m_idriver->receive_message(rxBuffer, 1, TOUT);
1949  for(int i=0; i<read_messages; i++)
1950  {
1951  if (rxBuffer[i].getData()[0]==0x22)
1952  {
1953  yDebug("strain2-amplifier-tuning: STEP-3. received message = [%x, %x, %x, %x, %x, %x, %x, %x]", rxBuffer[0].getData()[0], rxBuffer[0].getData()[1], rxBuffer[0].getData()[2], rxBuffer[0].getData()[3], rxBuffer[0].getData()[4], rxBuffer[0].getData()[5], rxBuffer[0].getData()[6], rxBuffer[0].getData()[7]);
1954 
1955  //dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
1956  uint8_t noisychannelmask = rxBuffer[i].getData()[2];
1957  uint8_t algorithmOKmask = rxBuffer[i].getData()[3];
1958  uint8_t finalmeasureOKmask = rxBuffer[i].getData()[4];
1959  uint16_t mae = (static_cast<uint32_t>(rxBuffer[i].getData()[6])) |
1960  (static_cast<uint32_t>(rxBuffer[i].getData()[7]) << 8);
1961 
1962  if((0x3f == algorithmOKmask) && (0x3f == finalmeasureOKmask))
1963  {
1964  yDebug() << "strain2-amplifier-tuning: STEP-3. OK. regularisation to value" << middle_val << "is done and MAE = " << mae;
1965  if(0 != noisychannelmask)
1966  {
1967  yDebug() << "however we found some noisy channels";
1968  yDebug("noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
1969 
1970  }
1971 
1972  }
1973  else
1974  {
1975  if(0x3f != algorithmOKmask)
1976  {
1977  yDebug() << "strain2-amplifier-tuning: STEP-3. KO. regularisation to value" << middle_val << "has sadly failed because algorithm found required values out of range of registers CFG0.OS or ZDAC.";
1978  }
1979  else
1980  {
1981  yDebug() << "strain2-amplifier-tuning: STEP-3. KO. regularisation to value" << middle_val << "has failed because MAE error is high on some channels.";
1982  }
1983 
1984  yDebug("noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
1985  for(uint8_t i=0; i<NUMofCHANNELS; i++)
1986  {
1987  if((algorithmOKmask & (0x01<<i)) == 0)
1988  {
1989  yDebug() << "calibration fails in channel" << i;
1990  }
1991  if((finalmeasureOKmask & (0x01<<i)) == 0)
1992  {
1993  yDebug() << "mae is high in channel" << i;
1994  }
1995  }
1996  }
1997  break;
1998  }
1999  }
2000 
2001 
2002  return 0;
2003  }
2004 #endif
2005 
2006 
2007  int channel=0;
2008  int i=0;
2009  int ret =0;
2010  long error = 0;
2011  unsigned int measure = 0;
2012  unsigned int dac = 0;
2013  int cycle =0;
2014  int read_messages;
2015 
2016  for (channel=0; channel<6; channel++)
2017  {
2018  // yDebug() << "starting OFFSET of channel" << channel;
2019  // Send read channel command to strain board
2020  txBuffer[0].setId((2 << 8) + target_id);
2021  txBuffer[0].setLen(3);
2022  txBuffer[0].getData()[0]= 0x0C;
2023  txBuffer[0].getData()[1]= channel;
2024  txBuffer[0].getData()[2]= 0;
2025  set_bus(txBuffer[0], bus);
2026  ret = m_idriver->send_message(txBuffer, 1);
2027  // check if send_message was successful
2028  if (ret==0)
2029  {
2030  if(_verbose) yError ("Unable to send message\n");
2031  return -1;
2032  }
2033  //read adc
2034  read_messages = m_idriver->receive_message(rxBuffer,1);
2035  for (i=0; i<read_messages; i++)
2036  {
2037  if (rxBuffer[i].getData()[0]==0x0C)
2038  {
2039  measure = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
2040  break;
2041  }
2042  }
2043 
2044  //read dac
2045  txBuffer[0].setId((2 << 8) + target_id);
2046  txBuffer[0].setLen(2);
2047  txBuffer[0].getData()[0]= 0x0B;
2048  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
2049  set_bus(txBuffer[0], bus);
2050  ret = m_idriver->send_message(txBuffer, 1);
2051 
2052  read_messages = m_idriver->receive_message(rxBuffer,1);
2053  for (i=0; i<read_messages; i++)
2054  {
2055  if (rxBuffer[i].getData()[0]==0x0B)
2056  {
2057  dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
2058  break;
2059  }
2060  }
2061 
2062  error = long(measure) - long(middle_val);
2063  cycle=0;
2064 
2065  while (abs(error)>tolerance && cycle<daclimit)
2066  {
2067  if (error>0) dac -= dacstep;
2068  else dac += dacstep;
2069 
2070  if (dac>daclimit) dac = daclimit;
2071  if (dac<0) dac = 0;
2072 
2073  //yDebug() << "iter" << cycle << "err = " << error << "next dac =" << dac;
2074 
2075  // Send transmission command to strain board
2076  txBuffer[0].setId((2 << 8) + target_id);
2077  txBuffer[0].setLen(4);
2078  txBuffer[0].getData()[0]= 0x04;
2079  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
2080  txBuffer[0].getData()[2]= dac >> 8;
2081  txBuffer[0].getData()[3]= dac & 0xFF;
2082  set_bus(txBuffer[0], bus);
2083  int ret = m_idriver->send_message(txBuffer, 1);
2084 
2085  //wait
2086  drv_sleep(3);
2087 
2088  // Send read channel command to strain board
2089  txBuffer[0].setId((2 << 8) + target_id);
2090  txBuffer[0].setLen(3);
2091  txBuffer[0].getData()[0]= 0x0C;
2092  txBuffer[0].getData()[1]= channel;
2093  txBuffer[0].getData()[2]= 0;
2094  set_bus(txBuffer[0], bus);
2095  ret = m_idriver->send_message(txBuffer, 1);
2096  // check if send_message was successful
2097  if (ret==0)
2098  {
2099  if(_verbose) yError ("Unable to send message\n");
2100  return -1;
2101  }
2102  //read adc
2103  read_messages = m_idriver->receive_message(rxBuffer, 1);
2104  for (i=0; i<read_messages; i++)
2105  {
2106  if (rxBuffer[i].getData()[0]==0x0C)
2107  {
2108  measure = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
2109  break;
2110  }
2111  }
2112 
2113  error = long(measure) - long(middle_val);
2114  cycle++;
2115  }
2116 
2117  }
2118 
2119  return 0;
2120 }
2121 
2122 //*****************************************************************/
2123 int cDownloader::get_serial_no (int bus, int target_id, char* serial_no)
2124 {
2125  int ret = -1;
2126  int i;
2127  if (serial_no == NULL) return -1;
2128 
2129  memset (serial_no,0,8);
2130 
2131  // check if driver is running
2132  if (m_idriver == NULL)
2133  {
2134  if(_verbose) yError ("Driver not ready\n");
2135  return -1;
2136  }
2137 
2138  for (i=0; i<board_list_size; i++)
2139  {
2140  if ((board_list[i].pid==target_id) &&
2141  (board_list[i].type==icubCanProto_boardType__strain) || (board_list[i].type==icubCanProto_boardType__strain2))
2142  {
2143  this->strain_get_serial_number(bus, target_id, serial_no);
2144  ret = 0;
2145  }
2146  }
2147 
2148  return ret;
2149 }
2150 
2151 
2152 #define EOCANPROT_D_CREATE_CANID(clss, orig, dest) ( (((clss)&0xF) << 8) | (((orig)&0xF) << 4) | ((dest)&0xF) )
2153 
2154 //#warning add controls vs sending it in broadcast or to all buses
2155 int cDownloader::get_firmware_version(int bus, int target_id, eObrd_cantype_t boardtype, eObrd_info_t *info, bool &noreply)
2156 {
2157  noreply = true;
2158 
2159  if(NULL == info)
2160  {
2161  return -1;
2162  }
2163  // check if driver is running
2164  if(NULL == m_idriver)
2165  {
2166  if(_verbose) yError ("cDownloader::get_firmware_version(): driver not ready\n");
2167  return -1;
2168  }
2169 
2170  int read_messages = 0;
2171 
2172  // reset the answer
2173  info->type = boardtype;
2174  info->firmware.major = info->firmware.minor = info->firmware.build = 0;
2175  info->protocol.major = info->protocol.minor = 0;
2176 
2177  txBuffer[0].setLen(3);
2178  txBuffer[0].getData()[0] = 0; // fill it later on
2179  txBuffer[0].getData()[1] = 0;
2180  txBuffer[0].getData()[2] = 0; // we send a (0, 0) prototocol version.
2181 
2182  //#warning -> check if sending a get-prot-version message with wrong prot version is OK or not (hopefully it will not send boards in hw fault).
2183 
2184  // prepare command. it depends on board type.
2185 
2186  bool boardisMC = false;
2187  switch(boardtype)
2188  {
2189  case eobrd_cantype_dsp:
2190  case eobrd_cantype_mc4:
2191  case eobrd_cantype_2dc:
2192  case eobrd_cantype_bll:
2193  case eobrd_cantype_foc:
2194  {
2195  boardisMC = true;
2196  txBuffer[0].setId(EOCANPROT_D_CREATE_CANID(ICUBCANPROTO_CLASS_POLLING_MOTORCONTROL, 0, target_id));
2197  txBuffer[0].getData()[0] = ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION;
2198  } break;
2199 
2200  case eobrd_cantype_mtb:
2201  case eobrd_cantype_strain:
2202  case eobrd_cantype_mais:
2203  case eobrd_cantype_6sg:
2204  case eobrd_cantype_mtb4:
2205  case eobrd_cantype_strain2:
2206  case eobrd_cantype_rfe:
2207  case eobrd_cantype_sg3:
2208  case eobrd_cantype_psc:
2209  case eobrd_cantype_mtb4w:
2210  case eobrd_cantype_pmc:
2211  case eobrd_cantype_amcbldc:
2212  case eobrd_cantype_mtb4c:
2213  case eobrd_cantype_strain2c:
2214  {
2215  boardisMC = false;
2216  txBuffer[0].setId(EOCANPROT_D_CREATE_CANID(ICUBCANPROTO_CLASS_POLLING_ANALOGSENSOR, 0, target_id));
2217  txBuffer[0].getData()[0] = ICUBCANPROTO_POL_AS_CMD__GET_FW_VERSION;
2218  } break;
2219 
2220  default:
2221  {
2222  if(_verbose) yError ("cDownloader::get_firmware_version(): this board %d is not supported. returning all zeros\n", boardtype);
2223  return -2;
2224  }
2225  }
2226 
2227 
2228  set_bus(txBuffer[0], bus);
2229  int ret = m_idriver->send_message(txBuffer, 1);
2230  if(ret==0)
2231  {
2232  if(_verbose) yError ("Unable to send message\n");
2233  return -1;
2234  }
2235 
2236  read_messages = m_idriver->receive_message(rxBuffer, 1, 1.0);
2237 
2238  if(0 == read_messages)
2239  { // it does not support teh message
2240  return 0;
2241  }
2242 
2243  noreply = false;
2244 
2245  for (int i=0; i<read_messages; i++)
2246  {
2247 
2248 #if 0
2249  if (rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_GET_ADDITIONAL_INFO)
2250  {
2251  fprintf(stderr, "%.4x ", rxBuffer[i].getId());
2252  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[0]);
2253  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[1]);
2254  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[2]);
2255  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[3]);
2256  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[4]);
2257  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[5]);
2258  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[6]);
2259  fprintf(stderr, "%.2x\n", rxBuffer[i].getData()[7]);
2260  }
2261 #endif
2262 
2263  if ((txBuffer[0].getData()[0] == rxBuffer[i].getData()[0]) && (8 == rxBuffer[i].getLen()))
2264  {
2265  info->type = rxBuffer[i].getData()[1];
2266  info->firmware.major = rxBuffer[i].getData()[2];
2267  info->firmware.minor = rxBuffer[i].getData()[3];
2268  info->firmware.build = rxBuffer[i].getData()[4];
2269  info->protocol.major = rxBuffer[i].getData()[5];
2270  info->protocol.minor = rxBuffer[i].getData()[6];
2271  }
2272  else
2273  {
2274  yWarning() << "unknown message";
2275  }
2276  }
2277 
2278  return 0;
2279 }
2280 
2281 //*****************************************************************/
2282 int cDownloader::get_board_info (int bus, int target_id, char* board_info)
2283 {
2284  int i;
2285  if (board_info == NULL) return -1;
2286 
2287  memset (board_info,0x3f,32);
2288 
2289  // check if driver is running
2290  if (m_idriver == NULL)
2291  {
2292  if(_verbose) yError ("Driver not ready\n");
2293  return -1;
2294  }
2295 
2296  //riceve la risposta
2297  int read_messages = 0; //m_idriver->receive_message(rxBuffer, 10, 0);
2298 
2299  // Send command
2300  txBuffer[0].setId(build_id(ID_MASTER, target_id));
2301  txBuffer[0].setLen(1);
2302  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_GET_ADDITIONAL_INFO;
2303  set_bus(txBuffer[0], bus);
2304  int ret = m_idriver->send_message(txBuffer, 1);
2305 // ret=0;
2306  // check if send_message was successful
2307  if (ret==0)
2308  {
2309  if(_verbose) yError ("Unable to send message\n");
2310  return -1;
2311  }
2312 
2313  //pause
2314  //drv_sleep(10);
2315 
2316  //riceve la risposta
2317  read_messages = m_idriver->receive_message(rxBuffer, 64, 1.0);
2318 
2319  //One (or more) answers received
2320  int endString=0;
2321  int j=0;
2322 
2323  //reset the addtional info string
2324  for (j=0; j<31; j++) board_info[j]=0;
2325 
2326  //fills the additional info string
2327  for (i=0; i<read_messages; i++)
2328  {
2329 
2330 #if 0
2331  if (rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_GET_ADDITIONAL_INFO)
2332  {
2333  fprintf(stderr, "%.4x ", rxBuffer[i].getId());
2334  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[0]);
2335  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[1]);
2336  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[2]);
2337  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[3]);
2338  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[4]);
2339  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[5]);
2340  fprintf(stderr, "%.2x ", rxBuffer[i].getData()[6]);
2341  fprintf(stderr, "%.2x\n", rxBuffer[i].getData()[7]);
2342  }
2343 #endif
2344 
2345  if (rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_GET_ADDITIONAL_INFO && rxBuffer[i].getLen()==6)
2346  {
2347  int part = rxBuffer[i].getData()[1];
2348  for (j = 0; j< 4; j++)
2349  {
2350  int tmp=part*4+j;
2351  board_info[tmp]=rxBuffer[i].getData()[j+2];
2352  if (tmp>endString)
2353  {
2354  endString=tmp;
2355  }
2356  }
2357  }
2358  }
2359 
2360  return 0;
2361 }
2362 
2363 
2364 //*****************************************************************/
2365 int cDownloader::change_board_info(int bus, int target_id, char* board_info)
2366 {
2367  // check if driver is running
2368  if (m_idriver == NULL)
2369  {
2370  if(_verbose) yError ("Driver not ready\n");
2371  return -1;
2372  }
2373 
2374  // Send command
2375  int counter =0;
2376  int ret =0;
2377  int j = 0;
2378 
2379  for (counter = 0 ; counter < 8; counter++)
2380  {
2381  //do {}
2382  //while (CAN1_getStateTX () == 0) ;
2383  {
2384  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_SET_ADDITIONAL_INFO;
2385  txBuffer[0].getData()[1]= counter;
2386  txBuffer[0].setId(build_id(ID_MASTER, target_id));
2387  txBuffer[0].setLen(6);
2388  for (j=0; j<4; j++)
2389  {
2390  txBuffer[0].getData()[2+j] = board_info[j+counter*4];
2391  }
2392  set_bus(txBuffer[0], bus);
2393  ret |= m_idriver->send_message(txBuffer, 1);
2394  }
2395  }
2396 
2397  // check if send_message was successful
2398  if (ret==0)
2399  {
2400  if(_verbose) yError ("Unable to send message\n");
2401  return -1;
2402  }
2403 
2404  //pause
2405  drv_sleep(500);
2406 
2407  // update the board list
2408  initschede();
2409 
2410  return 0;
2411 }
2412 
2413 //*****************************************************************/
2414 int cDownloader::change_card_address(int bus, int target_id, int new_id, int board_type)
2415 {
2416  int i = 0;
2417 
2418  // check if driver is running
2419  if (m_idriver == NULL)
2420  {
2421  if(_verbose) yError ("Driver not ready\n");
2422  return -1;
2423  }
2424 
2425  switch (board_type)
2426  {
2427  case icubCanProto_boardType__strain:
2428  case icubCanProto_boardType__skin:
2429  case icubCanProto_boardType__mais:
2430  case icubCanProto_boardType__6sg:
2431  case icubCanProto_boardType__mtb4:
2432  case icubCanProto_boardType__strain2:
2433  case icubCanProto_boardType__rfe:
2434  case icubCanProto_boardType__sg3:
2435  case icubCanProto_boardType__psc:
2436  case icubCanProto_boardType__mtb4w:
2437  case icubCanProto_boardType__mtb4c:
2438  case eobrd_cantype_pmc:
2439  case eobrd_cantype_amcbldc:
2440  case eobrd_cantype_strain2c:
2441 
2442  txBuffer[0].setId((0x02 << 8) + (ID_MASTER << 4) + target_id);
2443  txBuffer[0].setLen(2);
2444  txBuffer[0].getData()[0]= ICUBCANPROTO_POL_MC_CMD__SET_BOARD_ID;
2445  txBuffer[0].getData()[1]= new_id;
2446  break;
2447 
2448  case icubCanProto_boardType__dsp:
2449  case icubCanProto_boardType__pic:
2450  case icubCanProto_boardType__2dc:
2451  case icubCanProto_boardType__4dc:
2452  case icubCanProto_boardType__bll:
2453  case icubCanProto_boardType__2foc:
2454  case icubCanProto_boardType__jog:
2455  txBuffer[0].setId((ID_MASTER << 4) + target_id);
2456  txBuffer[0].setLen(2);
2457  txBuffer[0].getData()[0]= ICUBCANPROTO_POL_MC_CMD__SET_BOARD_ID;
2458  txBuffer[0].getData()[1]= new_id;
2459  break;
2460 
2461  default:
2462  if(_verbose) yError ("Unknown board type for change of CAN address\n");
2463  return -1;
2464 
2465  }
2466 
2467  set_bus(txBuffer[0], bus);
2468  int ret = m_idriver->send_message(txBuffer, 1);
2469 
2470  // check if send_message was successful
2471  if (ret==0)
2472  {
2473  if(_verbose) yError ("Unable to send message\n");
2474  return -1;
2475  }
2476  // pause
2477  drv_sleep(500);
2478  // update the board list
2479  initschede();
2480 
2481  return 0;
2482 }
2483 //*****************************************************************/
2484 
2485 int cDownloader::initschede()
2486 {
2487  int i;
2488 
2489  // check if driver is running
2490  if (m_idriver == NULL)
2491  {
2492  if(_verbose) yError ("Driver not ready\n");
2493  return -1;
2494  }
2495 
2496  // Send discovery command
2497 
2498  int ret = 0;
2499 
2500 #if defined(DOWNLOADER_ETH_SUPPORTS_MULTIBUS)
2501 
2502  if(iDriver2::eth_driver2 == m_idriver->type())
2503  {
2504  // in here we send the discovery command on the selected CAN bus (CAN1 / CAN2) or on both
2505  if(CanPacket::everyCANbus == get_canbus_id())
2506  {
2507  if(_verbose) yDebug("working on every CAN bus");
2508  }
2509 
2510  set_bus(txBuffer[0], get_canbus_id());
2511  txBuffer[0].setId(build_id(ID_MASTER,ID_BROADCAST));
2512  txBuffer[0].setLen(1);
2513  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_BROADCAST;
2514  ret = m_idriver->send_message(txBuffer, 1);
2515 
2516  }
2517  else
2518  {
2519  // we send the discovery command only on one bus
2520  set_bus(txBuffer[0], get_canbus_id());
2521  txBuffer[0].setId(build_id(ID_MASTER,ID_BROADCAST));
2522  txBuffer[0].setLen(1);
2523  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_BROADCAST;
2524  ret = m_idriver->send_message(txBuffer, 1);
2525  }
2526 
2527 #else
2528 
2529  if(CanPacket::everyCANbus == get_canbus_id())
2530  {
2531  if(_verbose) yDebug("Discovery on every CAN bus is not allowed: reverting to bus CAN1");
2532  set_canbus_id(1);
2533  }
2534 
2535  // we send discovery only on the relevant CAN bus.
2536  set_bus(txBuffer[0], get_canbus_id());
2537 
2538  txBuffer[0].setId(build_id(ID_MASTER,ID_BROADCAST));
2539  txBuffer[0].setLen(1);
2540  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_BROADCAST;
2541  ret = m_idriver->send_message(txBuffer, 1);
2542 
2543 #endif
2544 
2545  // check if send_message was successful
2546  if (ret==0)
2547  {
2548  if(_verbose) yError ("Unable to send message\n");
2549  return -1;
2550  }
2551 
2552  // pause
2553  drv_sleep(300);
2554 
2555  // riceve la risposta
2556  bool done=false;
2557  int read_messages=0;
2558  while(!done)
2559  {
2560  read_messages = m_idriver->receive_message(rxBuffer);
2561  //Timeout: no answers
2562  if (read_messages==0)
2563  {
2564  if(_verbose) yError ("No answers\n");
2565  return -1;
2566  }
2567 
2568  //One (or more) answers received
2569  //Counts the number of the boards
2570  board_list_size = 0;
2571  for (i=0; i<read_messages; i++)
2572  {
2574 #if 0
2575  //fprintf(stderr, "id %.4x ", rxBuffer[i].getId());
2576  fprintf(stderr, "CAN%d:%.2x, l=%d", get_bus(rxBuffer[i]), rxBuffer[i].getId(), rxBuffer[i].getLen());
2577  fprintf(stderr, "d[0] %.2x ", rxBuffer[i].getData()[0]);
2578  fprintf(stderr, "d[1] %.2x ", rxBuffer[i].getData()[1]);
2579  fprintf(stderr, "d[2] %.2x ", rxBuffer[i].getData()[2]);
2580  fprintf(stderr, "d[3] %.2x ", rxBuffer[i].getData()[3]);
2581  fprintf(stderr, "d[4] %.2x ", rxBuffer[i].getData()[4]);
2582  fprintf(stderr, "d[5] %.2x ", rxBuffer[i].getData()[5]);
2583  fprintf(stderr, "d[6] %.2x ", rxBuffer[i].getData()[6]);
2584  fprintf(stderr, "d[7] %.2x\n", rxBuffer[i].getData()[7]);
2585 #endif
2587  if ((rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_BROADCAST) &&
2588  ((rxBuffer[i].getLen()==4)||(rxBuffer[i].getLen()==5)))
2589  board_list_size++;
2590  }
2591 
2592  if (board_list_size==0)
2593  {
2594  // printf ("No Boards found\n");
2595  // return -1;
2596  static int times=0;
2597  times++;
2598  if (times==100)
2599  {
2600  if(_verbose) yError ("No Boards found\n");
2601  return -1;
2602  }
2603  }
2604  else
2605  {
2606  done=true;
2607  }
2608 
2609  }
2610 
2611 // if(_verbose) yDebug ("received all answers to FF\n");
2612 
2613 
2614  //Create the list of the boards
2615  if (board_list !=NULL) delete board_list;
2616  board_list= new sBoard[board_list_size];
2617 
2618  int j = 0;
2619  for (i=0; i<read_messages; i++)
2620  {
2621  if ((rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_BROADCAST) &&
2622  ((rxBuffer[i].getLen()==4)||(rxBuffer[i].getLen()==5))) //old board firmware (backward compatibility)
2623  {
2624 #if defined(DOWNLOADER_USE_IDRIVER2)
2625  board_list[j].bus = rxBuffer[i].getCanBus();
2626 #else
2627  board_list[j].bus = get_canbus_id();
2628 #endif
2629  board_list[j].pid = (rxBuffer[i].getId() >> 4) & 0x0F;
2630  board_list[j].type = rxBuffer[i].getData()[1];
2631  board_list[j].applicationisrunning = (5 == rxBuffer[i].getLen()) ? (true) : (false); // the application replies with a message of len 5, the bootloader 4.
2632  board_list[j].appl_vers_major = rxBuffer[i].getData()[2];
2633  board_list[j].appl_vers_minor = rxBuffer[i].getData()[3];
2634  board_list[j].status = BOARD_RUNNING;
2635  board_list[j].selected = false;
2636  board_list[j].eeprom =false;
2637  memset (board_list[j].serial, 0, 8);
2638  memset (board_list[j].add_info, 0, 32);
2639  if (rxBuffer[i].getLen()==4)
2640  board_list[j].appl_vers_build = -1;
2641  else
2642  board_list[j].appl_vers_build = rxBuffer[i].getData()[4];
2643  board_list[j].prot_vers_major = 0;
2644  board_list[j].prot_vers_minor = 0;
2645 
2646  j++;
2647  }
2648  }
2649 
2650  //if(_verbose) yDebug ("about to ask boardinfo \n");
2651 
2652  for (i=0; i<board_list_size; i++)
2653  {
2654  char board_info [32];
2655  get_board_info (board_list[i].bus, board_list[i].pid, board_info);
2656  strcpy (board_list[i].add_info, board_info);
2657  //pause
2658  drv_sleep(10);
2659  }
2660 
2661 
2662  //if(_verbose) yDebug ("about to ask serialno \n");
2663 
2664  for (i=0; i<board_list_size; i++)
2665  {
2666  char serial_no [32];
2667  get_serial_no (board_list[i].bus, board_list[i].pid, serial_no);
2668  strcpy (board_list[i].serial, serial_no);
2669  if(0 == strlen(board_list[i].serial))
2670  {
2671  snprintf(board_list[i].serial, sizeof(board_list[i].serial), "N/A");
2672  }
2673  //pause
2674  drv_sleep(10);
2675  }
2676 
2677  for (i=0; i<board_list_size; i++)
2678  {
2679  board_list[i].strainregsetinuse = board_list[i].strainregsetatboot = 0;
2680  if(board_list[i].type==icubCanProto_boardType__strain)
2681  {
2682  board_list[i].strainregsetinuse = board_list[i].strainregsetatboot = 1;
2683  }
2684  else if(board_list[i].type==icubCanProto_boardType__strain2 || board_list[i].type==icubCanProto_boardType__strain2c)
2685  {
2686  this->strain_get_regulationset(board_list[i].bus, board_list[i].pid, board_list[i].strainregsetinuse, strain_regsetmode_temporary);
2687  this->strain_get_regulationset(board_list[i].bus, board_list[i].pid, board_list[i].strainregsetatboot, strain_regsetmode_permanent);
2688  }
2689  //pause
2690  drv_sleep(10);
2691  }
2692 
2693 #define TEST_GET_FW_VERSION
2694 
2695 #if defined(TEST_GET_FW_VERSION)
2696 
2697  if(_verbose) yDebug ("about to ask fw version \n");
2698  for(i=0; i<board_list_size; i++)
2699  {
2700  // marco.accame on 25 may 2016: i have added this code for demostration of how we can use the get_firmware_version() function.
2701  // this info is useful for the new fwUpdater. Moreover, it is a good way to further verify if we are in bootloader or not.
2702  // the bootloader does not reply to get-fw-version, whereas the applications replies.
2703  // The only known exception is the mtb application which replies o get-fw-version only after ... somewhere in early 2016.
2704  eObrd_info_t info = {0};
2705  memset(&info, 0, sizeof(info));
2706  bool noreply = true;
2707  int rr = get_firmware_version(board_list[i].bus, board_list[i].pid, (eObrd_cantype_t)board_list[i].type, &info, noreply);
2708  if(_verbose)
2709  {
2710  fprintf(stderr, "board %d: ret = %d, reply = %d, type = %d, f=(%d, %d, %d), pr=(%d, %d)\n", i, rr, !noreply,
2711  info.type,
2712  info.firmware.major, info.firmware.minor, info.firmware.build,
2713  info.protocol.major, info.protocol.minor);
2714  }
2715  board_list[i].prot_vers_major = info.protocol.major;
2716  board_list[i].prot_vers_minor = info.protocol.minor;
2717  drv_sleep(10);
2718  }
2719 #endif
2720 
2721 
2722  if(_verbose) yInfo("CONNECTED: %d Boards",board_list_size);
2723  if(_verbose) yDebug(" BUS:id type version");
2724  for (int i = 0; i < board_list_size; i++)
2725  {
2726  if(_verbose) yDebug(" CAN%d:%d %5d %d.%d.%d", board_list[i].bus, board_list[i].pid, board_list[i].type , board_list[i].appl_vers_major , board_list[i].appl_vers_minor , board_list[i].appl_vers_build);
2727  }
2728 
2729  return 0;
2730 }
2731 
2732 
2733 //*****************************************************************/
2734 int cDownloader::get_canbus_id()
2735 {
2736  return canbus_id;
2737 }
2738 
2739 void cDownloader::set_canbus_id(int id)
2740 {
2741  canbus_id = id;
2742 }
2743 
2744 //*****************************************************************/
2745 
2746 int cDownloader::startscheda(int bus, int board_pid, bool board_eeprom, int board_type)
2747 {
2748  // check if driver is running
2749  if (m_idriver == NULL)
2750  {
2751  if(_verbose) yError ("START_CMD: Driver not ready\n");
2752  return -1;
2753  }
2754 
2755  switch (board_type)
2756  {
2757  case icubCanProto_boardType__dsp:
2758  case icubCanProto_boardType__pic:
2759  case icubCanProto_boardType__2dc:
2760  case icubCanProto_boardType__4dc:
2761  case icubCanProto_boardType__bll:
2762  {
2763  // Send command
2764  txBuffer[0].setId(build_id(ID_MASTER, board_pid));
2765  txBuffer[0].setLen(1);
2766  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_BOARD;
2767 
2768  //makes the first jump
2769  set_bus(txBuffer[0], bus);
2770  m_idriver->send_message(txBuffer, 1);
2771  drv_sleep(250);
2772  }
2773  break;
2774  case icubCanProto_boardType__skin:
2775  case icubCanProto_boardType__strain:
2776  case icubCanProto_boardType__mais:
2777  case icubCanProto_boardType__2foc:
2778  case icubCanProto_boardType__6sg:
2779  case icubCanProto_boardType__jog:
2780  case icubCanProto_boardType__mtb4:
2781  case icubCanProto_boardType__strain2:
2782  case icubCanProto_boardType__rfe:
2783  case icubCanProto_boardType__sg3:
2784  case icubCanProto_boardType__psc:
2785  case icubCanProto_boardType__mtb4w:
2786  case icubCanProto_boardType__mtb4c:
2787  case eobrd_cantype_pmc:
2788  case eobrd_cantype_amcbldc:
2789  case eobrd_cantype_strain2c:
2790  case icubCanProto_boardType__unknown:
2791  {
2792  // Send command
2793  txBuffer[0].setId(build_id(ID_MASTER,board_pid));
2794  txBuffer[0].setLen(2);
2795  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_BOARD;
2796  txBuffer[0].getData()[1]= (int) board_eeprom;
2797 
2798  //makes the first jump
2799  set_bus(txBuffer[0], bus);
2800  m_idriver->send_message(txBuffer, 1);
2801  drv_sleep(1500);
2802  }
2803  break;
2804  }
2805  set_bus(txBuffer[0], bus);
2806  int ret = m_idriver->send_message(txBuffer, 1);
2807 
2808  // check if send_message was successful
2809  if (ret==0)
2810  {
2811  if(_verbose) yError ("START_CMD: Unable to send message\n");
2812  return -1;
2813  }
2814  // marco.accame: wait some more time (it was 500 ms) to wait amcbldc and pmc boards to erase their flash
2815  drv_sleep(2000);
2816 
2817  // riceve la risposta
2818  int read_messages = m_idriver->receive_message(rxBuffer);
2819 
2820 
2821  //One (or more) answers received
2822  for (int i=0; i<read_messages; i++)
2823  {
2824  if (rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_BOARD &&
2825  (((rxBuffer[i].getId() >> 4) & 0x0F) == board_pid) &&
2826  (((rxBuffer[i].getId() >> 8) & 0x07) == ICUBCANPROTO_CLASS_BOOTLOADER))
2827  {
2828  //received ACK from board
2829  //printf ("START_CMD: ACK received from board: %d\n", board_pid);
2830  return 0;
2831  }
2832  }
2833  // return 0; //DEBUG
2834 
2835  //ERROR
2836  if(_verbose) yError ("START_CMD: No ACK received from board %d\n", board_pid);
2837  return -1;
2838 
2839 }
2840 
2841 //*****************************************************************/
2842 
2843 int cDownloader::stopscheda(int bus, int board_pid)
2844 {
2845  // check if driver is running
2846  if (m_idriver == NULL)
2847  {
2848  if(_verbose) yError ("STOP_CMD: Driver not ready\n");
2849  return -1;
2850  }
2851 
2852  // Send command
2853  txBuffer[0].setId(build_id(ID_MASTER, board_pid));
2854  txBuffer[0].setLen(1);
2855  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_END;
2856  set_bus(txBuffer[0], bus);
2857  int ret = m_idriver->send_message(txBuffer, 1);
2858 
2859  // check if send_message was successful
2860  if (ret==0)
2861  {
2862  if(_verbose) yError ("STOP_CMD: Unable to send message\n");
2863  return -1;
2864  }
2865 
2866  //pause
2867  drv_sleep(5);
2868 
2869  // riceve la risposta
2870  int read_messages = m_idriver->receive_message(rxBuffer);
2871 
2872  //One (or more) answers received
2873  for (int i=0; i<read_messages; i++)
2874  {
2875  if (rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_END &&
2876  (((rxBuffer[i].getId() >> 4) & 0x0F) == board_pid || board_pid == 15 ) &&
2877  (((rxBuffer[i].getId() >> 8) & 0x07) == ICUBCANPROTO_CLASS_BOOTLOADER))
2878  {
2879  //received ACK from board
2880  //printf ("STOP_CMD: ACK received from board: %d\n", board_pid);
2881  return 0;
2882  }
2883  }
2884 
2885  //ERROR
2886  if(_verbose) yError ("TOP_CMD: No ACK received from board %d\n", board_pid);
2887  return -1;
2888 }
2889 
2890 //*****************************************************************/
2891 
2892 int getvalue(char* line, int len)
2893 {
2894  char hexconv_buffer[5];
2895  memset (hexconv_buffer, '\0', sizeof(hexconv_buffer) );
2896  strncpy(hexconv_buffer,line,len);
2897  return axtoi (hexconv_buffer);
2898 }
2899 
2900 //*****************************************************************/
2901 int cDownloader::verify_ack(int command, int read_messages)
2902 {
2903 
2904  int i,k;
2905 
2906 /*
2907  for(int m=0;m<read_messages;m++)
2908  {
2909  fprintf(stderr, "%4x %d %d %d\n",
2910  rxBuffer[m].getId(),
2911  rxBuffer[m].getLen(),
2912  rxBuffer[m].getData()[0],
2913  rxBuffer[m].getData()[1]);
2914  }
2915 */
2916 
2917  for (i=0; i<board_list_size; i++)
2918  {
2919  if (board_list[i].selected==true)
2920  if (board_list[i].status == BOARD_WAITING ||
2921  board_list[i].status == BOARD_DOWNLOADING)
2922  {
2923  board_list[i].status = BOARD_WAITING_ACK;
2924 
2925  for (k=0; k<read_messages; k++)
2926  {
2927  if ((rxBuffer[k].getData()[0]==command) &&
2928  (rxBuffer[k].getLen() == 2) &&
2929  (rxBuffer[k].getData()[1]==1))
2930  {
2931  if(board_list[i].pid == get_src_from_id(rxBuffer[k].getId()))
2932  {
2933  #if defined(DOWNLOADER_USE_IDRIVER2)
2934  if(board_list[i].bus == rxBuffer[k].getCanBus())
2935  #else
2936  if(1)
2937  #endif
2938  {
2939  board_list[i].status=BOARD_DOWNLOADING;
2940  }
2941  }
2942  }
2943  }
2944  }
2945  }
2946 
2947  for (i=0; i<board_list_size; i++)
2948  {
2949  if (board_list[i].selected==true && board_list[i].status == BOARD_WAITING_ACK)
2950  {
2951  //@@@@ board_list[i].status=BOARD_ERR;
2952  return -1;
2953  }
2954  }
2955  return 0;
2956 }
2957 
2958 
2959 
2960 //*****************************************************************/
2961 // Return values:
2962 // 0 one line downloaded, continuing the download...
2963 // 1 Current downloading, everything OK
2964 // -1 Fatal error
2965 
2966 int cDownloader::download_motorola_line(char* line, int len, int bus, int board_pid)
2967 {
2968  static double now;
2969  static double prev;
2970 
2971  now=Time::now();
2972  double dT=now-prev;
2973  prev=now;
2974 
2975  // fprintf(stderr, "dT:%.2lf [ms]\n", dT*1000);
2976 
2977  char sprsRecordType=0;
2978  unsigned long int sprsChecksum=0;
2979  int sprsMemoryType=1;
2980  long unsigned int sprsAddress;
2981  int sprsLength;
2982  int i,j,k;
2983  int ret =0;
2984  int read_messages=0;
2985 
2986  for (i=2; i<len; i=i+2)
2987  {
2988  int value= getvalue(line+i,2);
2989  sprsChecksum+= value;
2990  // printf ("chk: %d %d\n", value, sprsChecksum);
2991 
2992  }
2993 
2994  if ((sprsChecksum & 0xFF) == 0xFF)
2995  {
2996  // printf ("Checksum OK\n");
2997  }
2998  else
2999  {
3000  if(_verbose) yError ("Failed Checksum\n");
3001  return -1;
3002  }
3003 
3004  //state: WAIT
3005  if (!(line[0] == 'S'))
3006  {
3007  if(_verbose) yError ("start tag character not found\n");
3008  return -1;
3009  }
3010  i=1;
3011 
3012  //state: TYPE
3013  sprsRecordType=char(*(line+i));
3014  i++;
3015 
3016  //state: LENGTH
3017  sprsLength=getvalue(line+i,2)-4-1;
3018  i=i+2;
3019 
3020  switch (sprsRecordType)
3021  {
3022  case SPRS_TYPE_0:
3023 
3024  return 0;
3025 
3026  case SPRS_TYPE_3:
3027 
3028  //state: ADDRESS
3029  sprsAddress=getvalue(line+i,4);
3030  i+=4;
3031 
3032  if (sprsAddress==0x0020)
3033  sprsMemoryType=1;
3034  else
3035  sprsMemoryType=0;
3036 
3037  sprsAddress=getvalue(line+i,4);
3038  i+=4;
3039 
3040  //state: SEND
3041  txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3042  txBuffer[0].setLen(5);
3043  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_ADDRESS;
3044  txBuffer[0].getData()[1]= sprsLength;
3045  txBuffer[0].getData()[2]= (unsigned char) ((sprsAddress) & 0x00FF);
3046  txBuffer[0].getData()[3]= (unsigned char) ((sprsAddress>>8) & 0x00FF);
3047  txBuffer[0].getData()[4]= sprsMemoryType;
3048 
3049  //send here
3050  set_bus(txBuffer[0], bus);
3051  ret = m_idriver->send_message(txBuffer,1);
3052 
3053  // check if send_message was successful
3054  if (ret==0)
3055  {
3056  if(_verbose) yError ("Unable to send message\n");
3057  return -1;
3058  }
3059 
3060  // pause
3061  // drv_sleep(5);
3062 
3063  //prepare packet
3064  int tmp, rest;
3065  if ((sprsLength%6) == 0)
3066  {
3067  tmp=sprsLength / 6;
3068  rest=6;
3069  }
3070  else
3071  {
3072  tmp=sprsLength / 6 + 1;
3073  rest=sprsLength % 6;
3074  }
3075 
3076  for (j=1; j<= tmp; j++)
3077  {
3078  txBuffer[0].getData()[0]=ICUBCANPROTO_BL_DATA;
3079  if (j<tmp) txBuffer[0].setLen(7);
3080  else txBuffer[0].setLen(rest+1);
3081 
3082  for (k=1; k<=6; k++)
3083  {
3084  txBuffer[0].getData()[k] = getvalue(line+i+((k-1)*2+((j-1)*12)),2);
3085  }
3086 
3087  //send here
3088  set_bus(txBuffer[0], bus);
3089  ret = m_idriver->send_message(txBuffer,1);
3090 
3091  // check if send_message was successful
3092  if (ret==0)
3093  {
3094  if(_verbose) yError ("Unable to send message\n");
3095  return -1;
3096  }
3097 
3098  //pause
3099  // drv_sleep(5);
3100  }
3101 
3102  //pause
3103  // drv_sleep(10);
3104 
3105  //receive one ack for the whole line
3106  double passed;
3107  passed=Time::now()-now;
3108  // fprintf(stderr, "Passed:%.2lf [ms]\n", passed*1000);
3109  read_messages = m_idriver->receive_message(rxBuffer, nSelectedBoards);
3110  // fprintf(stderr, "%u\n", read_messages);
3111  // fprintf(stderr, "Skipping ack\n");
3112  //return verify_ack(ICUBCANPROTO_BL_DATA, read_messages);
3113  return 0;
3114  break;
3115  case SPRS_TYPE_7:
3116 
3117  //state: SEND
3118  txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3119  txBuffer[0].setLen(5);
3120  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_START;
3121  txBuffer[0].getData()[4]= getvalue(line+i,2); i+=2;
3122  txBuffer[0].getData()[3]= getvalue(line+i,2); i+=2;
3123  txBuffer[0].getData()[2]= getvalue(line+i,2); i+=2;
3124  txBuffer[0].getData()[1]= getvalue(line+i,2);
3125 
3126  //send here
3127  set_bus(txBuffer[0], bus);
3128  ret = m_idriver->send_message(txBuffer, 1);
3129 
3130  // check if send_message was successful
3131  if (ret==0)
3132  {
3133  if(_verbose) yError ("Unable to send message\n");
3134  return -1;
3135  }
3136 
3137  //pause
3138  drv_sleep(10+5);
3139 
3140  // riceve la risposta
3141  read_messages = m_idriver->receive_message(rxBuffer);
3142  verify_ack(ICUBCANPROTO_BL_START, read_messages);
3143  return 0;
3144 
3145  break;
3146 
3147 
3148  default:
3149  if(_verbose) yError ("wrong format tag character %c (hex:%X)\n", sprsRecordType, sprsRecordType);
3150  return -1;
3151 
3152  break;
3153  }
3154 
3155  if(_verbose) yError ("Can't reach here!\n");
3156  return -1;
3157 }
3158 
3159 //*****************************************************************/
3160 // This function read one line of the hexintel file and send it to a board using the correct protocol
3161 // Return values:
3162 // 0 one line downloaded, continuing the download...
3163 // 1 Current downloading, everything OK
3164 // -1 Fatal error
3165 
3166 int cDownloader::download_hexintel_line(char* line, int len, int bus, int board_pid, bool eeprom, int board_type)
3167 {
3168  char sprsRecordType=0;
3169  unsigned int sprsState;
3170  unsigned long int sprsChecksum=0;
3171  int sprsMemoryType=0;
3172  long unsigned int sprsAddress=0;
3173  int sprsLength=0;
3174  unsigned int sprsData[50];
3175  int i,j,k;
3176  int ret =0;
3177  int read_messages=0;
3178 
3179  for (i=1; i<len; i=i+2)
3180  {
3181  int value= getvalue(line+i,2);
3182  sprsChecksum+= value;
3183  // printf ("chk: %d %d\n", value, sprsChecksum);
3184  }
3185  sprsChecksum = (sprsChecksum & 0xFF);
3186  if (sprsChecksum == 0x00)
3187  {
3188  // printf ("Checksum OK\n");
3189  }
3190  else
3191  {
3192  if(_verbose) yError ("Failed Checksum\n");
3193  return -1;
3194  }
3195 
3196  sprsState=SPRS_STATE_WAIT;
3197  // Init of parsing process
3198  do
3199  {
3200  switch (sprsState)
3201  {
3202  case SPRS_STATE_WAIT:
3203  //check the first character of the line
3204  if (!(line[0] == ':'))
3205  {
3206  if(_verbose) yError("start tag character not found in hex file\n");
3207  return -1;
3208  }
3209  else
3210  {
3211  sprsState=SPRS_STATE_LENGTH;
3212  i=1;
3213  }
3214  break;
3215  case SPRS_STATE_TYPE:
3216 
3217  sprsRecordType=char(*(line+i+1));//char(getvalue(line+i,2));//(char)(*(line+i));
3218  i=i+2;
3219  if (sprsLength==0)
3220  sprsState=SPRS_STATE_CHECKSUM;
3221  else
3222  sprsState=SPRS_STATE_DATA;
3223  break;
3224  case SPRS_STATE_LENGTH:
3225 
3226  sprsLength= getvalue(line+i,2);
3227  i=i+2;
3228  sprsState=SPRS_STATE_ADDRESS;
3229  break;
3230  case SPRS_STATE_ADDRESS:
3231 
3232  sprsAddress=getvalue(line+i,4);
3233  i=i+4;
3234  sprsState=SPRS_STATE_TYPE;
3235  break;
3236  case SPRS_STATE_DATA:
3237 
3238  switch (sprsRecordType)
3239  {
3240  case SPRS_TYPE_0:
3241 
3242  for (k=0;k<sprsLength;k++)
3243  {
3244  sprsData[k]=getvalue(line+i,2);
3245  i=i+2;
3246  }
3247  break;
3248 
3249  case SPRS_TYPE_4:
3250 
3251  sprsPage=getvalue(line+i,4);
3252  i=i+4;
3253 
3254  // marco.accame on 25may17:
3255  // here is extra safety to avoid an accidental loading of stm32 code (which starts at 0x0800) on dspic based boards.
3256  // the bootloader on dspic boards in such a case erases the first sector which tells to execute to the bootloader
3257  // at reset with teh result that the board become unreachable.
3258  // as we shall release strain2.hex and mtb4.hex which use stm32 mpus w/ code at 0x0800 and beyond, any accidental
3259  // attempt to program an old strain w/ strain2.hex becomes more probable. As the damage is high (removal of the the
3260  // FT sensor + disassembly + re-programming + recalibration), some sort of protection is mandatory.
3261  // instead, strain2/mtb4 are safe if any attempt is done to program them with old strain.hex/skin.hex code
3262  if(sprsPage >= 0x0800)
3263  { // only mtb4, strain2, rfe, sg3, psc, mtb4w are allowed to use such a code space.
3264  if((icubCanProto_boardType__mtb4 == board_type) || (icubCanProto_boardType__strain2 == board_type) ||
3265  (icubCanProto_boardType__rfe == board_type) || (icubCanProto_boardType__sg3 == board_type) ||
3266  (icubCanProto_boardType__psc == board_type) || (icubCanProto_boardType__mtb4w == board_type) ||
3267  (icubCanProto_boardType__pmc == board_type)
3268  || (icubCanProto_boardType__amcbldc == board_type)
3269  || (icubCanProto_boardType__mtb4c == board_type)
3270  || (icubCanProto_boardType__strain2c == board_type)
3271  )
3272  { // it is ok
3273  }
3274  else
3275  { // be careful with that axe, eugene. ahhhhhhhhhhhhhhhh
3276  //if(_verbose)
3277  {
3278  char msg[32] = {0};
3279  snprintf(msg, sizeof(msg), "0x%04X", sprsPage);
3280  yError() << "Upload of FW to board" << eoboards_type2string2((eObrd_type_t)board_type, eobool_true) << "is aborted because it was detected a wrong page number =" << msg << "in the .hex file";
3281  yError() << "You must have loaded the .hex file of another board. Perform a new discovery, check the file name and retry.";
3282  }
3283  return -1;
3284  }
3285  }
3286 
3287 
3288  break;
3289  }
3290  sprsState=SPRS_STATE_CHECKSUM;
3291  break;
3292  case SPRS_STATE_CHECKSUM:
3293 
3294  sprsState=SPRS_STATE_WAIT;
3295  if (sprsChecksum==0)
3296  {
3297  switch (sprsRecordType)
3298  {
3299  case SPRS_TYPE_0:
3300 
3301  //if (sprsPage==0)
3302  {
3303  //SEND
3304  txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3305  txBuffer[0].setLen(7);
3306  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_ADDRESS;
3307  txBuffer[0].getData()[1]= sprsLength;
3308  txBuffer[0].getData()[2]= (unsigned char) ((sprsAddress) & 0x00FF);
3309  txBuffer[0].getData()[3]= (unsigned char) ((sprsAddress>>8) & 0x00FF);
3310  txBuffer[0].getData()[4]= sprsMemoryType;
3311  txBuffer[0].getData()[5]= (unsigned char) ((sprsPage) & 0x00FF);
3312  txBuffer[0].getData()[6]= (unsigned char) ((sprsPage >>8) & 0x00FF);
3313  }
3314  //send here
3315  set_bus(txBuffer[0], bus);
3316  ret = m_idriver->send_message(txBuffer,1);
3317  // check if send_message was successful
3318  if (ret==0)
3319 
3320 
3321  {
3322  if(_verbose) yError ("Unable to send message\n");
3323  return -1;
3324  }
3325  //pause
3326  drv_sleep(10);
3327 
3328  //prepare packet
3329  int tmp, rest;
3330  if ((sprsLength%6) == 0)
3331  {
3332  tmp=sprsLength / 6;
3333  rest=6;
3334  }
3335  else
3336  {
3337  tmp=sprsLength / 6 + 1;
3338  rest=sprsLength % 6;
3339  }
3340 
3341  for (j=1; j<= tmp; j++)
3342  {
3343  txBuffer[0].getData()[0]=ICUBCANPROTO_BL_DATA;
3344  if (j<tmp) txBuffer[0].setLen(7);
3345  else txBuffer[0].setLen(rest+1);
3346 
3347  for (k=1; k<=6; k++)
3348  {
3349  txBuffer[0].getData()[k] = sprsData[(k-1)+((j-1)*6)];//getvalue(line+i+((k-1)*2+((j-1)*12)),2);
3350  }
3351 
3352  //send here
3353  set_bus(txBuffer[0], bus);
3354  ret = m_idriver->send_message(txBuffer,1);
3355 
3356  // check if send_message was successful
3357  if (ret==0)
3358  {
3359  if(_verbose) yError ("Unable to send message\n");
3360  return -1;
3361  }
3362  //pause
3363  drv_sleep(5);
3364  }
3365  //receive one ack for the whole line
3366  read_messages = m_idriver->receive_message(rxBuffer,nSelectedBoards, 10);
3367  ret=verify_ack(ICUBCANPROTO_BL_DATA, read_messages);
3368  //DEBUG
3369 
3370  // return 0;
3371  return ret;
3372  break;
3373 
3374  case SPRS_TYPE_1:
3375 
3376  //SEND
3377  txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3378  txBuffer[0].setLen(5);
3379  txBuffer[0].getData()[0]= ICUBCANPROTO_BL_START;
3380  txBuffer[0].getData()[1]= 0;
3381  txBuffer[0].getData()[2]= 0;
3382  txBuffer[0].getData()[3]= 0;
3383  txBuffer[0].getData()[4]= 0;
3384 
3385  //send here
3386  set_bus(txBuffer[0], bus);
3387  ret = m_idriver->send_message(txBuffer,1);
3388  // check if send_message was successful
3389  if (ret==0)
3390  {
3391  if(_verbose) yError ("Unable to send message\n");
3392  return -1;
3393  }
3394  //pause
3395  drv_sleep(5);
3396  //receive the ack from the board
3397  read_messages = m_idriver->receive_message(rxBuffer);
3398  ret=verify_ack(ICUBCANPROTO_BL_START, read_messages);
3399  //DEBUG
3400  //return 0;
3401  return ret;
3402 
3403  break;
3404  // case SPRS_TYPE_4:
3405  // break;
3406  // return 0;
3407  default:
3408  return 0;
3409  }
3410  } //end if
3411  else
3412  {
3413  if(_verbose) yError ("Checksum Error\n");
3414  }
3415  break;
3416  } //end switch
3417  }
3418  while(true);
3419 
3420  if(_verbose) yError ("Can't reach here!\n");
3421  return -1;
3422 }
3423 //*****************************************************************/
3424 
3425 int cDownloader::open_file(std::string file)
3426 {
3427  progress=0;
3428  filestr.close();
3429  filestr.clear();
3430  filestr.open (file.c_str(), fstream::in);
3431  if (!filestr.is_open())
3432  {
3433  if(_verbose) yError ("Error opening file!\n");
3434  return -1;
3435  }
3436 
3437  file_length=0;
3438  char buffer[256];
3439  while (!filestr.eof())
3440  {
3441  filestr.getline (buffer,256);
3442  file_length++;
3443  }
3444  //printf ("length: %d\n",file_length);
3445 
3446  filestr.close();
3447  filestr.clear();
3448  filestr.open (file.c_str(), fstream::in);
3449  if (!filestr.is_open())
3450  {
3451  if(_verbose) yError ("Error opening file!\n");
3452  return -1;
3453  }
3454 
3455  return 0;
3456 }
3457 
3458 //*****************************************************************/
3459 // Return values:
3460 // 0 Download terminated, everything OK
3461 // 1 Current downloading, everything OK
3462 // -1 Fatal error in sending one command
3463 int cDownloader::download_file(int bus, int board_pid, int download_type, bool board_eeprom)
3464 {
3465 
3466  if (!filestr.is_open())
3467  {
3468  if(_verbose) yError ("File not open!\n");
3469  return -1;
3470  }
3471 
3472  char buffer[256];
3473  int ret = 0;
3474 
3475  nSelectedBoards=0;
3476  int i=0;
3477  for (i=0; i<board_list_size; i++)
3478  {
3479  if (board_list[i].selected==true)
3480  nSelectedBoards++;
3481  }
3482 
3483  if (!filestr.eof())
3484  {
3485  filestr.getline (buffer,256);
3486 
3487  //avoid to download empty lines
3488  if (strlen(buffer)!=0)
3489  {
3490  switch (download_type)
3491  {
3492  case icubCanProto_boardType__dsp:
3493  case icubCanProto_boardType__2dc:
3494  case icubCanProto_boardType__4dc:
3495  case icubCanProto_boardType__bll:
3496  ret = download_motorola_line(buffer, strlen(buffer), bus, board_pid);
3497  break;
3498  case icubCanProto_boardType__pic:
3499  case icubCanProto_boardType__skin:
3500  case icubCanProto_boardType__strain:
3501  case icubCanProto_boardType__mais:
3502  case icubCanProto_boardType__2foc:
3503  case icubCanProto_boardType__jog:
3504  case icubCanProto_boardType__6sg:
3505  case icubCanProto_boardType__mtb4:
3506  case icubCanProto_boardType__strain2:
3507  case icubCanProto_boardType__rfe:
3508  case icubCanProto_boardType__sg3:
3509  case icubCanProto_boardType__psc:
3510  case icubCanProto_boardType__mtb4w:
3511  case icubCanProto_boardType__mtb4c:
3512  case icubCanProto_boardType__pmc:
3513  case icubCanProto_boardType__amcbldc:
3514  case icubCanProto_boardType__strain2c:
3515  ret = download_hexintel_line(buffer, strlen(buffer), bus, board_pid, board_eeprom, download_type);
3516 
3517  break;
3518  case icubCanProto_boardType__unknown:
3519  default:
3520  ret =-1;
3521  break;
3522  }
3523  if (ret != 0)
3524  {
3525  if(_verbose) yError("fatal error during download: abort\n");
3526  // fatal error during download, abort
3527  filestr.close();
3528  return -1;
3529  }
3530  }
3531 
3532  progress++;
3533  //everything OK
3534  return 1;
3535  }
3536  else
3537  {
3538  filestr.close();
3539  filestr.clear();
3540  //download terminated OK
3541  return 0;
3542  }
3543 }
3544 
3545 void cDownloader::clean_rx(void)
3546 {
3547  m_idriver->receive_message(rxBuffer,64,0.001);
3548 }
3549 
3550 #if defined(DOWNLOADER_USE_IDRIVER2)
3551 
3552 void cDownloader::set_bus(CanPacket &pkt, int bus)
3553 {
3554  pkt.setCanBus(bus);
3555 }
3556 
3557 int cDownloader::get_bus(CanPacket &pkt)
3558 {
3559  return pkt.getCanBus();
3560 }
3561 
3562 #else
3563 
3564 void cDownloader::set_bus(yarp::dev::CanMessage &msg, int bus)
3565 {
3566  // nothing
3567 }
3568 
3569 int cDownloader::get_bus(yarp::dev::CanMessage &msg)
3570 {
3571  return get_canbus_id();
3572 }
3573 
3574 #endif
3575 
3576 
3577 
3578 int cDownloader::strain_calibrate_offset2_strain1 (int bus, int target_id, int16_t t, string *errorstring)
3579 {
3580 #if 1
3581  return strain_calibrate_offset2_strain1safer(bus, target_id, t, 2, false, errorstring);
3582 #else
3583  // check if driver is running
3584  if (m_idriver == NULL)
3585  {
3586  if(_verbose) yError ("Driver not ready\n");
3587  return -1;
3588  }
3589 
3590  // marco.accame: transform [-32K, +32K) into [0, +64K)
3591  unsigned int middle_val = 32768 + t;
3592 
3593  // in strain1 we dont have the concept of regulationset. however, if we use strain_regset_inuse which is = 0 we are ok.
3594  const int regset = 0; // strain_regset_inuse;
3595 
3596  int daclimit = 0x3ff;
3597  int dacstep = 1;
3598  long tolerance = 256;
3599 
3600  int channel=0;
3601  int i=0;
3602  int ret =0;
3603  long error = 0;
3604  unsigned int measure = 0;
3605  unsigned int dac = 0;
3606  int cycle =0;
3607  int read_messages;
3608 
3609  for (channel=0; channel<6; channel++)
3610  {
3611  // yDebug() << "starting OFFSET of channel" << channel;
3612  // Send read channel command to strain board
3613  txBuffer[0].setId((2 << 8) + target_id);
3614  txBuffer[0].setLen(3);
3615  txBuffer[0].getData()[0]= 0x0C;
3616  txBuffer[0].getData()[1]= channel;
3617  txBuffer[0].getData()[2]= 0;
3618  set_bus(txBuffer[0], bus);
3619  ret = m_idriver->send_message(txBuffer, 1);
3620  // check if send_message was successful
3621  if (ret==0)
3622  {
3623  if(_verbose) yError ("Unable to send message\n");
3624  return -1;
3625  }
3626  //read adc
3627  read_messages = m_idriver->receive_message(rxBuffer,1);
3628  for (i=0; i<read_messages; i++)
3629  {
3630  if (rxBuffer[i].getData()[0]==0x0C)
3631  {
3632  measure = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
3633  break;
3634  }
3635  }
3636 
3637 
3638  //read dac
3639  txBuffer[0].setId((2 << 8) + target_id);
3640  txBuffer[0].setLen(2);
3641  txBuffer[0].getData()[0]= 0x0B;
3642  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
3643  txBuffer[0].getData()[1]= (channel & 0x0f);
3644  set_bus(txBuffer[0], bus);
3645  ret = m_idriver->send_message(txBuffer, 1);
3646 
3647  read_messages = m_idriver->receive_message(rxBuffer,1);
3648  for (i=0; i<read_messages; i++)
3649  {
3650  if (rxBuffer[i].getData()[0]==0x0B)
3651  {
3652  dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
3653  break;
3654  }
3655  }
3656 
3657  error = long(measure) - long(middle_val);
3658  cycle=0;
3659 
3660  while (abs(error)>tolerance && cycle<daclimit)
3661  {
3662  if (error>0) dac -= dacstep;
3663  else dac += dacstep;
3664 
3665  if (dac>daclimit) dac = daclimit;
3666  if (dac<0) dac = 0;
3667 
3668  //yDebug() << "iter" << cycle << "err = " << error << "next dac =" << dac;
3669 
3670  // Send transmission command to strain board
3671  txBuffer[0].setId((2 << 8) + target_id);
3672  txBuffer[0].setLen(4);
3673  txBuffer[0].getData()[0]= 0x04;
3674  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
3675  txBuffer[0].getData()[2]= dac >> 8;
3676  txBuffer[0].getData()[3]= dac & 0xFF;
3677  set_bus(txBuffer[0], bus);
3678  int ret = m_idriver->send_message(txBuffer, 1);
3679 
3680  //wait
3681  drv_sleep(3);
3682 
3683  // Send read channel command to strain board
3684  txBuffer[0].setId((2 << 8) + target_id);
3685  txBuffer[0].setLen(3);
3686  txBuffer[0].getData()[0]= 0x0C;
3687  txBuffer[0].getData()[1]= channel;
3688  txBuffer[0].getData()[2]= 0;
3689  set_bus(txBuffer[0], bus);
3690  ret = m_idriver->send_message(txBuffer, 1);
3691  // check if send_message was successful
3692  if (ret==0)
3693  {
3694  if(_verbose) yError ("Unable to send message\n");
3695  return -1;
3696  }
3697  //read adc
3698  read_messages = m_idriver->receive_message(rxBuffer, 1);
3699  for (i=0; i<read_messages; i++)
3700  {
3701  if (rxBuffer[i].getData()[0]==0x0C)
3702  {
3703  measure = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
3704  break;
3705  }
3706  }
3707 
3708  error = long(measure) - long(middle_val);
3709  cycle++;
3710  }
3711 
3712  }
3713 
3714  return 0;
3715 #endif
3716 }
3717 
3718 
3719 int cDownloader::strain_calibrate_offset2_strain2(int bus, int target_id, const std::vector<strain2_ampl_discretegain_t> &gains, const std::vector<int16_t> &targets, string *errorstring)
3720 {
3721  // the calibration of the offset is meaningful only for the calibration set in use.
3722  const int regset = strain_regset_inuse;
3723  const unsigned int NUMofCHANNELS = 6;
3724 
3725  std::ostringstream ss;
3726 
3727  // check if driver is running
3728  if (m_idriver == NULL)
3729  {
3730  if(_verbose) yError ("Driver not ready\n");
3731  Log(std::string("strain_calibrate_offset2_strain2(): failure. driver no ready"));
3732  return -1;
3733  }
3734 
3735 
3736  ss.str("");
3737  ss.clear();
3738  ss << "performing offset autotuning for strain2";
3739  Log(ss.str());
3740 
3741  // step 1: apply the gains. the initial offset will be meaning less. however strain_set_amplifier_discretegain() assign offsets all equal to 32k-1
3742  for(int channel=0; channel<NUMofCHANNELS; channel++)
3743  {
3744  ss.str("");
3745  ss.clear();
3746  ss << "- on ch " << std::to_string(channel) << ": we impose g = " << std::to_string(static_cast<int>(strain_amplifier_discretegain2float(gains[channel])));
3747 
3748  if(0 != strain_set_amplifier_discretegain(bus, target_id, channel, gains[channel], regset, errorstring))
3749  {
3750  if(_verbose)
3751  {
3752  Log(ss.str());
3753  Log("strain_calibrate_offset2_strain2(): failure of strain_set_amplifier_discretegain()");
3754  }
3755  return -1;
3756  }
3757  // i wait some time
3758  yarp::os::Time::delay(1.0);
3759 
3760  float gaain = 0;
3761  uint16_t ooffset = 0;
3762  strain_get_amplifier_gain_offset(bus, target_id, channel, gaain, ooffset, regset, errorstring);
3763 
3764  ss << " and read (g, o) = (" << std::to_string(static_cast<int>(gaain)) << ", " << std::to_string(ooffset) << ")";
3765  Log(ss.str());
3766  }
3767 
3768 
3769 
3770  yarp::os::Time::delay(2.0);
3771 
3772 
3773  // step 3: eval if the targets are equal or not. if they are all equal we send a single command.
3774  // if not we must send one command per channel.
3775 
3776  int16_t singletargetVALUE = targets[0];
3777  bool singletargetTHEREIS = true;
3778  for(int channel=1; channel<NUMofCHANNELS; channel++)
3779  {
3780  if(singletargetVALUE != targets[channel])
3781  {
3782  singletargetTHEREIS = false;
3783  break;
3784  }
3785  }
3786 
3787 
3788  long tolerance = 256;
3789 
3790 
3791 
3792  tolerance = 256;
3793  uint8_t samples2average = 8; // if zero, the board uses its default (= 4)
3794 
3795 
3796 
3797  // step3: autocalib
3798  const uint8_t everychannel = 0x0f;
3799 
3800  uint8_t channel2autocalib = everychannel; // the channel(s) ...
3801  unsigned int middle_val = 32768; // transform [-32K, +32K) into [0, +64K)
3802  uint8_t okmask = 0x3f; // all six channel
3803 
3804  if(true == singletargetTHEREIS)
3805  {
3806 
3807  channel2autocalib = everychannel;
3808  middle_val = 32768 + singletargetVALUE;
3809  okmask = 0x3f; // all six channel
3810 
3811 
3812  ss.str("");
3813  ss.clear();
3814  ss << "STEP-2. there is a single ADC target: performing parallel regularization";
3815  Log(ss.str());
3816 
3817  ss.str("");
3818  ss.clear();
3819  ss << " params: target = " << std::to_string(singletargetVALUE) << " tolerance = " << std::to_string(tolerance) << " samples2average = " << std::to_string(samples2average);
3820 
3821  // sending an autocalib message
3822  txBuffer[0].setId((2 << 8) + target_id);
3823  txBuffer[0].setLen(8);
3824  txBuffer[0].getData()[0]= 0x22;
3825  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel2autocalib & 0x0f);
3826  txBuffer[0].getData()[2]= 0; // mode oneshot, the only possible so far
3827  txBuffer[0].getData()[3]= middle_val & 0x00ff; // little endian
3828  txBuffer[0].getData()[4]= (middle_val >> 8) & 0x00ff; // little endian
3829  txBuffer[0].getData()[5]= tolerance & 0x00ff; // little endian
3830  txBuffer[0].getData()[6]= (tolerance >> 8) & 0x00ff; // little endian
3831  txBuffer[0].getData()[7]= samples2average;
3832  set_bus(txBuffer[0], bus);
3833  // yDebug("strain2-amplifier-tuning: STEP-3. sent message = [%x, %x, %x, %x, %x, %x, %x, %x]", txBuffer[0].getData()[0], txBuffer[0].getData()[1], txBuffer[0].getData()[2], txBuffer[0].getData()[3], txBuffer[0].getData()[4], txBuffer[0].getData()[5], txBuffer[0].getData()[6], txBuffer[0].getData()[7]);
3834  int ret = m_idriver->send_message(txBuffer, 1);
3835  // check if send_message was successful
3836  if (ret==0)
3837  {
3838  if(_verbose) yError ("Unable to send message\n");
3839  return -1;
3840  }
3841 
3842  // now wait for a reply for most 3 seconds
3843  double TOUT = 3.0;
3844 
3845  ss.str("");
3846  ss.clear();
3847  ss << "STEP-3. results ...";
3848  Log(ss.str());
3849 
3850  int read_messages = m_idriver->receive_message(rxBuffer, 1, TOUT);
3851  for(int i=0; i<read_messages; i++)
3852  {
3853  if (rxBuffer[i].getData()[0]==0x22)
3854  {
3855  //yDebug("strain2-amplifier-tuning: STEP-3. received message = [%x, %x, %x, %x, %x, %x, %x, %x]", rxBuffer[0].getData()[0], rxBuffer[0].getData()[1], rxBuffer[0].getData()[2], rxBuffer[0].getData()[3], rxBuffer[0].getData()[4], rxBuffer[0].getData()[5], rxBuffer[0].getData()[6], rxBuffer[0].getData()[7]);
3856 
3857  //dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
3858  uint8_t noisychannelmask = rxBuffer[i].getData()[2];
3859  uint8_t algorithmOKmask = rxBuffer[i].getData()[3];
3860  uint8_t finalmeasureOKmask = rxBuffer[i].getData()[4];
3861  uint16_t mae = (static_cast<uint32_t>(rxBuffer[i].getData()[6])) |
3862  (static_cast<uint32_t>(rxBuffer[i].getData()[7]) << 8);
3863 
3864  if((okmask == algorithmOKmask) && (okmask == finalmeasureOKmask))
3865  {
3866  ss.str("");
3867  ss.clear();
3868  ss << " OK w/ MAE = " << std::to_string(mae);
3869  Log(ss.str());
3870 
3871  if(0 != noisychannelmask)
3872  {
3873  ss.str("");
3874  ss.clear();
3875  ss << " BUT noisy acquisition of samples: ";
3876  if((0x40 & noisychannelmask) == 0x40)
3877  {
3878  ss << " in computing average ADC before algorithm ";
3879  }
3880  if((0x80 & noisychannelmask) == 0x80)
3881  {
3882  ss << "after algorithm in computing MAE";
3883  }
3884  Log(ss.str());
3885 
3886  ss.str("");
3887  ss.clear();
3888  char tmp[256] = {0};
3889  snprintf(tmp, sizeof(tmp), "noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
3890  ss << "COMPLETE RES: " << tmp;
3891  Log(ss.str());
3892  }
3893 
3894  }
3895  else
3896  {
3897  ss.str("");
3898  ss.clear();
3899  ss << " KO: ";
3900  char tmp[256] = {0};
3901  snprintf(tmp, sizeof(tmp), "noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
3902  ss << tmp;
3903  Log(ss.str());
3904 
3905  if(0 != noisychannelmask)
3906  {
3907  ss.str("");
3908  ss.clear();
3909  ss << " WITH noisy acquisition of samples: ";
3910  if((0x40 & noisychannelmask) == 0x40)
3911  {
3912  ss << " in computing average ADC before algorithm ";
3913  }
3914  if((0x80 & noisychannelmask) == 0x80)
3915  {
3916  ss << "after algorithm in computing MAE";
3917  }
3918  Log(ss.str());
3919  }
3920 
3921  for(uint8_t channel=0; channel<NUMofCHANNELS; channel++)
3922  {
3923  ss.str("");
3924  ss.clear();
3925  bool problems = false;
3926  ss << "- on ch " << std::to_string(channel) << ":";
3927  if((algorithmOKmask & (0x01<<channel)) == 0)
3928  {
3929  problems = true;
3930  ss << " [algorithm fails]";
3931  }
3932  if((finalmeasureOKmask & (0x01<<channel)) == 0)
3933  {
3934  problems = true;
3935  ss << " [mae is high (does ADC work?)]";
3936  }
3937  if(((noisychannelmask) & (0x01<<channel)) == (0x01<<channel))
3938  {
3939  problems = true;
3940  ss << " [noisy acquition]";
3941  }
3942  if(!problems)
3943  {
3944  ss << " [no detected problem]";
3945  }
3946  Log(ss.str());
3947  }
3948  }
3949  break;
3950  }
3951  }
3952  }
3953  else
3954  {
3955  // for all six channels
3956  ss.str("");
3957  ss.clear();
3958  ss << "STEP-2. there are multiple ADC targets: performing regularization channel by channel";
3959  Log(ss.str());
3960 
3961  ss.str("");
3962  ss.clear();
3963  ss << " common params: tolerance = " << std::to_string(tolerance) << " samples2average = " << std::to_string(samples2average);
3964  Log(ss.str());
3965 
3966 
3967  for(int channel=0; channel<NUMofCHANNELS; channel++)
3968  {
3969  channel2autocalib = channel;
3970  middle_val = 32768 + targets[channel];
3971  okmask = 0x01 << channel;
3972 
3973  ss.str("");
3974  ss.clear();
3975  ss << "- on ch " << std::to_string(channel) << ": ADC target = " << std::to_string(targets[channel]);
3976  Log(ss.str());
3977 
3978 
3979  // send message, check results ...
3980 
3981  // sending an autocalib message
3982  txBuffer[0].setId((2 << 8) + target_id);
3983  txBuffer[0].setLen(8);
3984  txBuffer[0].getData()[0]= 0x22;
3985  txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel2autocalib & 0x0f);
3986  txBuffer[0].getData()[2]= 0; // mode oneshot, the only possible so far
3987  txBuffer[0].getData()[3]= middle_val & 0x00ff; // little endian
3988  txBuffer[0].getData()[4]= (middle_val >> 8) & 0x00ff; // little endian
3989  txBuffer[0].getData()[5]= tolerance & 0x00ff; // little endian
3990  txBuffer[0].getData()[6]= (tolerance >> 8) & 0x00ff; // little endian
3991  txBuffer[0].getData()[7]= samples2average;
3992  set_bus(txBuffer[0], bus);
3993  int ret = m_idriver->send_message(txBuffer, 1);
3994  // check if send_message was successful
3995  if (ret==0)
3996  {
3997  if(_verbose) yError ("Unable to send message\n");
3998  return -1;
3999  }
4000 
4001  // now wait for a reply for most 3 seconds
4002  double TOUT = 3.0;
4003 
4004 
4005  int read_messages = m_idriver->receive_message(rxBuffer, 1, TOUT);
4006  for(int i=0; i<read_messages; i++)
4007  {
4008  if (rxBuffer[i].getData()[0]==0x22)
4009  {
4010  //yDebug("strain2-amplifier-tuning: STEP-3. received message = [%x, %x, %x, %x, %x, %x, %x, %x]", rxBuffer[0].getData()[0], rxBuffer[0].getData()[1], rxBuffer[0].getData()[2], rxBuffer[0].getData()[3], rxBuffer[0].getData()[4], rxBuffer[0].getData()[5], rxBuffer[0].getData()[6], rxBuffer[0].getData()[7]);
4011 
4012  //dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
4013  uint8_t noisychannelmask = rxBuffer[i].getData()[2];
4014  uint8_t algorithmOKmask = rxBuffer[i].getData()[3];
4015  uint8_t finalmeasureOKmask = rxBuffer[i].getData()[4];
4016  uint16_t mae = (static_cast<uint32_t>(rxBuffer[i].getData()[6])) |
4017  (static_cast<uint32_t>(rxBuffer[i].getData()[7]) << 8);
4018 
4019  if((okmask == algorithmOKmask) && (okmask == finalmeasureOKmask))
4020  {
4021  ss.str("");
4022  ss.clear();
4023  ss << " OK w/ MAE = " << std::to_string(mae);
4024  Log(ss.str());
4025 
4026  if(0 != (noisychannelmask & okmask))
4027  {
4028  ss.str("");
4029  ss.clear();
4030  ss << " BUT noisy acquisition of samples: ";
4031  Log(ss.str());
4032 
4033  if((0x40 & noisychannelmask) == 0x40)
4034  {
4035  ss.str("");
4036  ss.clear();
4037  ss << " - in computing average ADC before algorithm ";
4038  Log(ss.str());
4039  }
4040  if((0x80 & noisychannelmask) == 0x80)
4041  {
4042  ss.str("");
4043  ss.clear();
4044  ss << " - after algorithm in computing MAE";
4045  Log(ss.str());
4046  }
4047 
4048  ss.str("");
4049  ss.clear();
4050  char tmp[256] = {0};
4051  snprintf(tmp, sizeof(tmp), "noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
4052  ss << "COMPLETE RES: " << tmp;
4053  Log(ss.str());
4054  }
4055 
4056  }
4057  else
4058  {
4059 
4060  ss.str("");
4061  ss.clear();
4062  ss << " KO /w MAE = " << std::to_string(mae) << " because: ";
4063  Log(ss.str());
4064 
4065  ss.str("");
4066  ss.clear();
4067  char tmp[256] = {0};
4068  snprintf(tmp, sizeof(tmp), " KO details: noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
4069  ss << tmp;
4070  Log(ss.str());
4071 
4072  bool problems = false;
4073  if((algorithmOKmask & okmask) == 0)
4074  {
4075  problems = true;
4076  ss.str("");
4077  ss.clear();
4078  ss << " - algorithm fails";
4079  Log(ss.str());
4080  }
4081  if((finalmeasureOKmask & okmask) == 0)
4082  {
4083  problems = true;
4084  ss.str("");
4085  ss.clear();
4086  ss << " - mae is high (does ADC work?)";
4087  Log(ss.str());
4088  }
4089  if(!problems)
4090  {
4091  ss.str("");
4092  ss.clear();
4093  ss << " .. strange: no detected problem";
4094  Log(ss.str());
4095  }
4096 
4097  }
4098  break;
4099  }
4100  }
4101  }
4102  }
4103 
4104  return 0;
4105 }
4106 
4107 int cDownloader::readADC(int bus, int target_id, int channel, int nmeasures)
4108 {
4109  const int type = 0; // raw
4110 
4111  txBuffer[0].setId((2 << 8) + target_id);
4112  txBuffer[0].setLen(3);
4113  txBuffer[0].getData()[0]= 0x0C;
4114  txBuffer[0].getData()[1]= channel;
4115  txBuffer[0].getData()[2]= type;
4116 
4117  int measure = 0;
4118 
4119  for(int n=0; n<nmeasures; n++)
4120  {
4121  int tmp = 0;
4122  set_bus(txBuffer[0], bus);
4123  int ret = m_idriver->send_message(txBuffer, 1);
4124  // check if send_message was successful
4125  if (ret==0)
4126  {
4127  yError ("Unable to send message\n");
4128  return 0;
4129  }
4130  //read adc
4131  int read_messages = m_idriver->receive_message(rxBuffer,1);
4132  for (int i=0; i<read_messages; i++)
4133  {
4134  if (rxBuffer[i].getData()[0]==0x0C)
4135  {
4136  tmp = (rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4]);
4137  break;
4138  }
4139  else
4140  {
4141  printf("cDownloader::strain_calibrate_offset2_strain1(): fails in reading reply for a measure\n");
4142  }
4143  }
4144 
4145  measure += tmp;
4146  }
4147 
4148  measure /= nmeasures;
4149 
4150  return measure;
4151 }
4152 
4153 int cDownloader::strain_calibrate_offset2_strain1safer (int bus, int target_id, int16_t t, uint8_t nmeasures, bool fullsearch, string *errorstring)
4154 {
4155  // check if driver is running
4156  if (m_idriver == NULL)
4157  {
4158  if(_verbose) yError ("Driver not ready\n");
4159  return -1;
4160  }
4161 
4162  std::ostringstream ss;
4163 
4164  // marco.accame: transform [-32K, +32K) into [0, +64K)
4165  unsigned int middle_val = 32768 + t;
4166 
4167 
4168  if(fullsearch)
4169  {
4170  //yDebug() << "performing full search in dac space to find best value to match adc ="<< t << " using" << nmeasures << "adc acquisions for better averaging";
4171 
4172  ss.str("");
4173  ss.clear();
4174  ss << "performing offset autotuning for strain1 in full search mode";
4175  Log(ss.str());
4176 
4177  ss.str("");
4178  ss.clear();
4179  ss << "params: " << "adc target =" << std::to_string(t) << " using" << std::to_string(nmeasures) << "adc acquisions for better averaging";
4180  Log(ss.str());
4181 
4182  for(int channel=0; channel<6; channel++)
4183  {
4184 
4185  long minABSerror = 128*1024;
4186  unsigned int minDAC = 0;
4187 
4188  // loop over all possible dac values
4189  for(unsigned int testdac=0; testdac<1024; testdac++)
4190  {
4191  // send new dac
4192  strain_set_offset(bus, target_id, channel, testdac);
4193  //wait
4194  drv_sleep(3);
4195  // verify it
4196  unsigned int tmp = 0;
4197  strain_get_offset(bus, target_id, channel, tmp);
4198  if(tmp != testdac)
4199  {
4200  yError() << "failed to impose DAC = " << testdac << "read:" << tmp;
4201  }
4202 
4203  // read value
4204  int measure = readADC(bus, target_id, channel, nmeasures);
4205  // compute error vs target
4206  long error = long(measure) - long(middle_val);
4207 
4208  if(fabs(error) < fabs(minABSerror))
4209  {
4210  minABSerror = fabs(error);
4211  minDAC = testdac;
4212  //yDebug() << "PROGESS: channel =" << channel << "minerror = " << minABSerror << "mindac =" << minDAC;
4213  }
4214  }
4215 
4216  // apply the best dac
4217  strain_set_offset(bus, target_id, channel, minDAC);
4218  // wait
4219  drv_sleep(3);
4220  // verify it
4221  unsigned int tmp = 0;
4222  strain_get_offset(bus, target_id, channel, tmp);
4223  if(tmp != minDAC)
4224  {
4225  yError() << "failed to impose DAC = " << minDAC << "read:" << tmp;
4226  }
4227 
4228  ss.str("");
4229  ss.clear();
4230  ss << "RESULT of FULL SEARCH w/ nsamples average =" << std::to_string(nmeasures) << "-> channel =" << std::to_string(channel) << "minerror = " << std::to_string(minABSerror) << "mindac =" << std::to_string(minDAC);
4231  Log(ss.str());
4232 
4233  //yDebug() << "RESULT of FULL SEARCH w/ nsamples average =" << nmeasures << "-> channel =" << channel << "minerror = " << minABSerror << "mindac =" << minDAC;
4234 
4235  } // channel
4236 
4237  }
4238  else
4239  {
4240  const int daclimit = 0x3ff;
4241  const int dacstep = 1;
4242  const long tolerance = 128;
4243  const int maxiterations = 1024;
4244 
4245  ss.str("");
4246  ss.clear();
4247  ss << "performing gradient descend in dac space to find best value to match adc ="<< std::to_string(t) << " using" << std::to_string(nmeasures) << "adc acquisions for better averaging";
4248  Log(ss.str());
4249 
4250  ss.str("");
4251  ss.clear();
4252  ss << "exit conditions: max number of iterations =" << std::to_string(maxiterations) << "error tolerance =" << std::to_string(tolerance);
4253  Log(ss.str());
4254 
4255  for(int channel=0; channel<6; channel++)
4256  {
4257 
4258  long minABSerror = 128*1024;
4259  unsigned int minDAC = 0;
4260  unsigned int dac = 0;
4261 
4262 
4263  int measure = readADC(bus, target_id, channel, nmeasures);
4264 
4265  // read dac
4266  strain_get_offset(bus, target_id, channel, dac);
4267 
4268  long error = long(measure) - long(middle_val);
4269  int cycle =0;
4270 
4271  minABSerror = fabs(error);
4272  minDAC = dac;
4273 
4274  // now i perform a sort of gradient descend
4275  while ((abs(error)>tolerance) && (cycle<daclimit) && (cycle<maxiterations))
4276  {
4277  if (error>0) dac -= dacstep;
4278  else dac += dacstep;
4279 
4280  if (dac>daclimit) dac = daclimit;
4281  if (dac<0) dac = 0;
4282 
4283  //yDebug() << "channel =" << channel << "iter =" << cycle << "err = " << error << "next dac =" << dac << "minerror = " << minABSerror << "mindac =" << minDAC;
4284 
4285  // send new dac
4286  strain_set_offset(bus, target_id, channel, dac);
4287  //wait
4288  drv_sleep(3);
4289  // verify it
4290  unsigned int tmp = 0;
4291  strain_get_offset(bus, target_id, channel, tmp);
4292  if(tmp != dac)
4293  {
4294  yError() << "failed to impose DAC = " << dac << "read:" << tmp;
4295  }
4296 
4297  // read value
4298  measure = readADC(bus, target_id, channel, nmeasures);
4299 
4300  error = long(measure) - long(middle_val);
4301  cycle++;
4302 
4303  if(fabs(error) < fabs(minABSerror))
4304  {
4305  minABSerror = fabs(error);
4306  minDAC = dac;
4307  }
4308  }
4309 
4310  ss.str("");
4311  ss.clear();
4312  ss << "RESULT of gradient descend w/ nsamples average =" << std::to_string(nmeasures) << " -> channel =" << std::to_string(channel) << " num iters =" << std::to_string(cycle) << " err = " << std::to_string(error) << " applied dac =" << std::to_string(dac) << " minerror = " << std::to_string(minABSerror) << " mindac =" << std::to_string(minDAC);
4313  Log(ss.str());
4314 
4315  } // channel
4316 
4317  }
4318 
4319 
4320  return 0;
4321 }
4322 
4323 
4324 
4325 
4326 
4327 // eof
4328 
4329 
4330 
tolerance
constexpr double tolerance
Definition: WholeBodyPlayerModule.h:62
CanPacket::everyCANbus
@ everyCANbus
Definition: driver.h:51
CanPacket::setCanBus
void setCanBus(unsigned int bus)
Definition: driver.h:88
CanPacket::getCanBus
int getCanBus() const
Definition: driver.h:87
cDownloader::open_file
int open_file(std::string file)
Definition: downloader.cpp:3425
cDownloader::strain_get_serial_number
int strain_get_serial_number(int bus, int target_id, char *serial_number, string *errorstring=NULL)
Definition: downloader.cpp:672
cDownloader::strain_set_matrix_gain
int strain_set_matrix_gain(int bus, int target_id, unsigned int gain, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:891
cDownloader::strain_acquire_start
int strain_acquire_start(int bus, int target_id, uint8_t txratemilli=20, bool calibmode=true, strain_acquisition_mode_t acqmode=strain_acquisition_mode_streaming, string *errorstring=NULL)
Definition: downloader.cpp:1351
cDownloader::strain_get_offset
int strain_get_offset(int bus, int target_id, char channel, unsigned int &offset, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:399
cDownloader::change_card_address
int change_card_address(int bus, int target_id, int new_id, int board_type)
Definition: downloader.cpp:2414
cDownloader::strain_get_full_scale
int strain_get_full_scale(int bus, int target_id, unsigned char channel, unsigned int &full_scale, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1184
cDownloader::strain_get_calib_bias
int strain_get_calib_bias(int bus, int target_id, char channel, signed int &bias, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:444
cDownloader::strain_reset_curr_bias
int strain_reset_curr_bias(int bus, int target_id, string *errorstring=NULL)
Definition: downloader.cpp:622
cDownloader::get_canbus_id
int get_canbus_id()
Definition: downloader.cpp:2734
cDownloader::get_board_info
int get_board_info(int bus, int target_id, char *board_info)
Definition: downloader.cpp:2282
cDownloader::strain_set_offset
int strain_set_offset(int bus, int target_id, char channel, unsigned int offset, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1316
cDownloader::startscheda
int startscheda(int bus, int board_pid, bool board_eeprom, int download_type)
Definition: downloader.cpp:2746
cDownloader::strain_set_serial_number
int strain_set_serial_number(int bus, int target_id, const char *serial_number, string *errorstring=NULL)
Definition: downloader.cpp:644
cDownloader::strain_acquire_stop
int strain_acquire_stop(int bus, int target_id, strain_acquisition_mode_t acqmode=strain_acquisition_mode_streaming, string *errorstring=NULL)
Definition: downloader.cpp:1404
cDownloader::strain_set_amplifier_discretegain
int strain_set_amplifier_discretegain(int bus, int target_id, unsigned char channel, strain2_ampl_discretegain_t ampset, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1009
cDownloader::strain_get_regulationset
int strain_get_regulationset(int bus, int target_id, int &regset, const int regsetmode=strain_regsetmode_temporary, string *errorstring=NULL)
Definition: downloader.cpp:837
cDownloader::stopscheda
int stopscheda(int bus, int board_pid)
Definition: downloader.cpp:2843
cDownloader::strain_save_to_eeprom
int strain_save_to_eeprom(int bus, int target_id, string *errorstring=NULL)
Definition: downloader.cpp:214
cDownloader::set_verbose
void set_verbose(bool verbose)
Definition: downloader.cpp:98
cDownloader::strain_acquire_get
int strain_acquire_get(int bus, int target_id, vector< strain_value_t > &values, const unsigned int howmany=10, void(*updateProgressBar)(void *, float)=NULL, void *arg=NULL, strain_acquisition_mode_t acqmode=strain_acquisition_mode_streaming, const unsigned int maxerrors=1, string *errorstring=NULL)
Definition: downloader.cpp:1450
cDownloader::change_board_info
int change_board_info(int bus, int target_id, char *board_info)
Definition: downloader.cpp:2365
cDownloader::strain_get_adc
int strain_get_adc(int bus, int target_id, char channel, unsigned int &adc, int type, string *errorstring=NULL)
Definition: downloader.cpp:348
cDownloader::strain_stop_sampling
int strain_stop_sampling(int bus, int target_id, string *errorstring=NULL)
Definition: downloader.cpp:1659
cDownloader::cDownloader
cDownloader(bool verbose=true)
Definition: downloader.cpp:87
cDownloader::strain_acquisition_mode_t
strain_acquisition_mode_t
Definition: downloader.h:257
cDownloader::initdriver
int initdriver(yarp::os::Searchable &config, bool verbose=true)
Definition: downloader.cpp:142
cDownloader::set_canbus_id
void set_canbus_id(int id)
Definition: downloader.cpp:2739
cDownloader::strain_get_eeprom_saved
int strain_get_eeprom_saved(int bus, int target_id, bool *status, string *errorstring=NULL)
Definition: downloader.cpp:713
cDownloader::get_firmware_version
int get_firmware_version(int bus, int target_id, eObrd_cantype_t boardtype, eObrd_info_t *info, bool &noreply)
Definition: downloader.cpp:2155
cDownloader::get_serial_no
int get_serial_no(int bus, int target_id, char *board_info)
Definition: downloader.cpp:2123
cDownloader::strain_reset_calib_bias
int strain_reset_calib_bias(int bus, int target_id, string *errorstring=NULL)
Definition: downloader.cpp:524
cDownloader::strain_get_matrix_gain
int strain_get_matrix_gain(int bus, int target_id, unsigned int &gain, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:746
cDownloader::strain_get_matrix_rc
int strain_get_matrix_rc(int bus, int target_id, char r, char c, unsigned int &elem, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1245
cDownloader::set_external_logger
void set_external_logger(void *caller=NULL, void(*logger)(void *, const std::string &)=NULL)
Definition: downloader.cpp:103
cDownloader::strain_set_full_scale
int strain_set_full_scale(int bus, int target_id, unsigned char channel, unsigned int full_scale, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1221
cDownloader::stopdriver
int stopdriver()
Definition: downloader.cpp:122
cDownloader::strain_set_regulationset
int strain_set_regulationset(int bus, int target_id, int regset=strain_regset_one, int regsetmode=strain_regsetmode_temporary, string *errorstring=NULL)
Definition: downloader.cpp:804
cDownloader::strain_set_calib_bias
int strain_set_calib_bias(int bus, int target_id, string *errorstring=NULL)
Definition: downloader.cpp:476
cDownloader::strain_get_amplifier_regs
int strain_get_amplifier_regs(int bus, int target_id, unsigned char channel, strain2_ampl_regs_t &ampregs, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:947
cDownloader::strain_set_curr_bias
int strain_set_curr_bias(int bus, int target_id, string *errorstring=NULL)
Definition: downloader.cpp:575
cDownloader::strain_get_curr_bias
int strain_get_curr_bias(int bus, int target_id, char channel, signed int &bias, string *errorstring=NULL)
Definition: downloader.cpp:545
cDownloader::strain_start_sampling
int strain_start_sampling(int bus, int target_id, string *errorstring=NULL)
Definition: downloader.cpp:1633
cDownloader::initschede
int initschede()
Definition: downloader.cpp:2485
cDownloader::download_file
int download_file(int bus, int board_pid, int download_type, bool eeprom)
Definition: downloader.cpp:3463
cDownloader::strain_get_amplifier_gain_offset
int strain_get_amplifier_gain_offset(int bus, int target_id, unsigned char channel, float &gain, uint16_t &offset, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1088
cDownloader::strain_set_matrix_rc
int strain_set_matrix_rc(int bus, int target_id, char r, char c, unsigned int elem, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1291
cDownloader::strain_set_amplifier_regs
int strain_set_amplifier_regs(int bus, int target_id, unsigned char channel, const strain2_ampl_regs_t &ampregs, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:913
cDownloader::strain_set_amplifier_gain_offset
int strain_set_amplifier_gain_offset(int bus, int target_id, unsigned char channel, float gain, uint16_t offset, int regset=strain_regset_inuse, string *errorstring=NULL)
Definition: downloader.cpp:1127
cDownloader::strain_amplifier_discretegain2float
float strain_amplifier_discretegain2float(strain2_ampl_discretegain_t c)
Definition: downloader.cpp:992
cDownloader::strain_calibrate_offset2
int strain_calibrate_offset2(int bus, int target_id, icubCanProto_boardType_t boardtype, const std::vector< strain2_ampl_discretegain_t > &gains, const std::vector< int16_t > &targets, string *errorstring=NULL)
Definition: downloader.cpp:1699
cDownloader::strain_calibrate_offset
int strain_calibrate_offset(int bus, int target_id, icubCanProto_boardType_t boardtype, unsigned int middle_val, string *errorstring=NULL)
Definition: downloader.cpp:1718
iDriver2::eth_driver2
@ eth_driver2
Definition: driver.h:97
strain::amplifier::PGA308::Registers::fill
virtual bool fill(void *data, size_t &size)
Definition: strain.cpp:735
strain::amplifier::PGA308::import
bool import(const Registers &regs, WideParams *wideparams=nullptr)
Definition: strain.cpp:808
x
x
Definition: compute_ekf_sym.m:21
n
int n
Definition: debugFunctions.cpp:58
_NUMofREGS
#define _NUMofREGS
drv_sleep
void drv_sleep(double time)
Definition: downloader.cpp:26
axtoi
int axtoi(char *hexStg)
Definition: downloader.cpp:33
EOCANPROT_D_CREATE_CANID
#define EOCANPROT_D_CREATE_CANID(clss, orig, dest)
Definition: downloader.cpp:2152
getvalue
int getvalue(char *line, int len)
Definition: downloader.cpp:2892
ID_MASTER
#define ID_MASTER
Definition: downloader.h:65
SPRS_STATE_ADDRESS
#define SPRS_STATE_ADDRESS
Definition: downloader.h:54
SPRS_TYPE_3
#define SPRS_TYPE_3
Definition: downloader.h:61
SPRS_STATE_DATA
#define SPRS_STATE_DATA
Definition: downloader.h:56
BOARD_WAITING_ACK
#define BOARD_WAITING_ACK
Definition: downloader.h:47
SPRS_STATE_WAIT
#define SPRS_STATE_WAIT
Definition: downloader.h:52
SPRS_STATE_CHECKSUM
#define SPRS_STATE_CHECKSUM
Definition: downloader.h:57
SPRS_TYPE_4
#define SPRS_TYPE_4
Definition: downloader.h:62
SPRS_STATE_LENGTH
#define SPRS_STATE_LENGTH
Definition: downloader.h:55
BOARD_RUNNING
#define BOARD_RUNNING
Definition: downloader.h:45
BOARD_DOWNLOADING
#define BOARD_DOWNLOADING
Definition: downloader.h:48
SPRS_TYPE_0
#define SPRS_TYPE_0
Definition: downloader.h:59
SPRS_TYPE_1
#define SPRS_TYPE_1
Definition: downloader.h:60
BOARD_WAITING
#define BOARD_WAITING
Definition: downloader.h:46
SPRS_STATE_TYPE
#define SPRS_STATE_TYPE
Definition: downloader.h:53
ID_BROADCAST
#define ID_BROADCAST
Definition: downloader.h:66
strain2_ampl_discretegain_t
strain2_ampl_discretegain_t
Definition: downloader.h:105
ampl_gain08
@ ampl_gain08
Definition: downloader.h:107
ampl_gain24
@ ampl_gain24
Definition: downloader.h:106
ampl_gain10
@ ampl_gain10
Definition: downloader.h:107
SPRS_TYPE_7
#define SPRS_TYPE_7
Definition: downloader.h:63
MAX_READ_MSG
#define MAX_READ_MSG
Definition: driver.h:40
done
bool done
Definition: main.cpp:42
file
FILE * file
Definition: main.cpp:81
strain::amplifier::convert
Gain convert(const DiscreteGain dg)
Definition: strain.cpp:673
strain::amplifier::mapofgains
static const float mapofgains[static_cast< uint8_t >(DiscreteGain::maxnumberof)]
Definition: strain.cpp:668
fprintf
fprintf(fid,'\n')
z
z
Definition: show_eyes_axes.m:22
y
y
Definition: show_eyes_axes.m:21
offset
degrees offset
Definition: sine.m:4
time
degrees time
Definition: sine.m:5
cDownloader::strain_value_t
Definition: downloader.h:240
cDownloader::strain_value_t::saturationinfo
icubCanProto_strain_saturationInfo_t saturationinfo[6]
Definition: downloader.h:245
cDownloader::strain_value_t::calibrated
bool calibrated
Definition: downloader.h:242
cDownloader::strain_value_t::valid
bool valid
Definition: downloader.h:241
cDownloader::strain_value_t::saturated
bool saturated
Definition: downloader.h:243
cDownloader::strain_value_t::channel
unsigned int channel[6]
Definition: downloader.h:244
sBoard::bus
int bus
Definition: downloader.h:25
strain2_ampl_regs_t
Definition: downloader.h:95
strain2_ampl_regs_t::data
uint8_t data[6]
Definition: downloader.h:96
strain2_ampl_regs_t::memory
void * memory()
Definition: downloader.h:99
strain::amplifier::WideParams::load
void load(Gain _g, Offset _o)
Definition: strain.h:158
Generated on Wed Nov 13 2024 21:11:42 for iCub-main by doxygen 1.9.1