iCub-main
•All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
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
20using namespace yarp::dev;
21using namespace yarp::os;
22using namespace std;
23
24
25//*****************************************************************/
26void 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
33int 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
68int cDownloader::build_id(int source, int dest)
69{
70 return (ICUBCANPROTO_CLASS_BOOTLOADER << 8) + ( source << 4) + dest;
71}
72
73int cDownloader::get_src_from_id (int id)
74{
75 // 000 1111 0000
76 return ((id >> 4) & 0x0F);
77}
78
79int cDownloader::get_dst_from_id (int id)
80{
81 // 000 0000 1111
82 return (id & 0x0F);
83}
84
85//*****************************************************************/
86
87cDownloader::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
98void cDownloader::set_verbose(bool verbose)
99{
100 _verbose = verbose;
101}
102
103void cDownloader::set_external_logger(void *caller, void (*logger)(void *, const std::string &))
104{
105 _externalLoggerFptr = logger;
106 _externalLoggerCaller = caller;
107}
108
109void 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
122int 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
142int 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//*****************************************************************/
214int 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//*****************************************************************/
348int 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//*****************************************************************/
399int 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//*****************************************************************/
444int 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//*****************************************************************/
476int 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//*****************************************************************/
497int 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//*****************************************************************/
524int 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//*****************************************************************/
545int 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//*****************************************************************/
575int 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//*****************************************************************/
597int 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//*****************************************************************/
622int 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//*****************************************************************/
644int 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//*****************************************************************/
672int 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//*****************************************************************/
713int 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//*****************************************************************/
746int 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
804int 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
837int 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//*****************************************************************/
891int 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
913int 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
947int 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
992float 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
1009int 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
1088int 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
1127int 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//*****************************************************************/
1184int 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//*****************************************************************/
1221int 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//*****************************************************************/
1245int 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//*****************************************************************/
1291int 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//*****************************************************************/
1316int 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
1351int 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
1404int 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
1450int 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//*****************************************************************/
1633int 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//*****************************************************************/
1659int 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
1699int 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//*****************************************************************/
1718int 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//*****************************************************************/
2123int 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
2155int 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//*****************************************************************/
2282int 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//*****************************************************************/
2365int 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//*****************************************************************/
2414int 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
2485int 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 if (rxBuffer[i].getLen()==4)
2635 board_list[j].appl_vers_build = -1;
2636 else
2637 board_list[j].appl_vers_build = rxBuffer[i].getData()[4];
2638 board_list[j].status = BOARD_RUNNING;
2639 board_list[j].selected = false;
2640 board_list[j].eeprom =false;
2641 memset (board_list[j].serial, 0, 8);
2642 memset (board_list[j].add_info, 0, 32);
2643 j++;
2644 }
2645 }
2646
2647 //if(_verbose) yDebug ("about to ask boardinfo \n");
2648
2649 for (i=0; i<board_list_size; i++)
2650 {
2651 char board_info [32];
2652 get_board_info (board_list[i].bus, board_list[i].pid, board_info);
2653 strcpy (board_list[i].add_info, board_info);
2654 //pause
2655 drv_sleep(10);
2656 }
2657
2658
2659 //if(_verbose) yDebug ("about to ask serialno \n");
2660
2661 for (i=0; i<board_list_size; i++)
2662 {
2663 char serial_no [32];
2664 get_serial_no (board_list[i].bus, board_list[i].pid, serial_no);
2665 strcpy (board_list[i].serial, serial_no);
2666 if(0 == strlen(board_list[i].serial))
2667 {
2668 snprintf(board_list[i].serial, sizeof(board_list[i].serial), "N/A");
2669 }
2670 //pause
2671 drv_sleep(10);
2672 }
2673
2674 for (i=0; i<board_list_size; i++)
2675 {
2676 board_list[i].strainregsetinuse = board_list[i].strainregsetatboot = 0;
2677 if(board_list[i].type==icubCanProto_boardType__strain)
2678 {
2679 board_list[i].strainregsetinuse = board_list[i].strainregsetatboot = 1;
2680 }
2681 else if(board_list[i].type==icubCanProto_boardType__strain2 || board_list[i].type==icubCanProto_boardType__strain2c)
2682 {
2683 this->strain_get_regulationset(board_list[i].bus, board_list[i].pid, board_list[i].strainregsetinuse, strain_regsetmode_temporary);
2684 this->strain_get_regulationset(board_list[i].bus, board_list[i].pid, board_list[i].strainregsetatboot, strain_regsetmode_permanent);
2685 }
2686 //pause
2687 drv_sleep(10);
2688 }
2689
2690#define TEST_GET_FW_VERSION
2691
2692#if defined(TEST_GET_FW_VERSION)
2693
2694 if(_verbose) yDebug ("about to ask fw version \n");
2695 for(i=0; i<board_list_size; i++)
2696 {
2697 // marco.accame on 25 may 2016: i have added this code for demostration of how we can use the get_firmware_version() function.
2698 // this info is useful for the new fwUpdater. Moreover, it is a good way to further verify if we are in bootloader or not.
2699 // the bootloader does not reply to get-fw-version, whereas the applications replies.
2700 // The only known exception is the mtb application which replies o get-fw-version only after ... somewhere in early 2016.
2701 eObrd_info_t info = {0};
2702 memset(&info, 0, sizeof(info));
2703 bool noreply = true;
2704 int rr = get_firmware_version(board_list[i].bus, board_list[i].pid, (eObrd_cantype_t)board_list[i].type, &info, noreply);
2705 if(_verbose)
2706 {
2707 fprintf(stderr, "board %d: ret = %d, reply = %d, type = %d, f=(%d, %d, %d), pr=(%d, %d)\n", i, rr, !noreply,
2708 info.type,
2709 info.firmware.major, info.firmware.minor, info.firmware.build,
2710 info.protocol.major, info.protocol.minor);
2711 }
2712 board_list[i].prot_vers_major = info.protocol.major;
2713 board_list[i].prot_vers_minor = info.protocol.minor;
2714 drv_sleep(10);
2715 }
2716#endif
2717
2718
2719 if(_verbose) yInfo("CONNECTED: %d Boards",board_list_size);
2720 if(_verbose) yDebug(" BUS:id type version");
2721 for (int i = 0; i < board_list_size; i++)
2722 {
2723 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);
2724 }
2725
2726 return 0;
2727}
2728
2729
2730//*****************************************************************/
2731int cDownloader::get_canbus_id()
2732{
2733 return canbus_id;
2734}
2735
2736void cDownloader::set_canbus_id(int id)
2737{
2738 canbus_id = id;
2739}
2740
2741//*****************************************************************/
2742
2743int cDownloader::startscheda(int bus, int board_pid, bool board_eeprom, int board_type)
2744{
2745 // check if driver is running
2746 if (m_idriver == NULL)
2747 {
2748 if(_verbose) yError ("START_CMD: Driver not ready\n");
2749 return -1;
2750 }
2751
2752 switch (board_type)
2753 {
2754 case icubCanProto_boardType__dsp:
2755 case icubCanProto_boardType__pic:
2756 case icubCanProto_boardType__2dc:
2757 case icubCanProto_boardType__4dc:
2758 case icubCanProto_boardType__bll:
2759 {
2760 // Send command
2761 txBuffer[0].setId(build_id(ID_MASTER, board_pid));
2762 txBuffer[0].setLen(1);
2763 txBuffer[0].getData()[0]= ICUBCANPROTO_BL_BOARD;
2764
2765 //makes the first jump
2766 set_bus(txBuffer[0], bus);
2767 m_idriver->send_message(txBuffer, 1);
2768 drv_sleep(250);
2769 }
2770 break;
2771 case icubCanProto_boardType__skin:
2772 case icubCanProto_boardType__strain:
2773 case icubCanProto_boardType__mais:
2774 case icubCanProto_boardType__2foc:
2775 case icubCanProto_boardType__6sg:
2776 case icubCanProto_boardType__jog:
2777 case icubCanProto_boardType__mtb4:
2778 case icubCanProto_boardType__strain2:
2779 case icubCanProto_boardType__rfe:
2780 case icubCanProto_boardType__sg3:
2781 case icubCanProto_boardType__psc:
2782 case icubCanProto_boardType__mtb4w:
2783 case icubCanProto_boardType__mtb4c:
2784 case eobrd_cantype_pmc:
2785 case eobrd_cantype_amcbldc:
2786 case eobrd_cantype_strain2c:
2787 case icubCanProto_boardType__unknown:
2788 {
2789 // Send command
2790 txBuffer[0].setId(build_id(ID_MASTER,board_pid));
2791 txBuffer[0].setLen(2);
2792 txBuffer[0].getData()[0]= ICUBCANPROTO_BL_BOARD;
2793 txBuffer[0].getData()[1]= (int) board_eeprom;
2794
2795 //makes the first jump
2796 set_bus(txBuffer[0], bus);
2797 m_idriver->send_message(txBuffer, 1);
2798 drv_sleep(1500);
2799 }
2800 break;
2801 }
2802 set_bus(txBuffer[0], bus);
2803 int ret = m_idriver->send_message(txBuffer, 1);
2804
2805 // check if send_message was successful
2806 if (ret==0)
2807 {
2808 if(_verbose) yError ("START_CMD: Unable to send message\n");
2809 return -1;
2810 }
2811 // marco.accame: wait some more time (it was 500 ms) to wait amcbldc and pmc boards to erase their flash
2812 drv_sleep(2000);
2813
2814 // riceve la risposta
2815 int read_messages = m_idriver->receive_message(rxBuffer);
2816
2817
2818 //One (or more) answers received
2819 for (int i=0; i<read_messages; i++)
2820 {
2821 if (rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_BOARD &&
2822 (((rxBuffer[i].getId() >> 4) & 0x0F) == board_pid) &&
2823 (((rxBuffer[i].getId() >> 8) & 0x07) == ICUBCANPROTO_CLASS_BOOTLOADER))
2824 {
2825 //received ACK from board
2826 //printf ("START_CMD: ACK received from board: %d\n", board_pid);
2827 return 0;
2828 }
2829 }
2830 // return 0; //DEBUG
2831
2832 //ERROR
2833 if(_verbose) yError ("START_CMD: No ACK received from board %d\n", board_pid);
2834 return -1;
2835
2836}
2837
2838//*****************************************************************/
2839
2840int cDownloader::stopscheda(int bus, int board_pid)
2841{
2842 // check if driver is running
2843 if (m_idriver == NULL)
2844 {
2845 if(_verbose) yError ("STOP_CMD: Driver not ready\n");
2846 return -1;
2847 }
2848
2849 // Send command
2850 txBuffer[0].setId(build_id(ID_MASTER, board_pid));
2851 txBuffer[0].setLen(1);
2852 txBuffer[0].getData()[0]= ICUBCANPROTO_BL_END;
2853 set_bus(txBuffer[0], bus);
2854 int ret = m_idriver->send_message(txBuffer, 1);
2855
2856 // check if send_message was successful
2857 if (ret==0)
2858 {
2859 if(_verbose) yError ("STOP_CMD: Unable to send message\n");
2860 return -1;
2861 }
2862
2863 //pause
2864 drv_sleep(5);
2865
2866 // riceve la risposta
2867 int read_messages = m_idriver->receive_message(rxBuffer);
2868
2869 //One (or more) answers received
2870 for (int i=0; i<read_messages; i++)
2871 {
2872 if (rxBuffer[i].getData()[0]==ICUBCANPROTO_BL_END &&
2873 (((rxBuffer[i].getId() >> 4) & 0x0F) == board_pid || board_pid == 15 ) &&
2874 (((rxBuffer[i].getId() >> 8) & 0x07) == ICUBCANPROTO_CLASS_BOOTLOADER))
2875 {
2876 //received ACK from board
2877 //printf ("STOP_CMD: ACK received from board: %d\n", board_pid);
2878 return 0;
2879 }
2880 }
2881
2882 //ERROR
2883 if(_verbose) yError ("TOP_CMD: No ACK received from board %d\n", board_pid);
2884 return -1;
2885}
2886
2887//*****************************************************************/
2888
2889int getvalue(char* line, int len)
2890{
2891 char hexconv_buffer[5];
2892 memset (hexconv_buffer, '\0', sizeof(hexconv_buffer) );
2893 strncpy(hexconv_buffer,line,len);
2894 return axtoi (hexconv_buffer);
2895}
2896
2897//*****************************************************************/
2898int cDownloader::verify_ack(int command, int read_messages)
2899{
2900
2901 int i,k;
2902
2903/*
2904 for(int m=0;m<read_messages;m++)
2905 {
2906 fprintf(stderr, "%4x %d %d %d\n",
2907 rxBuffer[m].getId(),
2908 rxBuffer[m].getLen(),
2909 rxBuffer[m].getData()[0],
2910 rxBuffer[m].getData()[1]);
2911 }
2912*/
2913
2914 for (i=0; i<board_list_size; i++)
2915 {
2916 if (board_list[i].selected==true)
2917 if (board_list[i].status == BOARD_WAITING ||
2918 board_list[i].status == BOARD_DOWNLOADING)
2919 {
2920 board_list[i].status = BOARD_WAITING_ACK;
2921
2922 for (k=0; k<read_messages; k++)
2923 {
2924 if ((rxBuffer[k].getData()[0]==command) &&
2925 (rxBuffer[k].getLen() == 2) &&
2926 (rxBuffer[k].getData()[1]==1))
2927 {
2928 if(board_list[i].pid == get_src_from_id(rxBuffer[k].getId()))
2929 {
2930 #if defined(DOWNLOADER_USE_IDRIVER2)
2931 if(board_list[i].bus == rxBuffer[k].getCanBus())
2932 #else
2933 if(1)
2934 #endif
2935 {
2936 board_list[i].status=BOARD_DOWNLOADING;
2937 }
2938 }
2939 }
2940 }
2941 }
2942 }
2943
2944 for (i=0; i<board_list_size; i++)
2945 {
2946 if (board_list[i].selected==true && board_list[i].status == BOARD_WAITING_ACK)
2947 {
2948 //@@@@ board_list[i].status=BOARD_ERR;
2949 return -1;
2950 }
2951 }
2952 return 0;
2953}
2954
2955
2956
2957//*****************************************************************/
2958// Return values:
2959// 0 one line downloaded, continuing the download...
2960// 1 Current downloading, everything OK
2961// -1 Fatal error
2962
2963int cDownloader::download_motorola_line(char* line, int len, int bus, int board_pid)
2964{
2965 static double now;
2966 static double prev;
2967
2968 now=Time::now();
2969 double dT=now-prev;
2970 prev=now;
2971
2972 // fprintf(stderr, "dT:%.2lf [ms]\n", dT*1000);
2973
2974 char sprsRecordType=0;
2975 unsigned long int sprsChecksum=0;
2976 int sprsMemoryType=1;
2977 long unsigned int sprsAddress;
2978 int sprsLength;
2979 int i,j,k;
2980 int ret =0;
2981 int read_messages=0;
2982
2983 for (i=2; i<len; i=i+2)
2984 {
2985 int value= getvalue(line+i,2);
2986 sprsChecksum+= value;
2987 // printf ("chk: %d %d\n", value, sprsChecksum);
2988
2989 }
2990
2991 if ((sprsChecksum & 0xFF) == 0xFF)
2992 {
2993 // printf ("Checksum OK\n");
2994 }
2995 else
2996 {
2997 if(_verbose) yError ("Failed Checksum\n");
2998 return -1;
2999 }
3000
3001 //state: WAIT
3002 if (!(line[0] == 'S'))
3003 {
3004 if(_verbose) yError ("start tag character not found\n");
3005 return -1;
3006 }
3007 i=1;
3008
3009 //state: TYPE
3010 sprsRecordType=char(*(line+i));
3011 i++;
3012
3013 //state: LENGTH
3014 sprsLength=getvalue(line+i,2)-4-1;
3015 i=i+2;
3016
3017 switch (sprsRecordType)
3018 {
3019 case SPRS_TYPE_0:
3020
3021 return 0;
3022
3023 case SPRS_TYPE_3:
3024
3025 //state: ADDRESS
3026 sprsAddress=getvalue(line+i,4);
3027 i+=4;
3028
3029 if (sprsAddress==0x0020)
3030 sprsMemoryType=1;
3031 else
3032 sprsMemoryType=0;
3033
3034 sprsAddress=getvalue(line+i,4);
3035 i+=4;
3036
3037 //state: SEND
3038 txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3039 txBuffer[0].setLen(5);
3040 txBuffer[0].getData()[0]= ICUBCANPROTO_BL_ADDRESS;
3041 txBuffer[0].getData()[1]= sprsLength;
3042 txBuffer[0].getData()[2]= (unsigned char) ((sprsAddress) & 0x00FF);
3043 txBuffer[0].getData()[3]= (unsigned char) ((sprsAddress>>8) & 0x00FF);
3044 txBuffer[0].getData()[4]= sprsMemoryType;
3045
3046 //send here
3047 set_bus(txBuffer[0], bus);
3048 ret = m_idriver->send_message(txBuffer,1);
3049
3050 // check if send_message was successful
3051 if (ret==0)
3052 {
3053 if(_verbose) yError ("Unable to send message\n");
3054 return -1;
3055 }
3056
3057 // pause
3058 // drv_sleep(5);
3059
3060 //prepare packet
3061 int tmp, rest;
3062 if ((sprsLength%6) == 0)
3063 {
3064 tmp=sprsLength / 6;
3065 rest=6;
3066 }
3067 else
3068 {
3069 tmp=sprsLength / 6 + 1;
3070 rest=sprsLength % 6;
3071 }
3072
3073 for (j=1; j<= tmp; j++)
3074 {
3075 txBuffer[0].getData()[0]=ICUBCANPROTO_BL_DATA;
3076 if (j<tmp) txBuffer[0].setLen(7);
3077 else txBuffer[0].setLen(rest+1);
3078
3079 for (k=1; k<=6; k++)
3080 {
3081 txBuffer[0].getData()[k] = getvalue(line+i+((k-1)*2+((j-1)*12)),2);
3082 }
3083
3084 //send here
3085 set_bus(txBuffer[0], bus);
3086 ret = m_idriver->send_message(txBuffer,1);
3087
3088 // check if send_message was successful
3089 if (ret==0)
3090 {
3091 if(_verbose) yError ("Unable to send message\n");
3092 return -1;
3093 }
3094
3095 //pause
3096 // drv_sleep(5);
3097 }
3098
3099 //pause
3100 // drv_sleep(10);
3101
3102 //receive one ack for the whole line
3103 double passed;
3104 passed=Time::now()-now;
3105 // fprintf(stderr, "Passed:%.2lf [ms]\n", passed*1000);
3106 read_messages = m_idriver->receive_message(rxBuffer, nSelectedBoards);
3107 // fprintf(stderr, "%u\n", read_messages);
3108 // fprintf(stderr, "Skipping ack\n");
3109 //return verify_ack(ICUBCANPROTO_BL_DATA, read_messages);
3110 return 0;
3111 break;
3112 case SPRS_TYPE_7:
3113
3114 //state: SEND
3115 txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3116 txBuffer[0].setLen(5);
3117 txBuffer[0].getData()[0]= ICUBCANPROTO_BL_START;
3118 txBuffer[0].getData()[4]= getvalue(line+i,2); i+=2;
3119 txBuffer[0].getData()[3]= getvalue(line+i,2); i+=2;
3120 txBuffer[0].getData()[2]= getvalue(line+i,2); i+=2;
3121 txBuffer[0].getData()[1]= getvalue(line+i,2);
3122
3123 //send here
3124 set_bus(txBuffer[0], bus);
3125 ret = m_idriver->send_message(txBuffer, 1);
3126
3127 // check if send_message was successful
3128 if (ret==0)
3129 {
3130 if(_verbose) yError ("Unable to send message\n");
3131 return -1;
3132 }
3133
3134 //pause
3135 drv_sleep(10+5);
3136
3137 // riceve la risposta
3138 read_messages = m_idriver->receive_message(rxBuffer);
3139 verify_ack(ICUBCANPROTO_BL_START, read_messages);
3140 return 0;
3141
3142 break;
3143
3144
3145 default:
3146 if(_verbose) yError ("wrong format tag character %c (hex:%X)\n", sprsRecordType, sprsRecordType);
3147 return -1;
3148
3149 break;
3150 }
3151
3152 if(_verbose) yError ("Can't reach here!\n");
3153 return -1;
3154}
3155
3156//*****************************************************************/
3157// This function read one line of the hexintel file and send it to a board using the correct protocol
3158// Return values:
3159// 0 one line downloaded, continuing the download...
3160// 1 Current downloading, everything OK
3161// -1 Fatal error
3162
3163int cDownloader::download_hexintel_line(char* line, int len, int bus, int board_pid, bool eeprom, int board_type)
3164{
3165 char sprsRecordType=0;
3166 unsigned int sprsState;
3167 unsigned long int sprsChecksum=0;
3168 int sprsMemoryType=0;
3169 long unsigned int sprsAddress=0;
3170 int sprsLength=0;
3171 unsigned int sprsData[50];
3172 int i,j,k;
3173 int ret =0;
3174 int read_messages=0;
3175
3176 for (i=1; i<len; i=i+2)
3177 {
3178 int value= getvalue(line+i,2);
3179 sprsChecksum+= value;
3180 // printf ("chk: %d %d\n", value, sprsChecksum);
3181 }
3182 sprsChecksum = (sprsChecksum & 0xFF);
3183 if (sprsChecksum == 0x00)
3184 {
3185 // printf ("Checksum OK\n");
3186 }
3187 else
3188 {
3189 if(_verbose) yError ("Failed Checksum\n");
3190 return -1;
3191 }
3192
3193 sprsState=SPRS_STATE_WAIT;
3194 // Init of parsing process
3195 do
3196 {
3197 switch (sprsState)
3198 {
3199 case SPRS_STATE_WAIT:
3200 //check the first character of the line
3201 if (!(line[0] == ':'))
3202 {
3203 if(_verbose) yError("start tag character not found in hex file\n");
3204 return -1;
3205 }
3206 else
3207 {
3208 sprsState=SPRS_STATE_LENGTH;
3209 i=1;
3210 }
3211 break;
3212 case SPRS_STATE_TYPE:
3213
3214 sprsRecordType=char(*(line+i+1));//char(getvalue(line+i,2));//(char)(*(line+i));
3215 i=i+2;
3216 if (sprsLength==0)
3217 sprsState=SPRS_STATE_CHECKSUM;
3218 else
3219 sprsState=SPRS_STATE_DATA;
3220 break;
3221 case SPRS_STATE_LENGTH:
3222
3223 sprsLength= getvalue(line+i,2);
3224 i=i+2;
3225 sprsState=SPRS_STATE_ADDRESS;
3226 break;
3227 case SPRS_STATE_ADDRESS:
3228
3229 sprsAddress=getvalue(line+i,4);
3230 i=i+4;
3231 sprsState=SPRS_STATE_TYPE;
3232 break;
3233 case SPRS_STATE_DATA:
3234
3235 switch (sprsRecordType)
3236 {
3237 case SPRS_TYPE_0:
3238
3239 for (k=0;k<sprsLength;k++)
3240 {
3241 sprsData[k]=getvalue(line+i,2);
3242 i=i+2;
3243 }
3244 break;
3245
3246 case SPRS_TYPE_4:
3247
3248 sprsPage=getvalue(line+i,4);
3249 i=i+4;
3250
3251 // marco.accame on 25may17:
3252 // here is extra safety to avoid an accidental loading of stm32 code (which starts at 0x0800) on dspic based boards.
3253 // the bootloader on dspic boards in such a case erases the first sector which tells to execute to the bootloader
3254 // at reset with teh result that the board become unreachable.
3255 // as we shall release strain2.hex and mtb4.hex which use stm32 mpus w/ code at 0x0800 and beyond, any accidental
3256 // attempt to program an old strain w/ strain2.hex becomes more probable. As the damage is high (removal of the the
3257 // FT sensor + disassembly + re-programming + recalibration), some sort of protection is mandatory.
3258 // instead, strain2/mtb4 are safe if any attempt is done to program them with old strain.hex/skin.hex code
3259 if(sprsPage >= 0x0800)
3260 { // only mtb4, strain2, rfe, sg3, psc, mtb4w are allowed to use such a code space.
3261 if((icubCanProto_boardType__mtb4 == board_type) || (icubCanProto_boardType__strain2 == board_type) ||
3262 (icubCanProto_boardType__rfe == board_type) || (icubCanProto_boardType__sg3 == board_type) ||
3263 (icubCanProto_boardType__psc == board_type) || (icubCanProto_boardType__mtb4w == board_type) ||
3264 (icubCanProto_boardType__pmc == board_type)
3265 || (icubCanProto_boardType__amcbldc == board_type)
3266 || (icubCanProto_boardType__mtb4c == board_type)
3267 || (icubCanProto_boardType__strain2c == board_type)
3268 )
3269 { // it is ok
3270 }
3271 else
3272 { // be careful with that axe, eugene. ahhhhhhhhhhhhhhhh
3273 //if(_verbose)
3274 {
3275 char msg[32] = {0};
3276 snprintf(msg, sizeof(msg), "0x%04X", sprsPage);
3277 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";
3278 yError() << "You must have loaded the .hex file of another board. Perform a new discovery, check the file name and retry.";
3279 }
3280 return -1;
3281 }
3282 }
3283
3284
3285 break;
3286 }
3287 sprsState=SPRS_STATE_CHECKSUM;
3288 break;
3289 case SPRS_STATE_CHECKSUM:
3290
3291 sprsState=SPRS_STATE_WAIT;
3292 if (sprsChecksum==0)
3293 {
3294 switch (sprsRecordType)
3295 {
3296 case SPRS_TYPE_0:
3297
3298 //if (sprsPage==0)
3299 {
3300 //SEND
3301 txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3302 txBuffer[0].setLen(7);
3303 txBuffer[0].getData()[0]= ICUBCANPROTO_BL_ADDRESS;
3304 txBuffer[0].getData()[1]= sprsLength;
3305 txBuffer[0].getData()[2]= (unsigned char) ((sprsAddress) & 0x00FF);
3306 txBuffer[0].getData()[3]= (unsigned char) ((sprsAddress>>8) & 0x00FF);
3307 txBuffer[0].getData()[4]= sprsMemoryType;
3308 txBuffer[0].getData()[5]= (unsigned char) ((sprsPage) & 0x00FF);
3309 txBuffer[0].getData()[6]= (unsigned char) ((sprsPage >>8) & 0x00FF);
3310 }
3311 //send here
3312 set_bus(txBuffer[0], bus);
3313 ret = m_idriver->send_message(txBuffer,1);
3314 // check if send_message was successful
3315 if (ret==0)
3316
3317
3318 {
3319 if(_verbose) yError ("Unable to send message\n");
3320 return -1;
3321 }
3322 //pause
3323 drv_sleep(10);
3324
3325 //prepare packet
3326 int tmp, rest;
3327 if ((sprsLength%6) == 0)
3328 {
3329 tmp=sprsLength / 6;
3330 rest=6;
3331 }
3332 else
3333 {
3334 tmp=sprsLength / 6 + 1;
3335 rest=sprsLength % 6;
3336 }
3337
3338 for (j=1; j<= tmp; j++)
3339 {
3340 txBuffer[0].getData()[0]=ICUBCANPROTO_BL_DATA;
3341 if (j<tmp) txBuffer[0].setLen(7);
3342 else txBuffer[0].setLen(rest+1);
3343
3344 for (k=1; k<=6; k++)
3345 {
3346 txBuffer[0].getData()[k] = sprsData[(k-1)+((j-1)*6)];//getvalue(line+i+((k-1)*2+((j-1)*12)),2);
3347 }
3348
3349 //send here
3350 set_bus(txBuffer[0], bus);
3351 ret = m_idriver->send_message(txBuffer,1);
3352
3353 // check if send_message was successful
3354 if (ret==0)
3355 {
3356 if(_verbose) yError ("Unable to send message\n");
3357 return -1;
3358 }
3359 //pause
3360 drv_sleep(5);
3361 }
3362 //receive one ack for the whole line
3363 read_messages = m_idriver->receive_message(rxBuffer,nSelectedBoards, 10);
3364 ret=verify_ack(ICUBCANPROTO_BL_DATA, read_messages);
3365 //DEBUG
3366
3367 // return 0;
3368 return ret;
3369 break;
3370
3371 case SPRS_TYPE_1:
3372
3373 //SEND
3374 txBuffer[0].setId(build_id(ID_MASTER,board_pid));
3375 txBuffer[0].setLen(5);
3376 txBuffer[0].getData()[0]= ICUBCANPROTO_BL_START;
3377 txBuffer[0].getData()[1]= 0;
3378 txBuffer[0].getData()[2]= 0;
3379 txBuffer[0].getData()[3]= 0;
3380 txBuffer[0].getData()[4]= 0;
3381
3382 //send here
3383 set_bus(txBuffer[0], bus);
3384 ret = m_idriver->send_message(txBuffer,1);
3385 // check if send_message was successful
3386 if (ret==0)
3387 {
3388 if(_verbose) yError ("Unable to send message\n");
3389 return -1;
3390 }
3391 //pause
3392 drv_sleep(5);
3393 //receive the ack from the board
3394 read_messages = m_idriver->receive_message(rxBuffer);
3395 ret=verify_ack(ICUBCANPROTO_BL_START, read_messages);
3396 //DEBUG
3397 //return 0;
3398 return ret;
3399
3400 break;
3401 // case SPRS_TYPE_4:
3402 // break;
3403 // return 0;
3404 default:
3405 return 0;
3406 }
3407 } //end if
3408 else
3409 {
3410 if(_verbose) yError ("Checksum Error\n");
3411 }
3412 break;
3413 } //end switch
3414 }
3415 while(true);
3416
3417 if(_verbose) yError ("Can't reach here!\n");
3418 return -1;
3419}
3420//*****************************************************************/
3421
3422int cDownloader::open_file(std::string file)
3423{
3424 progress=0;
3425 filestr.close();
3426 filestr.clear();
3427 filestr.open (file.c_str(), fstream::in);
3428 if (!filestr.is_open())
3429 {
3430 if(_verbose) yError ("Error opening file!\n");
3431 return -1;
3432 }
3433
3434 file_length=0;
3435 char buffer[256];
3436 while (!filestr.eof())
3437 {
3438 filestr.getline (buffer,256);
3439 file_length++;
3440 }
3441 //printf ("length: %d\n",file_length);
3442
3443 filestr.close();
3444 filestr.clear();
3445 filestr.open (file.c_str(), fstream::in);
3446 if (!filestr.is_open())
3447 {
3448 if(_verbose) yError ("Error opening file!\n");
3449 return -1;
3450 }
3451
3452 return 0;
3453}
3454
3455//*****************************************************************/
3456// Return values:
3457// 0 Download terminated, everything OK
3458// 1 Current downloading, everything OK
3459// -1 Fatal error in sending one command
3460int cDownloader::download_file(int bus, int board_pid, int download_type, bool board_eeprom)
3461{
3462
3463 if (!filestr.is_open())
3464 {
3465 if(_verbose) yError ("File not open!\n");
3466 return -1;
3467 }
3468
3469 char buffer[256];
3470 int ret = 0;
3471
3472 nSelectedBoards=0;
3473 int i=0;
3474 for (i=0; i<board_list_size; i++)
3475 {
3476 if (board_list[i].selected==true)
3477 nSelectedBoards++;
3478 }
3479
3480 if (!filestr.eof())
3481 {
3482 filestr.getline (buffer,256);
3483
3484 //avoid to download empty lines
3485 if (strlen(buffer)!=0)
3486 {
3487 switch (download_type)
3488 {
3489 case icubCanProto_boardType__dsp:
3490 case icubCanProto_boardType__2dc:
3491 case icubCanProto_boardType__4dc:
3492 case icubCanProto_boardType__bll:
3493 ret = download_motorola_line(buffer, strlen(buffer), bus, board_pid);
3494 break;
3495 case icubCanProto_boardType__pic:
3496 case icubCanProto_boardType__skin:
3497 case icubCanProto_boardType__strain:
3498 case icubCanProto_boardType__mais:
3499 case icubCanProto_boardType__2foc:
3500 case icubCanProto_boardType__jog:
3501 case icubCanProto_boardType__6sg:
3502 case icubCanProto_boardType__mtb4:
3503 case icubCanProto_boardType__strain2:
3504 case icubCanProto_boardType__rfe:
3505 case icubCanProto_boardType__sg3:
3506 case icubCanProto_boardType__psc:
3507 case icubCanProto_boardType__mtb4w:
3508 case icubCanProto_boardType__mtb4c:
3509 case icubCanProto_boardType__pmc:
3510 case icubCanProto_boardType__amcbldc:
3511 case icubCanProto_boardType__strain2c:
3512 ret = download_hexintel_line(buffer, strlen(buffer), bus, board_pid, board_eeprom, download_type);
3513
3514 break;
3515 case icubCanProto_boardType__unknown:
3516 default:
3517 ret =-1;
3518 break;
3519 }
3520 if (ret != 0)
3521 {
3522 if(_verbose) yError("fatal error during download: abort\n");
3523 // fatal error during download, abort
3524 filestr.close();
3525 return -1;
3526 }
3527 }
3528
3529 progress++;
3530 //everything OK
3531 return 1;
3532 }
3533 else
3534 {
3535 filestr.close();
3536 filestr.clear();
3537 //download terminated OK
3538 return 0;
3539 }
3540}
3541
3542void cDownloader::clean_rx(void)
3543{
3544 m_idriver->receive_message(rxBuffer,64,0.001);
3545}
3546
3547#if defined(DOWNLOADER_USE_IDRIVER2)
3548
3549void cDownloader::set_bus(CanPacket &pkt, int bus)
3550{
3551 pkt.setCanBus(bus);
3552}
3553
3554int cDownloader::get_bus(CanPacket &pkt)
3555{
3556 return pkt.getCanBus();
3557}
3558
3559#else
3560
3561void cDownloader::set_bus(yarp::dev::CanMessage &msg, int bus)
3562{
3563 // nothing
3564}
3565
3566int cDownloader::get_bus(yarp::dev::CanMessage &msg)
3567{
3568 return get_canbus_id();
3569}
3570
3571#endif
3572
3573
3574
3575int cDownloader::strain_calibrate_offset2_strain1 (int bus, int target_id, int16_t t, string *errorstring)
3576{
3577#if 1
3578 return strain_calibrate_offset2_strain1safer(bus, target_id, t, 2, false, errorstring);
3579#else
3580 // check if driver is running
3581 if (m_idriver == NULL)
3582 {
3583 if(_verbose) yError ("Driver not ready\n");
3584 return -1;
3585 }
3586
3587 // marco.accame: transform [-32K, +32K) into [0, +64K)
3588 unsigned int middle_val = 32768 + t;
3589
3590 // in strain1 we dont have the concept of regulationset. however, if we use strain_regset_inuse which is = 0 we are ok.
3591 const int regset = 0; // strain_regset_inuse;
3592
3593 int daclimit = 0x3ff;
3594 int dacstep = 1;
3595 long tolerance = 256;
3596
3597 int channel=0;
3598 int i=0;
3599 int ret =0;
3600 long error = 0;
3601 unsigned int measure = 0;
3602 unsigned int dac = 0;
3603 int cycle =0;
3604 int read_messages;
3605
3606 for (channel=0; channel<6; channel++)
3607 {
3608 // yDebug() << "starting OFFSET of channel" << channel;
3609 // Send read channel command to strain board
3610 txBuffer[0].setId((2 << 8) + target_id);
3611 txBuffer[0].setLen(3);
3612 txBuffer[0].getData()[0]= 0x0C;
3613 txBuffer[0].getData()[1]= channel;
3614 txBuffer[0].getData()[2]= 0;
3615 set_bus(txBuffer[0], bus);
3616 ret = m_idriver->send_message(txBuffer, 1);
3617 // check if send_message was successful
3618 if (ret==0)
3619 {
3620 if(_verbose) yError ("Unable to send message\n");
3621 return -1;
3622 }
3623 //read adc
3624 read_messages = m_idriver->receive_message(rxBuffer,1);
3625 for (i=0; i<read_messages; i++)
3626 {
3627 if (rxBuffer[i].getData()[0]==0x0C)
3628 {
3629 measure = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
3630 break;
3631 }
3632 }
3633
3634
3635 //read dac
3636 txBuffer[0].setId((2 << 8) + target_id);
3637 txBuffer[0].setLen(2);
3638 txBuffer[0].getData()[0]= 0x0B;
3639 txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
3640 txBuffer[0].getData()[1]= (channel & 0x0f);
3641 set_bus(txBuffer[0], bus);
3642 ret = m_idriver->send_message(txBuffer, 1);
3643
3644 read_messages = m_idriver->receive_message(rxBuffer,1);
3645 for (i=0; i<read_messages; i++)
3646 {
3647 if (rxBuffer[i].getData()[0]==0x0B)
3648 {
3649 dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
3650 break;
3651 }
3652 }
3653
3654 error = long(measure) - long(middle_val);
3655 cycle=0;
3656
3657 while (abs(error)>tolerance && cycle<daclimit)
3658 {
3659 if (error>0) dac -= dacstep;
3660 else dac += dacstep;
3661
3662 if (dac>daclimit) dac = daclimit;
3663 if (dac<0) dac = 0;
3664
3665 //yDebug() << "iter" << cycle << "err = " << error << "next dac =" << dac;
3666
3667 // Send transmission command to strain board
3668 txBuffer[0].setId((2 << 8) + target_id);
3669 txBuffer[0].setLen(4);
3670 txBuffer[0].getData()[0]= 0x04;
3671 txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel & 0x0f);
3672 txBuffer[0].getData()[2]= dac >> 8;
3673 txBuffer[0].getData()[3]= dac & 0xFF;
3674 set_bus(txBuffer[0], bus);
3675 int ret = m_idriver->send_message(txBuffer, 1);
3676
3677 //wait
3678 drv_sleep(3);
3679
3680 // Send read channel command to strain board
3681 txBuffer[0].setId((2 << 8) + target_id);
3682 txBuffer[0].setLen(3);
3683 txBuffer[0].getData()[0]= 0x0C;
3684 txBuffer[0].getData()[1]= channel;
3685 txBuffer[0].getData()[2]= 0;
3686 set_bus(txBuffer[0], bus);
3687 ret = m_idriver->send_message(txBuffer, 1);
3688 // check if send_message was successful
3689 if (ret==0)
3690 {
3691 if(_verbose) yError ("Unable to send message\n");
3692 return -1;
3693 }
3694 //read adc
3695 read_messages = m_idriver->receive_message(rxBuffer, 1);
3696 for (i=0; i<read_messages; i++)
3697 {
3698 if (rxBuffer[i].getData()[0]==0x0C)
3699 {
3700 measure = rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4];
3701 break;
3702 }
3703 }
3704
3705 error = long(measure) - long(middle_val);
3706 cycle++;
3707 }
3708
3709 }
3710
3711 return 0;
3712#endif
3713}
3714
3715
3716int 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)
3717{
3718 // the calibration of the offset is meaningful only for the calibration set in use.
3719 const int regset = strain_regset_inuse;
3720 const unsigned int NUMofCHANNELS = 6;
3721
3722 std::ostringstream ss;
3723
3724 // check if driver is running
3725 if (m_idriver == NULL)
3726 {
3727 if(_verbose) yError ("Driver not ready\n");
3728 Log(std::string("strain_calibrate_offset2_strain2(): failure. driver no ready"));
3729 return -1;
3730 }
3731
3732
3733 ss.str("");
3734 ss.clear();
3735 ss << "performing offset autotuning for strain2";
3736 Log(ss.str());
3737
3738 // step 1: apply the gains. the initial offset will be meaning less. however strain_set_amplifier_discretegain() assign offsets all equal to 32k-1
3739 for(int channel=0; channel<NUMofCHANNELS; channel++)
3740 {
3741 ss.str("");
3742 ss.clear();
3743 ss << "- on ch " << std::to_string(channel) << ": we impose g = " << std::to_string(static_cast<int>(strain_amplifier_discretegain2float(gains[channel])));
3744
3745 if(0 != strain_set_amplifier_discretegain(bus, target_id, channel, gains[channel], regset, errorstring))
3746 {
3747 if(_verbose)
3748 {
3749 Log(ss.str());
3750 Log("strain_calibrate_offset2_strain2(): failure of strain_set_amplifier_discretegain()");
3751 }
3752 return -1;
3753 }
3754 // i wait some time
3755 yarp::os::Time::delay(1.0);
3756
3757 float gaain = 0;
3758 uint16_t ooffset = 0;
3759 strain_get_amplifier_gain_offset(bus, target_id, channel, gaain, ooffset, regset, errorstring);
3760
3761 ss << " and read (g, o) = (" << std::to_string(static_cast<int>(gaain)) << ", " << std::to_string(ooffset) << ")";
3762 Log(ss.str());
3763 }
3764
3765
3766
3767 yarp::os::Time::delay(2.0);
3768
3769
3770 // step 3: eval if the targets are equal or not. if they are all equal we send a single command.
3771 // if not we must send one command per channel.
3772
3773 int16_t singletargetVALUE = targets[0];
3774 bool singletargetTHEREIS = true;
3775 for(int channel=1; channel<NUMofCHANNELS; channel++)
3776 {
3777 if(singletargetVALUE != targets[channel])
3778 {
3779 singletargetTHEREIS = false;
3780 break;
3781 }
3782 }
3783
3784
3785 long tolerance = 256;
3786
3787
3788
3789 tolerance = 256;
3790 uint8_t samples2average = 8; // if zero, the board uses its default (= 4)
3791
3792
3793
3794 // step3: autocalib
3795 const uint8_t everychannel = 0x0f;
3796
3797 uint8_t channel2autocalib = everychannel; // the channel(s) ...
3798 unsigned int middle_val = 32768; // transform [-32K, +32K) into [0, +64K)
3799 uint8_t okmask = 0x3f; // all six channel
3800
3801 if(true == singletargetTHEREIS)
3802 {
3803
3804 channel2autocalib = everychannel;
3805 middle_val = 32768 + singletargetVALUE;
3806 okmask = 0x3f; // all six channel
3807
3808
3809 ss.str("");
3810 ss.clear();
3811 ss << "STEP-2. there is a single ADC target: performing parallel regularization";
3812 Log(ss.str());
3813
3814 ss.str("");
3815 ss.clear();
3816 ss << " params: target = " << std::to_string(singletargetVALUE) << " tolerance = " << std::to_string(tolerance) << " samples2average = " << std::to_string(samples2average);
3817
3818 // sending an autocalib message
3819 txBuffer[0].setId((2 << 8) + target_id);
3820 txBuffer[0].setLen(8);
3821 txBuffer[0].getData()[0]= 0x22;
3822 txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel2autocalib & 0x0f);
3823 txBuffer[0].getData()[2]= 0; // mode oneshot, the only possible so far
3824 txBuffer[0].getData()[3]= middle_val & 0x00ff; // little endian
3825 txBuffer[0].getData()[4]= (middle_val >> 8) & 0x00ff; // little endian
3826 txBuffer[0].getData()[5]= tolerance & 0x00ff; // little endian
3827 txBuffer[0].getData()[6]= (tolerance >> 8) & 0x00ff; // little endian
3828 txBuffer[0].getData()[7]= samples2average;
3829 set_bus(txBuffer[0], bus);
3830 // 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]);
3831 int ret = m_idriver->send_message(txBuffer, 1);
3832 // check if send_message was successful
3833 if (ret==0)
3834 {
3835 if(_verbose) yError ("Unable to send message\n");
3836 return -1;
3837 }
3838
3839 // now wait for a reply for most 3 seconds
3840 double TOUT = 3.0;
3841
3842 ss.str("");
3843 ss.clear();
3844 ss << "STEP-3. results ...";
3845 Log(ss.str());
3846
3847 int read_messages = m_idriver->receive_message(rxBuffer, 1, TOUT);
3848 for(int i=0; i<read_messages; i++)
3849 {
3850 if (rxBuffer[i].getData()[0]==0x22)
3851 {
3852 //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]);
3853
3854 //dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
3855 uint8_t noisychannelmask = rxBuffer[i].getData()[2];
3856 uint8_t algorithmOKmask = rxBuffer[i].getData()[3];
3857 uint8_t finalmeasureOKmask = rxBuffer[i].getData()[4];
3858 uint16_t mae = (static_cast<uint32_t>(rxBuffer[i].getData()[6])) |
3859 (static_cast<uint32_t>(rxBuffer[i].getData()[7]) << 8);
3860
3861 if((okmask == algorithmOKmask) && (okmask == finalmeasureOKmask))
3862 {
3863 ss.str("");
3864 ss.clear();
3865 ss << " OK w/ MAE = " << std::to_string(mae);
3866 Log(ss.str());
3867
3868 if(0 != noisychannelmask)
3869 {
3870 ss.str("");
3871 ss.clear();
3872 ss << " BUT noisy acquisition of samples: ";
3873 if((0x40 & noisychannelmask) == 0x40)
3874 {
3875 ss << " in computing average ADC before algorithm ";
3876 }
3877 if((0x80 & noisychannelmask) == 0x80)
3878 {
3879 ss << "after algorithm in computing MAE";
3880 }
3881 Log(ss.str());
3882
3883 ss.str("");
3884 ss.clear();
3885 char tmp[256] = {0};
3886 snprintf(tmp, sizeof(tmp), "noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
3887 ss << "COMPLETE RES: " << tmp;
3888 Log(ss.str());
3889 }
3890
3891 }
3892 else
3893 {
3894 ss.str("");
3895 ss.clear();
3896 ss << " KO: ";
3897 char tmp[256] = {0};
3898 snprintf(tmp, sizeof(tmp), "noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
3899 ss << tmp;
3900 Log(ss.str());
3901
3902 if(0 != noisychannelmask)
3903 {
3904 ss.str("");
3905 ss.clear();
3906 ss << " WITH noisy acquisition of samples: ";
3907 if((0x40 & noisychannelmask) == 0x40)
3908 {
3909 ss << " in computing average ADC before algorithm ";
3910 }
3911 if((0x80 & noisychannelmask) == 0x80)
3912 {
3913 ss << "after algorithm in computing MAE";
3914 }
3915 Log(ss.str());
3916 }
3917
3918 for(uint8_t channel=0; channel<NUMofCHANNELS; channel++)
3919 {
3920 ss.str("");
3921 ss.clear();
3922 bool problems = false;
3923 ss << "- on ch " << std::to_string(channel) << ":";
3924 if((algorithmOKmask & (0x01<<channel)) == 0)
3925 {
3926 problems = true;
3927 ss << " [algorithm fails]";
3928 }
3929 if((finalmeasureOKmask & (0x01<<channel)) == 0)
3930 {
3931 problems = true;
3932 ss << " [mae is high (does ADC work?)]";
3933 }
3934 if(((noisychannelmask) & (0x01<<channel)) == (0x01<<channel))
3935 {
3936 problems = true;
3937 ss << " [noisy acquition]";
3938 }
3939 if(!problems)
3940 {
3941 ss << " [no detected problem]";
3942 }
3943 Log(ss.str());
3944 }
3945 }
3946 break;
3947 }
3948 }
3949 }
3950 else
3951 {
3952 // for all six channels
3953 ss.str("");
3954 ss.clear();
3955 ss << "STEP-2. there are multiple ADC targets: performing regularization channel by channel";
3956 Log(ss.str());
3957
3958 ss.str("");
3959 ss.clear();
3960 ss << " common params: tolerance = " << std::to_string(tolerance) << " samples2average = " << std::to_string(samples2average);
3961 Log(ss.str());
3962
3963
3964 for(int channel=0; channel<NUMofCHANNELS; channel++)
3965 {
3966 channel2autocalib = channel;
3967 middle_val = 32768 + targets[channel];
3968 okmask = 0x01 << channel;
3969
3970 ss.str("");
3971 ss.clear();
3972 ss << "- on ch " << std::to_string(channel) << ": ADC target = " << std::to_string(targets[channel]);
3973 Log(ss.str());
3974
3975
3976 // send message, check results ...
3977
3978 // sending an autocalib message
3979 txBuffer[0].setId((2 << 8) + target_id);
3980 txBuffer[0].setLen(8);
3981 txBuffer[0].getData()[0]= 0x22;
3982 txBuffer[0].getData()[1]= ((regset << 4) & 0xf0) | (channel2autocalib & 0x0f);
3983 txBuffer[0].getData()[2]= 0; // mode oneshot, the only possible so far
3984 txBuffer[0].getData()[3]= middle_val & 0x00ff; // little endian
3985 txBuffer[0].getData()[4]= (middle_val >> 8) & 0x00ff; // little endian
3986 txBuffer[0].getData()[5]= tolerance & 0x00ff; // little endian
3987 txBuffer[0].getData()[6]= (tolerance >> 8) & 0x00ff; // little endian
3988 txBuffer[0].getData()[7]= samples2average;
3989 set_bus(txBuffer[0], bus);
3990 int ret = m_idriver->send_message(txBuffer, 1);
3991 // check if send_message was successful
3992 if (ret==0)
3993 {
3994 if(_verbose) yError ("Unable to send message\n");
3995 return -1;
3996 }
3997
3998 // now wait for a reply for most 3 seconds
3999 double TOUT = 3.0;
4000
4001
4002 int read_messages = m_idriver->receive_message(rxBuffer, 1, TOUT);
4003 for(int i=0; i<read_messages; i++)
4004 {
4005 if (rxBuffer[i].getData()[0]==0x22)
4006 {
4007 //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]);
4008
4009 //dac = rxBuffer[i].getData()[2]<<8 | rxBuffer[i].getData()[3];
4010 uint8_t noisychannelmask = rxBuffer[i].getData()[2];
4011 uint8_t algorithmOKmask = rxBuffer[i].getData()[3];
4012 uint8_t finalmeasureOKmask = rxBuffer[i].getData()[4];
4013 uint16_t mae = (static_cast<uint32_t>(rxBuffer[i].getData()[6])) |
4014 (static_cast<uint32_t>(rxBuffer[i].getData()[7]) << 8);
4015
4016 if((okmask == algorithmOKmask) && (okmask == finalmeasureOKmask))
4017 {
4018 ss.str("");
4019 ss.clear();
4020 ss << " OK w/ MAE = " << std::to_string(mae);
4021 Log(ss.str());
4022
4023 if(0 != (noisychannelmask & okmask))
4024 {
4025 ss.str("");
4026 ss.clear();
4027 ss << " BUT noisy acquisition of samples: ";
4028 Log(ss.str());
4029
4030 if((0x40 & noisychannelmask) == 0x40)
4031 {
4032 ss.str("");
4033 ss.clear();
4034 ss << " - in computing average ADC before algorithm ";
4035 Log(ss.str());
4036 }
4037 if((0x80 & noisychannelmask) == 0x80)
4038 {
4039 ss.str("");
4040 ss.clear();
4041 ss << " - after algorithm in computing MAE";
4042 Log(ss.str());
4043 }
4044
4045 ss.str("");
4046 ss.clear();
4047 char tmp[256] = {0};
4048 snprintf(tmp, sizeof(tmp), "noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
4049 ss << "COMPLETE RES: " << tmp;
4050 Log(ss.str());
4051 }
4052
4053 }
4054 else
4055 {
4056
4057 ss.str("");
4058 ss.clear();
4059 ss << " KO /w MAE = " << std::to_string(mae) << " because: ";
4060 Log(ss.str());
4061
4062 ss.str("");
4063 ss.clear();
4064 char tmp[256] = {0};
4065 snprintf(tmp, sizeof(tmp), " KO details: noisychannelmask = 0x%x, algorithmOKmask = 0x%x, finalmeasureOKmask = 0x%x, mae = %d", noisychannelmask, algorithmOKmask, finalmeasureOKmask, mae);
4066 ss << tmp;
4067 Log(ss.str());
4068
4069 bool problems = false;
4070 if((algorithmOKmask & okmask) == 0)
4071 {
4072 problems = true;
4073 ss.str("");
4074 ss.clear();
4075 ss << " - algorithm fails";
4076 Log(ss.str());
4077 }
4078 if((finalmeasureOKmask & okmask) == 0)
4079 {
4080 problems = true;
4081 ss.str("");
4082 ss.clear();
4083 ss << " - mae is high (does ADC work?)";
4084 Log(ss.str());
4085 }
4086 if(!problems)
4087 {
4088 ss.str("");
4089 ss.clear();
4090 ss << " .. strange: no detected problem";
4091 Log(ss.str());
4092 }
4093
4094 }
4095 break;
4096 }
4097 }
4098 }
4099 }
4100
4101 return 0;
4102}
4103
4104int cDownloader::readADC(int bus, int target_id, int channel, int nmeasures)
4105{
4106 const int type = 0; // raw
4107
4108 txBuffer[0].setId((2 << 8) + target_id);
4109 txBuffer[0].setLen(3);
4110 txBuffer[0].getData()[0]= 0x0C;
4111 txBuffer[0].getData()[1]= channel;
4112 txBuffer[0].getData()[2]= type;
4113
4114 int measure = 0;
4115
4116 for(int n=0; n<nmeasures; n++)
4117 {
4118 int tmp = 0;
4119 set_bus(txBuffer[0], bus);
4120 int ret = m_idriver->send_message(txBuffer, 1);
4121 // check if send_message was successful
4122 if (ret==0)
4123 {
4124 yError ("Unable to send message\n");
4125 return 0;
4126 }
4127 //read adc
4128 int read_messages = m_idriver->receive_message(rxBuffer,1);
4129 for (int i=0; i<read_messages; i++)
4130 {
4131 if (rxBuffer[i].getData()[0]==0x0C)
4132 {
4133 tmp = (rxBuffer[i].getData()[3]<<8 | rxBuffer[i].getData()[4]);
4134 break;
4135 }
4136 else
4137 {
4138 printf("cDownloader::strain_calibrate_offset2_strain1(): fails in reading reply for a measure\n");
4139 }
4140 }
4141
4142 measure += tmp;
4143 }
4144
4145 measure /= nmeasures;
4146
4147 return measure;
4148}
4149
4150int cDownloader::strain_calibrate_offset2_strain1safer (int bus, int target_id, int16_t t, uint8_t nmeasures, bool fullsearch, string *errorstring)
4151{
4152 // check if driver is running
4153 if (m_idriver == NULL)
4154 {
4155 if(_verbose) yError ("Driver not ready\n");
4156 return -1;
4157 }
4158
4159 std::ostringstream ss;
4160
4161 // marco.accame: transform [-32K, +32K) into [0, +64K)
4162 unsigned int middle_val = 32768 + t;
4163
4164
4165 if(fullsearch)
4166 {
4167 //yDebug() << "performing full search in dac space to find best value to match adc ="<< t << " using" << nmeasures << "adc acquisions for better averaging";
4168
4169 ss.str("");
4170 ss.clear();
4171 ss << "performing offset autotuning for strain1 in full search mode";
4172 Log(ss.str());
4173
4174 ss.str("");
4175 ss.clear();
4176 ss << "params: " << "adc target =" << std::to_string(t) << " using" << std::to_string(nmeasures) << "adc acquisions for better averaging";
4177 Log(ss.str());
4178
4179 for(int channel=0; channel<6; channel++)
4180 {
4181
4182 long minABSerror = 128*1024;
4183 unsigned int minDAC = 0;
4184
4185 // loop over all possible dac values
4186 for(unsigned int testdac=0; testdac<1024; testdac++)
4187 {
4188 // send new dac
4189 strain_set_offset(bus, target_id, channel, testdac);
4190 //wait
4191 drv_sleep(3);
4192 // verify it
4193 unsigned int tmp = 0;
4194 strain_get_offset(bus, target_id, channel, tmp);
4195 if(tmp != testdac)
4196 {
4197 yError() << "failed to impose DAC = " << testdac << "read:" << tmp;
4198 }
4199
4200 // read value
4201 int measure = readADC(bus, target_id, channel, nmeasures);
4202 // compute error vs target
4203 long error = long(measure) - long(middle_val);
4204
4205 if(fabs(error) < fabs(minABSerror))
4206 {
4207 minABSerror = fabs(error);
4208 minDAC = testdac;
4209 //yDebug() << "PROGESS: channel =" << channel << "minerror = " << minABSerror << "mindac =" << minDAC;
4210 }
4211 }
4212
4213 // apply the best dac
4214 strain_set_offset(bus, target_id, channel, minDAC);
4215 // wait
4216 drv_sleep(3);
4217 // verify it
4218 unsigned int tmp = 0;
4219 strain_get_offset(bus, target_id, channel, tmp);
4220 if(tmp != minDAC)
4221 {
4222 yError() << "failed to impose DAC = " << minDAC << "read:" << tmp;
4223 }
4224
4225 ss.str("");
4226 ss.clear();
4227 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);
4228 Log(ss.str());
4229
4230 //yDebug() << "RESULT of FULL SEARCH w/ nsamples average =" << nmeasures << "-> channel =" << channel << "minerror = " << minABSerror << "mindac =" << minDAC;
4231
4232 } // channel
4233
4234 }
4235 else
4236 {
4237 const int daclimit = 0x3ff;
4238 const int dacstep = 1;
4239 const long tolerance = 128;
4240 const int maxiterations = 1024;
4241
4242 ss.str("");
4243 ss.clear();
4244 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";
4245 Log(ss.str());
4246
4247 ss.str("");
4248 ss.clear();
4249 ss << "exit conditions: max number of iterations =" << std::to_string(maxiterations) << "error tolerance =" << std::to_string(tolerance);
4250 Log(ss.str());
4251
4252 for(int channel=0; channel<6; channel++)
4253 {
4254
4255 long minABSerror = 128*1024;
4256 unsigned int minDAC = 0;
4257 unsigned int dac = 0;
4258
4259
4260 int measure = readADC(bus, target_id, channel, nmeasures);
4261
4262 // read dac
4263 strain_get_offset(bus, target_id, channel, dac);
4264
4265 long error = long(measure) - long(middle_val);
4266 int cycle =0;
4267
4268 minABSerror = fabs(error);
4269 minDAC = dac;
4270
4271 // now i perform a sort of gradient descend
4272 while ((abs(error)>tolerance) && (cycle<daclimit) && (cycle<maxiterations))
4273 {
4274 if (error>0) dac -= dacstep;
4275 else dac += dacstep;
4276
4277 if (dac>daclimit) dac = daclimit;
4278 if (dac<0) dac = 0;
4279
4280 //yDebug() << "channel =" << channel << "iter =" << cycle << "err = " << error << "next dac =" << dac << "minerror = " << minABSerror << "mindac =" << minDAC;
4281
4282 // send new dac
4283 strain_set_offset(bus, target_id, channel, dac);
4284 //wait
4285 drv_sleep(3);
4286 // verify it
4287 unsigned int tmp = 0;
4288 strain_get_offset(bus, target_id, channel, tmp);
4289 if(tmp != dac)
4290 {
4291 yError() << "failed to impose DAC = " << dac << "read:" << tmp;
4292 }
4293
4294 // read value
4295 measure = readADC(bus, target_id, channel, nmeasures);
4296
4297 error = long(measure) - long(middle_val);
4298 cycle++;
4299
4300 if(fabs(error) < fabs(minABSerror))
4301 {
4302 minABSerror = fabs(error);
4303 minDAC = dac;
4304 }
4305 }
4306
4307 ss.str("");
4308 ss.clear();
4309 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);
4310 Log(ss.str());
4311
4312 } // channel
4313
4314 }
4315
4316
4317 return 0;
4318}
4319
4320int cDownloader::initSINGLEBOARD(int canbus, int canaddress)
4321{
4322 if (m_idriver == NULL)
4323 {
4324 if (_verbose) yError("Driver not ready\n");
4325 return -1;
4326 }
4327
4328 int retries = 5; // Number of retries
4329 for (int attempt = 0; attempt < retries; ++attempt)
4330 {
4331 txBuffer[0].setId(build_id(ID_MASTER, canaddress));
4332 txBuffer[0].setLen(1);
4333 txBuffer[0].getData()[0] = ICUBCANPROTO_BL_BROADCAST; // Discovery command
4334 set_bus(txBuffer[0], canbus);
4335
4336 int ret = m_idriver->send_message(txBuffer, 1);
4337 if (ret == 0)
4338 {
4339 if (_verbose) yError("Unable to send discovery message\n");
4340 return -1;
4341 }
4342
4343 drv_sleep(0.2); // Wait for response (reduced from 5.0)
4344
4345 int read_messages = m_idriver->receive_message(rxBuffer, 10, 0.2); // reduced timeout
4346
4347 for (int i = 0; i < read_messages; ++i)
4348 {
4349 yDebug("Received message ID: 0x%x, Data: [%x, %x, %x, %x, %x, %x, %x, %x]",
4350 rxBuffer[i].getId(),
4351 rxBuffer[i].getData()[0], rxBuffer[i].getData()[1], rxBuffer[i].getData()[2],
4352 rxBuffer[i].getData()[3], rxBuffer[i].getData()[4], rxBuffer[i].getData()[5],
4353 rxBuffer[i].getData()[6], rxBuffer[i].getData()[7]);
4354 }
4355
4356 for (int i = 0; i < read_messages; ++i)
4357 {
4358 if ((rxBuffer[i].getId() >> 4 & 0x0F) == canaddress)
4359 {
4360 if (_verbose) yInfo("Board %d found on CAN bus %d\n", canaddress, canbus);
4361
4362 // Populate board_list with discovered board
4363 if (board_list != NULL) {
4364 delete[] board_list;
4365 board_list = NULL;
4366 }
4367 board_list = new sBoard[1];
4368 board_list_size = 1;
4369
4370 sBoard& current_board = board_list[0];
4371 current_board.bus = canbus;
4372 current_board.pid = canaddress;
4373 current_board.type = rxBuffer[i].getData()[1];
4374 current_board.applicationisrunning = (rxBuffer[i].getLen() == 5);
4375 current_board.appl_vers_major = rxBuffer[i].getData()[2];
4376 current_board.appl_vers_minor = rxBuffer[i].getData()[3];
4377 if (current_board.applicationisrunning) {
4378 current_board.appl_vers_build = rxBuffer[i].getData()[4];
4379 } else {
4380 current_board.appl_vers_build = -1;
4381 }
4382 current_board.status = BOARD_RUNNING;
4383 current_board.selected = false;
4384 current_board.eeprom = false;
4385 // Optionally clear serial/add_info
4386 current_board.serial[0] = '\0';
4387 current_board.add_info[0] = '\0';
4388
4389 return 0; // Board found
4390 }
4391 }
4392 }
4393
4394 if (_verbose) yError("Board %d not found on CAN bus %d after %d attempts\n", canaddress, canbus, retries);
4395 return -1; // Board not found
4396}
4397
4398// eof
4399
4400
4401
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:3422
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::initSINGLEBOARD
int initSINGLEBOARD(int canbus, int canaddress)
Definition downloader.cpp:4320
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:2731
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:2743
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:2840
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:259
cDownloader::strain_acquisition_mode_polling
@ strain_acquisition_mode_polling
Definition downloader.h:261
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:2736
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::strain_regset_inuse
@ strain_regset_inuse
Definition downloader.h:147
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::board_list
sBoard * board_list
Definition downloader.h:150
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_regsetmode_permanent
@ strain_regsetmode_permanent
Definition downloader.h:148
cDownloader::strain_regsetmode_temporary
@ strain_regsetmode_temporary
Definition downloader.h:148
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::filestr
std::fstream filestr
Definition downloader.h:155
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::board_list_size
int board_list_size
Definition downloader.h:151
cDownloader::sprsPage
unsigned int sprsPage
Definition downloader.h:154
cDownloader::download_file
int download_file(int bus, int board_pid, int download_type, bool eeprom)
Definition downloader.cpp:3460
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::nSelectedBoards
int nSelectedBoards
Definition downloader.h:156
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
iDriver2::send_message
virtual int send_message(vector< CanPacket > &canpackets, int n)=0
iDriver2::type
virtual iDriver2Type type()=0
iDriver2::init
virtual int init(yarp::os::Searchable &config, bool verbose=true)=0
iDriver2::receive_message
virtual int receive_message(vector< CanPacket > &canpackets, int howMany=MAX_READ_MSG, double TIMEOUT=1)=0
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
error
bool error
Definition debugFunctions.cpp:57
_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:2889
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
drv_sleep
void drv_sleep(double time)
Definition downloader.cpp:26
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
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:242
cDownloader::strain_value_t::saturationinfo
icubCanProto_strain_saturationInfo_t saturationinfo[6]
Definition downloader.h:247
cDownloader::strain_value_t::calibrated
bool calibrated
Definition downloader.h:244
cDownloader::strain_value_t::valid
bool valid
Definition downloader.h:243
cDownloader::strain_value_t::saturated
bool saturated
Definition downloader.h:245
cDownloader::strain_value_t::channel
unsigned int channel[6]
Definition downloader.h:246
sBoard::eeprom
bool eeprom
Definition downloader.h:39
sBoard::serial
char serial[50]
Definition downloader.h:34
sBoard::appl_vers_major
int appl_vers_major
Definition downloader.h:29
sBoard::prot_vers_major
int prot_vers_major
Definition downloader.h:32
sBoard::prot_vers_minor
int prot_vers_minor
Definition downloader.h:33
sBoard::strainregsetatboot
int strainregsetatboot
Definition downloader.h:36
sBoard::appl_vers_minor
int appl_vers_minor
Definition downloader.h:30
sBoard::applicationisrunning
bool applicationisrunning
Definition downloader.h:28
sBoard::bus
int bus
Definition downloader.h:25
sBoard::type
int type
Definition downloader.h:27
sBoard::pid
int pid
Definition downloader.h:26
sBoard::add_info
char add_info[50]
Definition downloader.h:40
sBoard::appl_vers_build
int appl_vers_build
Definition downloader.h:31
sBoard::status
int status
Definition downloader.h:37
sBoard::strainregsetinuse
int strainregsetinuse
Definition downloader.h:35
sBoard::selected
bool selected
Definition downloader.h:38
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 Fri Jun 20 2025 14:53:55 for iCub-main by doxygen 1.9.8