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