iCub-main
strain.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 iCub Facility - Istituto Italiano di Tecnologia
3  * Author: Marco Accame
4  * email: marco.accame@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 
20 // --------------------------------------------------------------------------------------------------------------------
21 // - public interface
22 // --------------------------------------------------------------------------------------------------------------------
23 
24 #include "strain.h"
25 
26 
27 
28 // --------------------------------------------------------------------------------------------------------------------
29 // - external dependencies
30 // --------------------------------------------------------------------------------------------------------------------
31 
32 #include <cstring>
33 #include <vector>
34 #include <fstream>
35 
36 using namespace std;
37 
38 
39 
40 // --------------------------------------------------------------------------------------------------------------------
41 // - pimpl: private implementation (see scott meyers: item 22 of effective modern c++, item 31 of effective c++
42 // --------------------------------------------------------------------------------------------------------------------
43 
44 // so let me introduce to you the one and only .... pimpl
45 
46 
47 #define IMPORT_CODE_FROM_EMBOT_HW_PGA308
48 
50 {
51 
52  // marco.accame on 27jul18: i decide to use code already developed (and very well tested ....) in:
53  // namespace embot { namespace hw { namespace PGA308 {
54  // i encapluslate it under a different scope (this Impl) and I remove dependency from can protocol
55  // by changing: embot::app::canprotocol::analog::polling::PGA308cfg1
56  // with: strain::amplifier::PGA308::Registers
57  // teh two structs have the same bitmap layout. but Registers also have methods to import from and export to a can packet
58 
59 #if defined(IMPORT_CODE_FROM_EMBOT_HW_PGA308)
60 
61  struct ZDACregister
62  {
63  std::uint16_t value;
64  static const std::uint16_t Default = 0x8000;
65 
66  // ZDAC Register: Zero DAC Register (Fine Offset Adjust)
67  // bit: 15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00
68  // content: ZD15 ZD14 ZD13 ZD12 ZD11 ZD10 ZD9 ZD8 ZD7 ZD6 ZD5 ZD4 ZD3 ZD2 ZD1 ZD0
69  // teh value x contained in the register is mapped into: +VREF/2 – (x/65536) (VREF)
70  // the possible ranges of the offset is [+VREF*0.5, -VREF*0.4999847]
71 
72  ZDACregister() : value(0) {}
73 
74  ZDACregister(std::uint16_t v) : value(v) {}
75 
76  void reset() { value = 0; }
77 
78  void set(std::uint16_t zdac) { value = zdac; }
79 
80  void setDefault() { value = ZDACregister::Default; }
81  };
82 
83  struct GDACregister
84  {
85  std::uint16_t value;
86  static const std::uint16_t Default = 0x4000;
87 
88  // GDAC Register: Gain DAC Register
89  // bit: 15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00
90  // content: GD15 GD14 GD13 GD12 GD11 GD10 GD9 GD8 GD7 GD6 GD5 GD4 GD3 GD2 GD1 GD0
91  // the value x contained in the register is mapped into: 0.333333333 + x * (1.000000000 – 0.333333333) / 65536 = 0.333333333 + x * 1.0172526 * 10–5
92  // value of register x is: (DesiredGain – 0.333333333) / (1.0172526 x 10–5)
93  // the range of gain is [0.333333333, 0.999989824] and GDACregister::Default is 0.499999999
94 
95  GDACregister() : value(0) {}
96 
97  GDACregister(std::uint16_t v) : value(v) {}
98 
99  void reset() { value = 0; }
100 
101  void set(std::uint16_t gdac) { value = gdac; }
102 
103  void setDefault() { value = GDACregister::Default; }
104  };
105 
107  {
108  std::uint16_t value;
109 
110  static const std::uint8_t DefaultGO = 0x06; // output gain = 6.0 (0x06) OR 2.0 (0x00)
111  static const std::uint8_t DefaultGO_g_32_96 = 0x06; // output gain = 6.0 (0x06)
112  static const std::uint8_t DefaultGO_g_16_80 = 0x02; // output gain = 3.0 (0x02)
113  static const std::uint8_t DefaultGO_g_11_75 = 0x00; // output gain = 3.0 (0x00)
114  static const std::uint8_t DefaultMUX = 0x00; // VIN1= VINPositive, VIN2= VINNegative
115  static const std::uint8_t DefaultGI = 0x04; // input gain = 16
116  static const std::uint8_t DefaultOS = 0x20; // coarse offset // 0x25 0x8A
117 
118  // CFG0 Output Amplifier Gain Select, Front-End PGA Mux & Gain Select, Coarse Offset Adjust on Front-End PGA
119  // bit: 15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00
120  // content: GO2 GO1 GO0 GI4 GI3 GI2 GI1 GI0 OS7 OS6 OS5 OS4 OS3 OS2 OS1 OS0
121 
122  CFG0register() : value(0) {}
123 
124  CFG0register(std::uint8_t GO, std::uint8_t MUX, std::uint8_t GI, std::uint8_t OS)
125  {
126  value = 0;
127  setGO(GO);
128  setMUX(MUX);
129  setGI(GI);
130  setOS(OS);
131  }
132 
133  CFG0register(std::uint16_t v) : value(v) {}
134 
135  void reset() { value = 0; }
136 
137  void setDefault() { value = 0; setGO(CFG0register::DefaultGO); setMUX(CFG0register::DefaultMUX); setGI(CFG0register::DefaultGI); setOS(CFG0register::DefaultOS); }
138 
139  // Output Amplifier Gain Select: [GO2-GO0] -> from 000b to 111b: 2.0, 2.4, 3, 3.6, 4.0, 4.5, 6.0 disable-internal-feedback
140  void setGO(std::uint8_t GO) { value |= (static_cast<std::uint8_t>(GO&0x07) << 13); }
141  std::uint8_t getGO() const { return ((value >> 13) & 0x07); }
142 
143  // Front-End PGA Mux Select: [GI4] -> 0 is [VIN1= VINN, VIN2= VINP], 1 is [VIN1= VINP, VIN2= VINN]
144  void setMUX(std::uint8_t MUX) { value |= (static_cast<std::uint8_t>(MUX&0x01) << 12); }
145  std::uint8_t getMUX() const { return ((value >> 12) & 0x01); }
146 
147  // Front-End PGA Gain Select: [GI3-GI0] -> from 0000b to 1101b: 4 6 8 12 16 32 64 100 200 400 480 600 800 960 1200 1600
148  void setGI(std::uint8_t GI) { value |= (static_cast<std::uint8_t>(GI&0x0f) << 8); }
149  std::uint8_t getGI() const { return ((value >> 8) & 0x0f); }
150 
151  // Coarse Offset Adjust on Front-End PGA: [OS7-OS0] where OS7 is sign (0 is +) and [OS6-OS0] is value. valid range is only x = [-100, +100]
152  // which maps into (x/128)(Vref)(0.0256) for a global offset range of [-0.02*V_REF, +0.02*V_REF]. V_REF is in register CFG2.COSVR[1:0]
153  void setOS(std::uint8_t OS) { value |= (static_cast<std::uint8_t>(OS&0xff)); }
154  std::uint8_t getOS() const { return ((value) & 0xff); }
155 
156  };
157 
158 
160  {
161  std::uint16_t value;
162  static const std::uint16_t Default = 0x0000;
163 
164  // CFG1 Register: Configuration Register 1
165  // bit: 15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00
166  // content: FLT-REF FLT-IPU OU-CFG FLT-SEL CMP-SEL EXT-EN INT-EN EXT-POL INT-POL OU-EN HL2 HL1 HL0 LL2 LL1 LL0
167 
168  CFG1register() { value = 0; }
169 
170  CFG1register(std::uint16_t v) : value(v) {}
171 
172  void reset() { value = 0; }
173 
174  void set(std::uint16_t cfg1) { value = cfg1; }
175 
176  void setDefault() { value = Default; }
177  };
178 
180  {
181  std::uint16_t value;
182  static const std::uint16_t Default = 0x0400;
183 
184  // CFG2 Register: Configuration Register 2
185  // bit: 15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00
186  // content: OWD OWD-OFF DIS-OUT NOW COSVR1 COSVR0 RESERVD DOUTSEL DOUT SD RESERVD RESERVD RESERVD RESERVD RESERVD RESERVD
187  // COSVR: 00 -> ; 01-> ; 10 -> ; 11 -> ;
188  // with COSVR = 01b, as in our Default ... we have:
189  // - Coarse Offset Range = [2.4, 3.6] V
190  // - Coarse Offset Resolution = (1/128)(VREF)(0.0427)
191  // - Coarse Offset Range = (±100mV)(VREF/3)
192  CFG2register() { value = 0; }
193 
194  CFG2register(std::uint16_t v) : value(v) {}
195 
196  void reset() { value = 0; }
197 
198  void set(std::uint16_t cfg2) { value = cfg2; }
199 
200  void setDefault() { value = Default; }
201  };
202 
203 
205  {
206  std::uint16_t value;
207  static const std::uint16_t Default = 0x0050; // where ... there is the SOFTWARE LOCK MODE (Runs from RAM) and the PGA308 operates from data written into the RAM registers from the user
208 
209  // SFTC Register: Software Control
210  // bit: 15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00
211  // content: RESERVD RESERVD RESERVD RESERVD RESERVD RESERVD RESERVD CHKSFLG OW-DLY SW-L2 SW-L1 SW-L0 RFB0 XP5 XP4 XP3
212  // brief description:
213  // CHKSFLG: Register Checksum Bit (Read-only) 1 = register checksum correct
214  // OW-DLY: One-Wire Delay Bit 1 = 8-bit delay from transmit to receive during One-Wire reads, 0 = 1-bit delay from transmit to receive during One-Wire reads
215  // SWL[2:0] Software Lock Mode Control 101b = SOFTWARE LOCK (Runs from RAM), etc....
216  // XP[5:3] OTP Bank Selection for Software Lock Mode If XP[5:3] = '000', then the PGA308 operates from data written into the RAM registers from the user.
217  SFTCregister() : value(0) {}
218 
219  SFTCregister(std::uint16_t v) : value(v) {}
220 
221  void reset() { value = 0; }
222 
223  void set(std::uint16_t sftc) { value = sftc; }
224 
225  void setDefault() { value = SFTCregister::Default; }
226  };
227 
228 
229 
230 
232  {
233 
234  static const std::uint16_t VREF = 8192; // the allowed values are in range [0, VREF). it must be constant ...
235 
236  std::uint16_t GD; // gain DAC. values are [0.333333333, 0.999989824]
237  std::uint8_t GI : 4; // front end gain. from 0000b to 1101b: {4 6 8 12 16 32 64 100 200 400 480 600 800 960 1200 1600}
238  std::uint8_t muxsign : 1; // the sign: 0 is +, 1 is -
239  std::uint8_t GO : 3; // output gain. from 000b to 111b: {2.0, 2.4, 3, 3.6, 4.0, 4.5, 6.0, disable-internal-feedback}
240  std::uint8_t Vcoarseoffset; // Vcoarseoffset is stored as sign+value and must be in range [-100, +100].
241  std::uint16_t Vzerodac;
242 
243  enum class Parameter { GD = 0, GI = 1, muxsign = 2, GO = 3, Vcoarseoffset = 4, Vzerodac = 5 };
244 
245  // the formulas are:
246  //
247  // from Vin to Vout
248  // Vout = ((muxsign*Vin + Vcoarseoffset)*GI + Vzerodac)*GD*GO
249  // Vout = alpha * Vin + beta
250  // alpha = muxsign*GI*GD*GO
251  // beta = (Vcoarseoffset*GI + Vzerodac)*GD*GO
252  //
253  // we use only three variables x, y, z and we keep the other fixed to default ...
254  // x -> content of register GDAC (.GD)
255  // y -> content of register CFG0.OS (.Vcoarseoffset)
256  // z -> content of register ZDAC (.Vzerodac)
257  // we thus have:
258  // alpha(x) = valueOf(GI) * valueOf(GO) * ((1/3) + ((2/3)/64k) * x)
259  // beta(x, y, z) = alpha(x) * ((1/128)*VREF*COSVR*y + (1/valueOf(GI))*VREF*(1/64k)(32k - z))
260  // for some operations we need to represent Vout as a function only of y and z, hence
261  // Vout = a * y + b * z + c
262  // a = (d/dy) Vout = (d/dy) beta = alpha(x)*(1/128)*VREF*COSVR
263  // b = (d/dz) Vout = (d/dz) beta = - alpha(x)*(1/valueOf(GI))*VREF*(1/64k)
264  // c = Vout - a*y - b*z
265 
266 
267  TransferFunctionConfig() : GD(0), GI(0), muxsign(0), GO(0), Vcoarseoffset(0), Vzerodac(0) {}
268 
269  void load(const Registers &regs)
270  {
271  GD = regs.GD;
272  GI = regs.GI;
273  muxsign = regs.S;
274  GO = regs.GO;
275  Vcoarseoffset = regs.Voffsetcoarse;
276  Vzerodac = regs.Vzerodac;
277  }
278 
279  void get(Registers &regs)
280  {
281  regs.GD = GD;
282  regs.GI = GI;
283  regs.S = muxsign;
284  regs.GO = GO;
285  regs.Voffsetcoarse = Vcoarseoffset;
286  regs.Vzerodac = Vzerodac;
287  }
288 
289  void setDefault()
290  {
291  GD = GDACregister::Default;
292  GI = CFG0register::DefaultGI;
293  muxsign = CFG0register::DefaultMUX;
294  GO = CFG0register::DefaultGO;
295  Vcoarseoffset = CFG0register::DefaultOS;
296  Vzerodac = ZDACregister::Default;
297  }
298 
299  // from internal memory to the values of the registers to be written into the amplifier
300  void obtain(CFG0register &cfg0, ZDACregister &zdac, GDACregister &gdac) const
301  {
302  cfg0.reset(); cfg0.setGO(GO); cfg0.setMUX(muxsign); cfg0.setGI(GI); cfg0.setOS(Vcoarseoffset);
303  zdac.reset(); zdac.value = Vzerodac;
304  gdac.reset(); gdac.value = GD;
305  }
306 
307  // from the registers read from the amplifier to internal memory.
308  void assign(const CFG0register &cfg0, const ZDACregister &zdac, const GDACregister &gdac)
309  {
310  GO = cfg0.getGO();
311  muxsign = cfg0.getMUX();
312  GI = cfg0.getGI();
313  Vcoarseoffset = cfg0.getOS();
314  Vzerodac = zdac.value;
315  GD = gdac.value;
316  }
317 
318  void load(const CFG0register &cfg0)
319  {
320  GO = cfg0.getGO();
321  muxsign = cfg0.getMUX();
322  GI = cfg0.getGI();
323  Vcoarseoffset = cfg0.getOS();
324  }
325 
326  void load(const ZDACregister &zdac)
327  {
328  Vzerodac = zdac.value;
329  }
330 
331  void load(const GDACregister &gdac)
332  {
333  GD = gdac.value;
334  }
335 
336  float valueOfGI()
337  {
338  static const std::uint16_t mapGI2val[16] = {4, 6, 8, 12, 16, 32, 64, 100, 200, 400, 480, 600, 800, 960, 1200, 1600};
339  return static_cast<float>(mapGI2val[GI]);
340  }
341 
342  float valueOfGO()
343  {
344  static const float mapGO2val[8] = {2.0f, 2.4f, 3.0f, 3.6f, 4.0f, 4.5f, 6.0f, 1.0f};
345  return mapGO2val[GO];
346  }
347 
348  float valueOfGD()
349  {
350  //return 0.33333f + (static_cast<float>(GD) * 0.66666f) / 65536.0f;
351  //return (1.0f + static_cast<float>(GD)/32768.0f)/3.0f;
352  return valueOfGD(GD);
353  }
354 
355  float valueOfGD(std::uint16_t x)
356  {
357  //return 0.33333f + (static_cast<float>(GD) * 0.66666f) / 65536.0f;
358  return (1.0f + static_cast<float>(x)/32768.0f)/3.0f;
359  }
360 
361  float valueOfCOR()
362  {
363  //static const float mapCOSVR2val[4] = {0.064f, 0.0427f, 0.0320f, 0.0256f};
364  //std::uint8_t cosvr2 = 1;
365  //return static_cast<float>(VREF)*(1.0f/128.0f)*mapCOSVR2val[cosvr2];
366  return 2.7328f;
367  }
368 
369  float regvco2value(std::uint8_t co)
370  { // regvco is inside [-100, +100] in sign-value format
371  std::uint8_t v = co & 0x7f;
372  if(v > 100) { v = 100; }
373  std::uint8_t negative = co >> 7;
374  return (1 == negative) ? -v : +v;
375  }
376 
377  std::uint8_t value2regvco(float v)
378  { // regvco is inside [-100, +100] in sign-value format
379  std:: uint8_t r = 0;
380  if(v > 0)
381  {
382  v = std::floor(v + 0.5f);
383  if(v > +100.0f) { v = +100.0f; }
384  r = static_cast<std::uint8_t>(v);
385  }
386  else
387  {
388  v = -v;
389  v = std::floor(v + 0.5f);
390  if(v > +100.0f) { v = +100.0f; }
391  r = static_cast<std::uint8_t>(v) | 0x80;
392  }
393 
394  return r;
395  }
396 
398  {
399  return valueOfCOR()*regvco2value(Vcoarseoffset);
400  }
401 
403  {
404  return static_cast<float>(VREF)*(1.0f/65536.0f)*(32768.0f - static_cast<float>(Vzerodac));
405  //return static_cast<float>(VREF)*(0.0000152587890625f)*(32768.0f - static_cast<float>(Vzerodac));
406  }
407 
408  float alpha()
409  {
410  //float v = valueOfGD()*valueOfGO()*valueOfGI();
411  //return (0 == muxsign) ? v : -v;
412  return alpha(GD);
413  }
414 
415  float alpha(std::uint16_t x)
416  {
417  float v = valueOfGD(x)*valueOfGO()*valueOfGI();
418  return (0 == muxsign) ? v : -v;
419  }
420 
421  float beta()
422  {
423  float gi = valueOfGI();
424  return alpha()*(valueOfCoarseOffset() + valueOfFineOffset()/gi);
425  }
426 
427  void computeOffsetParams(const std::uint16_t vout, float &a, float &b, float &c)
428  {
429  // a = (d/dy) Vout = (d/dy) beta = alpha(x)*(1/128)*VREF*COSVR = alpha(x) * COR
430  // b = (d/dz) Vout = (d/dz) beta = - alpha(x)*(1/valueOf(GI))*VREF*(1/64k)
431  // c = Vout - a*y - b*z
432  a = alpha() * valueOfCOR();
433  b = - (alpha()*static_cast<float>(VREF)/valueOfGI())/65536.0f; // valueOfGI() is always != 0
434  c = static_cast<float>(vout) - a*regvco2value(Vcoarseoffset) - b*static_cast<float>(Vzerodac);
435  }
436 
437  bool alignVOUT(const std::uint16_t vout, const std::uint16_t target, std::uint8_t &Y, std::uint16_t &Z)
438  {
439  float a, b, c;
440  computeOffsetParams(vout, a, b, c);
441  float y = (target - c - b*static_cast<float>(Vzerodac))/a; // a is always != 0
442 
443  // round and limit y ... which must be integer and inside [-100, +100] and
444  std::uint8_t tmp = value2regvco(y);
445  y = regvco2value(tmp);
446 
447  // must apply new y to ...
448  float z = (target - c - a*y)/b;
449  if((z > 65535.0f) || (z < 0.0f))
450  {
451  return false;
452  }
453  Y = value2regvco(y);
454  Z = static_cast<std::uint16_t>(std::floor(z + 0.5f));
455  Vcoarseoffset = Y;
456  Vzerodac = Z;
457 
458  return true;
459  }
460 
461  bool setalpha(float a)
462  {
463  float xgd = 98304.0f * (a/(valueOfGI()*valueOfGO()) - (1.0f/3.0f));
464  std::int32_t x = static_cast<std::int32_t>(std::floor(xgd + 0.5f));
465  if((x >= 65536) || (x < 0))
466  {
467  return false;
468  }
469  GD = static_cast<std::uint16_t>(x);
470  return true;
471  }
472 
473  bool setbeta(float b)
474  {
475  float yco = b / (alpha() * valueOfCOR());
476  std::uint8_t y = value2regvco(yco);
477  // now we compute the fine adjustment
478  float zco = ( regvco2value(y) * valueOfCOR() + 0.5f*static_cast<float>(VREF)/valueOfGI() - b/alpha() ) / ( (static_cast<float>(VREF)/65536.0f)/valueOfGI() );
479  std::int32_t z = static_cast<std::int32_t>(std::floor(zco + 0.5f));
480  if((z >= 65536) || (z < 0))
481  {
482  return false;
483  }
484  Vcoarseoffset = y;
485  Vzerodac = static_cast<std::uint16_t>(z);
486 
487  return true;
488  }
489 
490  };
491 
493 
494 #endif // #if defined(IMPORT_CODE_FROM_EMBOT_HW_PGA308)
495 
496 
497  // data in here;
499  {
500 
501  }
503  {
504 
505  }
506 
507 
508  bool load(const DiscreteGain g)
509  {
510  // offset all equal to 32k-1
511  static const uint8_t _cfgmap[static_cast<uint8_t>(DiscreteGain::maxnumberof)][6] =
512  {
513  {0x00, 0x40, 0x46, 0x1f, 0xb1, 0x7f}, // gain = 48
514  {0x00, 0x10, 0x46, 0x2a, 0x80, 0x80}, // gain = 36
515  {0x00, 0x40, 0x42, 0x3e, 0x62, 0x7f}, // gain = 24 [or 0x26 instead of 0x42]
516  {0x00, 0x20, 0x42, 0x4b, 0x15, 0x80}, // gain = 20 [or 0x26 instead of 0x42]
517  {0x00, 0x00, 0x42, 0x5e, 0x72, 0x80}, // gain = 16 [or 0x26 instead of 0x42]
518  {0x00, 0xC0, 0x02, 0x64, 0xf6, 0x6e}, // gain = 10 [or 0x10 instead of 0x02]
519  {0x00, 0x80, 0x02, 0x64, 0x29, 0x62}, // gain = 08 [or 0x10 instead of 0x02]
520  {0x00, 0x40, 0x02, 0x64, 0xd4, 0x4c}, // gain = 06 [or 0x10 instead of 0x02]
521  {0x00, 0x40, 0x00, 0x64, 0x29, 0x22} // gain = 04
522  };
523 
524  bool ret = true;
525  switch(g)
526  {
527  case DiscreteGain::none:
528  case DiscreteGain::maxnumberof:
529  {
530  ret = false;
531  } break;
532 
533  default:
534  {
535  uint8_t index = static_cast<uint8_t>(g);
536  Registers cfg;
537  cfg.load(_cfgmap[index], 6);
538  tsf.load(cfg);
539  } break;
540 
541  }
542 
543  return ret;
544  }
545 
546  bool load_step1(const DiscreteGain g)
547  {
548  // offset are not meaningful yet. we just need zdac to be 0x8000 and cfg0.os = ... boh: 0x20
549  static const uint8_t _cfgmap[static_cast<uint8_t>(DiscreteGain::maxnumberof)][6] =
550  {
551  {0x00, 0x40, 0x46, 0x20, 0x00, 0x80}, // gain = 48
552  {0x00, 0x10, 0x46, 0x20, 0x00, 0x80}, // gain = 36
553  {0x00, 0x40, 0x42, 0x20, 0x00, 0x80}, // gain = 24 [or 0x26 instead of 0x42]
554  {0x00, 0x20, 0x42, 0x20, 0x00, 0x80}, // gain = 20 [or 0x26 instead of 0x42]
555  {0x00, 0x00, 0x42, 0x20, 0x00, 0x80}, // gain = 16 [or 0x26 instead of 0x42]
556  {0x00, 0xC0, 0x02, 0x20, 0x00, 0x80}, // gain = 10 [or 0x10 instead of 0x02]
557  {0x00, 0x80, 0x02, 0x20, 0x00, 0x80}, // gain = 08 [or 0x10 instead of 0x02]
558  {0x00, 0x40, 0x02, 0x20, 0x00, 0x80}, // gain = 06 [or 0x10 instead of 0x02]
559  {0x00, 0x40, 0x00, 0x20, 0x00, 0x80} // gain = 04
560  };
561 
562  bool ret = true;
563  switch(g)
564  {
565  case DiscreteGain::none:
566  case DiscreteGain::maxnumberof:
567  {
568  ret = false;
569  } break;
570 
571  default:
572  {
573  uint8_t index = static_cast<uint8_t>(g);
574  Registers cfg;
575  cfg.load(_cfgmap[index], 6);
576  tsf.load(cfg);
577  } break;
578 
579  }
580 
581  return ret;
582  }
583 
584  bool load(const DiscreteGain g, const Offset offset)
585  {
586  // there are some compinations of g+offset which cannot reached.
587  // if g is 6 and o8k > 7780 (ofsfet =
588  // if g is 4 and o8k > 5100 (offset =
589  if(((g == DiscreteGain::val06) && (offset > (62240))) || ((g == DiscreteGain::val04) && (offset > (40800))))
590  {
591  return false;
592  }
593 
594  // step1: we load the gain, so that we surely have some registers which are already ok.
595  // also we wnat zdac to be 0x8000
596  if(false == load_step1(g))
597  {
598  return false;
599  }
600 
601  bool ret = true;
602 
603  float o8k = offset / 8;
604 
605  // step2: we keep the values of the gain(s), and if ZDAC = 0x8000 we can compute cfg0.os in a simple mode:
606  // cfg0.os = o8k/(alpha()*valueOfCOR())
607  float cfg0os = o8k/(tsf.alpha()*tsf.valueOfCOR());
608 
609  // however, we must be sure that what we load is in range [-100, =100] and is in format sign-value.
610  int8_t Y = tsf.value2regvco(cfg0os);
611  // ok, we assign it.
612  tsf.Vcoarseoffset = Y;
613 
614  // but now we must compute the zdac with formula:
615 
616  // zdac = (64*1024)/(VREF) * ( (VREF/2) - (o8k - alpha() * valueOfCoarseOffset())/(g0*gd) )
617 
618  float z = ((64.0f*1024.0f)/tsf.VREF) * ( (tsf.VREF/2) - (o8k - tsf.alpha()*tsf.valueOfCoarseOffset())/(tsf.valueOfGO()*tsf.valueOfGD()) );
619 
620  if((z > 65535.0f) || (z < 0.0f))
621  {
622  return false;
623  }
624 
625  uint16_t Z = static_cast<std::uint16_t>(std::floor(z + 0.5f));
626  tsf.Vzerodac = Z;
627 
628  return true;
629  }
630 
631  bool get(Gain &gain, Offset &offset)
632  {
633  // this funtion extends / uses the struct TransferFunctionConfig
634  gain = tsf.alpha();
635  int32_t tmp = static_cast<int32_t>(round(8.0f*tsf.beta()));
636  if((tmp >= 0) && (tmp < 65536))
637  {
638  offset = static_cast<uint16_t>(tmp);
639  return true;
640  }
641  return false;
642  }
643 
644 
645 };
646 
647 
648 // --------------------------------------------------------------------------------------------------------------------
649 // - all the rest
650 // --------------------------------------------------------------------------------------------------------------------
651 
652 
653 
654 
655 namespace strain { namespace amplifier {
656 
657  static const float mapofgains[static_cast<uint8_t>(DiscreteGain::maxnumberof)] =
658  {
659  48, 36, 24, 20, 16, 10, 8, 6, 4
660  };
661 
663  {
664  uint8_t index = static_cast<uint8_t>(dg);
665  if(index >= static_cast<uint8_t>(DiscreteGain::maxnumberof))
666  {
667  return 0;
668  }
669  return mapofgains[index];
670  }
671 
672  bool convert(const Gain g, DiscreteGain &dg)
673  {
674  bool ret = false;
675  dg = DiscreteGain::none;
676 
677  // if g one of the allowed? i check by iteration
678  for(int i=0; i<static_cast<uint8_t>(DiscreteGain::maxnumberof); i++)
679  {
680  if(g == mapofgains[i])
681  {
682  dg = static_cast<DiscreteGain>(i);
683  ret = true;
684  break;
685  }
686  }
687 
688  return ret;
689  }
690 
691  bool convert(const WideParams &wp, DiscreteParams &dp)
692  {
693  DiscreteGain dg = DiscreteGain::none;
694  if(false == convert(wp.g, dg))
695  {
696  return false;
697  }
698  dp.dg = dg;
699  dp.o = wp.o;
700 
701  return true;
702  }
703 
704 
705  const std::uint8_t PGA308::Registers::defval[PGA308::Registers::sizeofregisters] = {0x00, 0x40, 0x46, 0x1f, 0xb1, 0x7f}; // gain = 48, offset = midrangeOffset
706 
707  bool PGA308::Registers::load(const void *data, const size_t size)
708  {
709  if((nullptr == data) || (size != sizeofregisters))
710  {
711  GD = GI = S = GO = Voffsetcoarse = Vzerodac = 0;
712  return false;
713  }
714  const uint16_t *u16 = reinterpret_cast<const uint16_t*>(data);
715  const uint8_t *u08 = reinterpret_cast<const uint8_t*>(data);
716  GD = u16[0]; Vzerodac = u16[2];
717  GI = (u08[2] >> 4) & 0x0f;
718  S = (u08[2] >> 3) & 0x01;
719  GO = (u08[2]) & 0x07;
720  Voffsetcoarse = u08[3];
721  return true;
722  }
723 
724  bool PGA308::Registers::fill(void *data, size_t &size)
725  {
726  if(nullptr == data)
727  {
728  return false;
729  }
730  uint16_t *u16 = reinterpret_cast<uint16_t*>(data);
731  uint8_t *u08 = reinterpret_cast<uint8_t*>(data);
732  u16[0] = GD; u16[2] = Vzerodac;
733  u08[2] = 0;
734  u08[2] = ((static_cast<uint8_t>(GI) & 0x0f) << 4) | ((static_cast<uint8_t>(S) & 0x01) << 3) | ((static_cast<uint8_t>(GO) & 0x07));
735  u08[3] = Voffsetcoarse;
736  size = sizeofregisters;
737  return true;
738  }
739 
740 
741  // struct basicRegs
742  // {
743  // std::uint16_t GD; // it is a gain register
744  // std::uint8_t GI : 4; // it is a gain register
745  // std::uint8_t S : 1; // it is a sign of gain register (keep it always 0)
746  // std::uint8_t GO : 3; // it is a gain register
747  // std::uint8_t Voffsetcoarse; // it is an offset register
748  // std::uint16_t Vzerodac; // it is an offset register
749 
750  // basicRegs() : GD(0), GI(0), S(0), GO(0), Voffsetcoarse(0), Vzerodac(0) {}
751  // basicRegs(void *data, size_t s) { load(data, s); }
752  // void load(const void *data, const size_t s); // import from a can frame
753  // bool fill(void *data, size_t &s); // export to a can frame
754  // };
755 
756 // void PGA308::basicRegs::load(const void *data, const size_t s)
757 // {
758 // if(nullptr == data) { GD = GI = S = GO = Voffsetcoarse = Vzerodac = 0; return; }
759 // const uint16_t *u16 = reinterpret_cast<const uint16_t*>(data);
760 // const uint8_t *u08 = reinterpret_cast<const uint8_t*>(data);
761 // GD = u16[0]; Vzerodac = u16[2];
762 // GI = (u08[2] >> 4) & 0x0f;
763 // S = (u08[2] >> 3) & 0x01;
764 // GO = (u08[2]) & 0x07;
765 // Voffsetcoarse = u08[3];
766 // }
767 
768 // bool PGA308::basicRegs::fill(void *data, size_t &s)
769 // {
770 // if(nullptr == data)
771 // {
772 // return false;
773 // }
774 // uint16_t *u16 = reinterpret_cast<uint16_t*>(data);
775 // uint8_t *u08 = reinterpret_cast<uint8_t*>(data);
776 // u16[0] = GD; u16[2] = Vzerodac;
777 // u08[2] = 0;
778 // u08[2] = ((static_cast<uint8_t>(GI) & 0x0f) << 4) | ((static_cast<uint8_t>(S) & 0x01) << 3) | ((static_cast<uint8_t>(GO) & 0x07));
779 // u08[3] = Voffsetcoarse;
780 // s = 6;
781 // return true;
782 // }
783 
784 
785 
786  PGA308::PGA308()
787  : pImpl(new Impl)
788  {
789 
790  }
791 
793  {
794  delete pImpl;
795  }
796 
797  bool PGA308::import(const Registers &regs, WideParams *wideparams)
798  {
799  load(regs);
800 
801  if(nullptr != wideparams)
802  {
803  get(wideparams->g, wideparams->o);
804  }
805 
806  return true;
807  }
808 
809  bool PGA308::import(const DiscreteParams &params, Registers *regs)
810  {
811  load(params);
812 
813  if(nullptr != regs)
814  {
815  get(*regs);
816  }
817 
818  return true;
819  }
820 
821 
822 
823  bool PGA308::load(const Registers &regs)
824  {
825  pImpl->tsf.load(regs);
826  return true;
827  }
828 
829  // attempts to load into the PGA308 a pair discretegain-offset, which produce a given Registers.
830  bool PGA308::load(const DiscreteGain g, const Offset offset)
831  {
832  if(midrangeOffset == offset)
833  {
834  return pImpl->load(g);
835  }
836 
837  return pImpl->load(g, offset);
838  }
839 
840  bool PGA308::load(const DiscreteParams &discreteparams)
841  {
842  return load(discreteparams.dg, discreteparams.o);
843  }
844 
845  // retrieve the pair gain-offset which is effectively in use after a load operation
847  {
848  return pImpl->get(gain, offset);
849  }
850 
851  bool PGA308::get(WideParams &wideparams)
852  {
853  return get(wideparams.g, wideparams.o);
854  }
855 
856 // bool PGA308::get(Params &params)
857 // {
858 // return get(params.g, params.o);
859 // }
860 
861  // retrieve the registers effectively in use.
862  bool PGA308::get(Registers &regs)
863  {
864  pImpl->tsf.get(regs);
865  return true;
866  }
867 
868 
869 
870 }} // namespace strain { namespace amplifier {
871 
872 
873 
874 // --------------------------------------------------------------------------------------------------------------------
875 // - test code
876 // --------------------------------------------------------------------------------------------------------------------
877 
878 namespace strain { namespace amplifier {
879 
880  // example of use.
881  // we must have an array of 6 PGA308 objects
882  // in the widget of the firmwareupdater:strainwindow we retrieve for each channel of the strain2 two values: a gain and an offset.
883  // we must have a
884 
886  {
887  uint8_t data[6];
888  Example_strain2_ampl_regs_t() { data[0] = data[1] = data[2] = data[3] = data[4] = data[5] = 0; }
889  void load(void *mem) { memmove(data, mem, sizeof(data)); }
890  void * memory() { return data; }
891  size_t size() { return 6; }
892  };
893 
894 
895  void testIT(std::string &output)
896  {
897 
898  const bool step1 = true;
899 
900  const bool step2 = true;
901 
902  // we use service object pga just to convert from and to.
903  PGA308 pga;
904 
906 
907  if(true == step1)
908  {
909  // in here we get two values of gain+offset (e.g., from the gui), we load int the pga and we retrieve the registers to send to the board
910  WideParams wp;
911  DiscreteParams dp;
912  PGA308::Registers regs;
913 
914 
915  wp.load(4.0, 32*1024);
916  if(true == convert(wp, dp))
917  {
918  // load dp and ...
919  //PGA308::Registers regs;
920  pga.import(dp, &regs);
921  // use the regs for instance by sending to a strain2_ampl_regs_t which can be initted from a void *
922  //Example_strain2_ampl_regs_t regstoTX;
923  size_t s;
924  regs.fill(regstoTX.memory(), s);
925  // now call
926  //cDownloader::strain_set_amplifier_regs(bus, id, regstoTX, strain_regset_inuse);
927  }
928 
929  //wp.load(48.0, 32*1024);
930  wp.load(48.0, 32531);
931  if(true == convert(wp, dp))
932  {
933  // load dp and ...
934  //PGA308::Registers regs;
935  pga.import(dp, &regs);
936  // use the regs for instance by sending to a strain2_ampl_regs_t which can be initted from a void *
937  //Example_strain2_ampl_regs_t regstoTX;
938  size_t s;
939  regs.fill(regstoTX.memory(), s);
940  // now call
941  //cDownloader::strain_set_amplifier_regs(bus, id, regstoTX, strain_regset_inuse);
942  }
943  output += "step1: ";
944  output += "done!";
945 
946  }
947 
949 
950  if(true == step2)
951  {
952  // in here we retrieve a register. and we compute the associated gain-offset
953 
954  RXregs = regstoTX;
955 
956  //cDownloader::strain_get_amplifier_regs(bus, id, RXregs, strain_regset_inuse);
957 
958  PGA308::Registers regs;
959  regs.load(RXregs.memory(), RXregs.size());
960 
961  // now regs has the content of what we have received from can bus
962  WideParams wparams;
963  pga.import(regs, &wparams);
964  // now we see if the wparams.g is one of the allowed
965  DiscreteParams dparams;
966  if(true == convert(wparams, dparams))
967  {
968  // use dparams
969  Gain gg = convert(dparams.dg);
970  Offset oo = dparams.o;
971  }
972  }
973 
974  }
975 
976  void exampleofuse(void)
977  {
978 
979 #if 1
980 
981  // we use service object pga just to convert from and to.
982  PGA308 pga;
983 
984  // in here we get two values of gain+offset (e.g., from the gui), we load int the pga and we retrieve the registers to send to the board
985  WideParams wp;
986  wp.load(48.0, 32*1024);
987  DiscreteParams dp;
988  if(true == convert(wp, dp))
989  {
990  // load dp and ...
991  PGA308::Registers regs;
992  pga.import(dp, &regs);
993  // use the regs for instance by sending to a strain2_ampl_regs_t which can be initted from a void *
995  size_t s;
996  regs.fill(regstoTX.memory(), s);
997  // now call
998  //cDownloader::strain_set_amplifier_regs(bus, id, regstoTX, strain_regset_inuse);
999  }
1000 
1001  // in here we retrieve a register. and we compute the associated gain-offset
1002 
1004 
1005  //cDownloader::strain_get_amplifier_regs(bus, id, RXregs, strain_regset_inuse);
1006 
1007  PGA308::Registers regs;
1008  regs.load(RXregs.memory(), RXregs.size());
1009 
1010  // now regs has the content of what we have received from can bus
1011  WideParams wparams;
1012  pga.import(regs, &wparams);
1013  // now we see if the wparams.g is one of the allowed
1014  DiscreteParams dparams;
1015  if(true == convert(wparams, dparams))
1016  {
1017  // use dparams
1018  Gain gg = convert(dp.dg);
1019  Offset oo = dp.o;
1020  }
1021 
1022 
1023 
1024 #else
1025  // we have one for each channel
1026  PGA308 thePGAs[6];
1027 
1028  // channel 0:
1029  DiscreteParams discreteparams;
1030  // get the value from teh widget
1031  discreteparams.load(DiscreteGain::val10, 32000);
1032  // load them into the object
1033  thePGAs[0].load(discreteparams);
1034  // now i want to send the packet to the board. i retrieve teh registers .
1035  PGA308::Registers regs;
1036  thePGAs[0].get(regs);
1037  // and now i transform them into a payload for can tx
1038  uint8_t payload[8] = {0};
1039  regs.fill(payload);
1040  // and now i use payload to tx.
1041 
1042  // now i ask the registers to the board. i receive a payload
1043  uint8_t rxpayload[8] = {0}; // ok, not zero but ...
1044  regs.load(rxpayload);
1045  // and i want to fill the widget with the real values
1046  PGA308::Registers rxregs;
1047  rxregs.load(rxpayload);
1048  float gg =0;
1049  uint16_t oo =0;
1050  thePGAs[0].load(rxregs);
1051  thePGAs[0].get(gg, oo);
1052  // print gg, oo
1053 #endif
1054 
1055  }
1056 
1057 
1058 }} // namespace strain { namespace amplifier
1059 
1060 
1061 
1062 namespace strain { namespace dsp { namespace fsc {
1063 
1064  FSC convert(const double v, bool &saturated)
1065  {
1066  saturated = false;
1067  FSC ret = static_cast<FSC>(v);
1068 
1069  if(v>fsc::max)
1070  {
1071  saturated = true;
1072  ret = fsc::max;
1073  }
1074 
1075  if(v<fsc::min)
1076  {
1077  saturated = true;
1078  ret = fsc::min;
1079  }
1080 
1081  return ret;
1082  }
1083 
1084  double convert(const FSC v)
1085  {
1086  return static_cast<double>(v);
1087  }
1088 
1089 
1090  bool convert(const std::vector<double> &in, std::vector<FSC> &out)
1091  {
1092  bool sat = false;
1093  bool ret = true;
1094  out.resize(0);
1095  for(int i=0; i<in.size(); i++)
1096  {
1097  out.push_back(strain::dsp::fsc::convert(in[i], sat));
1098  if(sat)
1099  {
1100  ret = false;
1101  }
1102  }
1103  return ret;
1104  }
1105 
1106  bool convert(const std::vector<FSC> &in, std::vector<double> &out)
1107  {
1108  bool ret = true;
1109  out.resize(0);
1110  for(int i=0; i<in.size(); i++)
1111  {
1112  out.push_back(strain::dsp::fsc::convert(in[i]));
1113  }
1114  return ret;
1115  }
1116 
1117 
1118 } } } // namespace strain { namespace dsp { namespace fsc {
1119 
1120 
1121 
1122 namespace strain { namespace dsp { namespace q15 {
1123 
1124  Q15 convert(const double v, bool &saturated)
1125  {
1126  Q15result rr = static_cast<Q15result>(v * 32768.0); // note: 32768.0 = 2^15
1127  return saturate(rr, saturated);
1128  }
1129 
1130  Q15 U16toQ15(const std::uint16_t valU16)
1131  {
1132  return valU16-0x8000;
1133  }
1134 
1135  bool convert(const std::vector<double> &in, std::vector<Q15> &out)
1136  {
1137  bool sat = false;
1138  bool ret = true;
1139  out.resize(0);
1140  for(int i=0; i<in.size(); i++)
1141  {
1142  out.push_back(convert(in[i], sat));
1143  if(sat)
1144  {
1145  ret = false;
1146  }
1147  }
1148  return ret;
1149  }
1150 
1151  bool convert(const std::vector<Q15> &in, std::vector<double> &out)
1152  {
1153  bool ret = true;
1154  out.resize(0);
1155  for(int i=0; i<in.size(); i++)
1156  {
1157  out.push_back(convert(in[i]));
1158  }
1159  return ret;
1160  }
1161 
1162  std::uint16_t Q15toU16(const Q15 valQ15)
1163  {
1164  return valQ15+0x8000;
1165  }
1166 
1167  double convert(const Q15 v)
1168  {
1169  return static_cast<double>(v) / 32768.0; // note: 32768.0 = 2^15
1170  }
1171 
1173  {
1174  if(negOne == v)
1175  {
1176  return posOneNearly;
1177  }
1178  return -v;
1179  }
1180 
1181  Q15 saturate(const Q15result x, bool &saturated)
1182  {
1183  if (x > 0x7FFF)
1184  {
1185  saturated = true;
1186  return 0x7FFF;
1187  }
1188  else if (x < -0x8000)
1189  {
1190  saturated = true;
1191  return -0x8000;
1192  }
1193 
1194  saturated = false;
1195  return static_cast<Q15>(x);
1196  }
1197 
1198  Q15 add(const Q15 a, const Q15 b)
1199  {
1200  bool saturated = false;
1201  return add(a, b, saturated);
1202  }
1203 
1204  Q15 add(const Q15 a, const Q15 b, bool &saturated)
1205  {
1206  Q15result tmp = static_cast<Q15result>(a) + static_cast<Q15result>(b);
1207 
1208  return saturate(tmp, saturated);
1209  }
1210 
1211  Q15 sub(Q15 &a, Q15 &b)
1212  {
1213  return a-b;
1214  }
1215 
1216  Q15 mul(const Q15 a, const Q15 b, bool &saturated)
1217  {
1218  Q15result tmp = static_cast<Q15result>(a) * static_cast<Q15result>(b);
1219 
1220  // Rounding; mid values are rounded up
1221  tmp += (1 << (15 - 1));
1222  // Correct by dividing by base
1223  tmp >>= 15;
1224  // and saturate result
1225  return saturate(tmp, saturated);
1226  }
1227 
1228  Q15 mul(const Q15 a, const Q15 b)
1229  {
1230  bool saturated = false;
1231  return mul(a, b, saturated);
1232  }
1233 
1234  Q15 div(const Q15 a, const Q15 b, bool &saturated)
1235  {
1236  Q15result n1 = static_cast<Q15result>(a);
1237  Q15result n2 = static_cast<Q15result>(b);
1238  Q15result rr = 0;
1239  std::uint8_t neg = 0;
1240 
1241  if(0 == n2)
1242  {
1243  saturated = true;
1244  return 0;
1245  }
1246 
1247  if(n1 < 0)
1248  {
1249  neg = !neg;
1250  n1 = -n1;
1251  }
1252 
1253  if(n2 < 0)
1254  {
1255  neg = !neg;
1256  n2 = -n2;
1257  }
1258 
1259  rr = (n1 << 15) / n2;
1260 
1261  if(0 != neg)
1262  {
1263  rr = - rr;
1264  }
1265 
1266  return saturate(rr, saturated);
1267  }
1268 
1269  bool add(const matrix &m1, const matrix &m2, matrix &res, bool &saturated)
1270  {
1271  if((m1.ncols != m2.ncols) || (m1.ncols != res.ncols))
1272  {
1273  return false;
1274  }
1275  if((m1.nrows != m2.nrows) || (m1.nrows != res.nrows))
1276  {
1277  return false;
1278  }
1279 
1280  saturated = false;
1281 
1282  // ok, we do action
1283  for(int r=0; r<res.nrows; r++)
1284  {
1285  for(int c=0; c<res.ncols; c++)
1286  {
1287  bool sat1 = false;
1288  res.data[r*res.ncols+c] = add(m1.data[r*m1.ncols+c], m2.data[r*m2.ncols+c], sat1);
1289 
1290  if(sat1)
1291  {
1292  saturated = true;
1293  }
1294  }
1295  }
1296 
1297  return true;
1298  }
1299 
1300  bool multiply(const matrix &m1, const matrix &m2, matrix &res, bool &saturated)
1301  {
1302  if(m1.ncols != m2.nrows)
1303  {
1304  return false;
1305  }
1306 
1307  if(m1.nrows != res.nrows)
1308  {
1309  return false;
1310  }
1311 
1312  if(m2.ncols != res.ncols)
1313  {
1314  return false;
1315  }
1316 
1317  saturated = false;
1318  std::uint32_t nsat = 0;
1319 
1320  // ok, we do action
1321  for(int r=0; r<res.nrows; r++)
1322  {
1323  for(int c=0; c<res.ncols; c++)
1324  {
1325  Q15 v = 0;
1326 
1327  const Q15 *row_wise = &m1.data[r*m1.ncols]; // navigo con +1
1328  const Q15 *col_wise = &m2.data[c]; // navigo con +(m2.ncols)
1329  for(int i=0; i<m1.ncols; i++)
1330  {
1331  bool sat1 = false;
1332  bool sat2 = false;
1333  Q15 x = mul(*row_wise, *col_wise, sat1);
1334  v = add(v, x, sat2);
1335  if(sat1 || sat2)
1336  {
1337  nsat ++;
1338  saturated = true;
1339  }
1340  row_wise += 1;
1341  col_wise += m2.ncols;
1342  }
1343 
1344  res.data[c + r*res.ncols] = v;
1345  }
1346  }
1347 
1348  return true;
1349  }
1350 
1351 } } } // namespace strain { namespace dsp { namespace q15 {
1352 
1353 namespace strain { namespace regulation {
1354 
1355  bool read(const std::string fname, FullRegulation &reg)
1356  {
1357  bool ret = false;
1358 #if 0
1359  // to be done later on
1360 
1361  reg.clear();
1362 
1363  if(fname.empty())
1364  {
1365 // yError("File not found!\n");
1366 // appendLogMsg("File not found!");
1367  return false;
1368  }
1369 
1370 // if (selected_id <1 || selected_id >= 15){
1371 // yError("Invalid board address!\n");
1372 // appendLogMsg("Invalid board address!");
1373 // return false;
1374 // }
1375 
1376  const char * filename = fname.c_str();
1377  int file_version=0;
1378  std::fstream filestr;
1379  filestr.open (filename, fstream::in);
1380  if (!filestr.is_open()){
1381 // yError("Error opening calibration file!\n");
1382 // appendLogMsg("Error opening calibration file!");
1383  return false;
1384  }
1385 
1386  int i=0;
1387  char buffer[256];
1388 
1389  //file version
1390  filestr.getline (buffer,256);
1391  filestr.getline (buffer,256);
1392  sscanf (buffer,"%d",&file_version);
1393 
1394 
1395  if((4 != file_version) && (3 != file_version))
1396  {
1397 // yError("Wrong file. Calibration version not supported for strain2: %d\n", file_version);
1398 // appendLogMsg("Wrong file. Calibration version not supported for strain2");
1399  return false;
1400  }
1401 
1402  reg.version = static_cast<Version>(file_version);
1403 
1404  if(Version::three == reg.version)
1405  {
1406  // Board type:
1407  filestr.getline (buffer,256);
1408  filestr.getline (buffer,256);
1409  if(0 != strcmp(buffer, "strain2"))
1410  {
1411 // yError("Wrong file. Board type not supported: %s\n", buffer);
1412 // appendLogMsg("Wrong file. Board type not supported");
1413  return false;
1414  }
1415 
1416  reg.board = Board::strain2;
1417 
1418  // Serial number:
1419  char serial_no[256] = {0};
1420  filestr.getline (buffer,256);
1421  filestr.getline (buffer,256);
1422  snprintf(serial_no, sizeof(serial_no), "%s", buffer);
1423  //core->getDownloader()->strain_set_serial_number(bus,id, serial_no);
1424 
1425  reg.serial = std::string(serial_no);
1426 
1427  // there is only one regulation set
1428 
1429  Set set;
1430  set.clear();
1431 
1432  // Amplifier registers:
1433  filestr.getline (buffer,256);
1434  for (i=0;i<6; i++)
1435  {
1436  filestr.getline (buffer,256);
1437 // yDebug() << buffer;
1438  unsigned int t08[6] = {0};
1439  sscanf (buffer,"0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x", &t08[0], &t08[1], &t08[2], &t08[3], &t08[4], &t08[5]);
1440  for(int j=0; j<6; j++)
1441  {
1442  set.analog.amplregs[i][j] = t08[j];
1443  }
1444 
1445 // core->getDownloader()->strain_set_amplifier_regs(core->getDownloader()->board_list[selected].bus, core->getDownloader()->board_list[selected].pid, i, amp_registers[i], regset);
1446 
1447  // downloader.strain_set_offset (downloader.board_list[selected].bus, downloader.board_list[selected].pid, i, offset[i]);
1448  //core->getDownloader()->strain_set_offset (bus,id, i, offset[i]);
1449  //printf("0X%02x, 0X%02x, 0X%02x, 0X%02x, 0X%02x,0X%02x", amp_registers[i].data[0], amp_registers[i].data[1], amp_registers[i].data[2], amp_registers[i].data[3], amp_registers[i].data[4], amp_registers[i].data[5]);
1450  //fflush(stdout);
1451 // drv_sleep(10);
1452  }
1453 
1454 
1455 
1456  //calibration matrix
1457  filestr.getline (buffer,256);
1458  for (i=0;i<36; i++){
1459  unsigned int tmp = 0;
1460  filestr.getline (buffer,256);
1461  sscanf (buffer,"%x", &tmp);
1462  set.digital.matrix[i] = static_cast<strain::dsp::Q15>(tmp);
1463 // core->getDownloader()->strain_set_matrix_rc(bus,id, ri, ci, calib_matrix[index][ri][ci], regset);
1464  }
1465 
1466 
1467 
1468  //matrix gain
1469  filestr.getline (buffer,256);
1470  filestr.getline (buffer,256);
1471  int cc=0;
1472  sscanf (buffer,"%d",&cc);
1473 // core->getDownloader()->strain_set_matrix_gain(bus,id, cc, regset);
1474 
1475  //tare
1476  filestr.getline (buffer,256);
1477  for (i=0;i<6; i++){
1478  filestr.getline (buffer,256);
1479  int tt = 0;
1480  sscanf (buffer,"%d", &tt);
1481  set.digital.tare[i] = static_cast<std::uint16_t>(tt);
1482 // core->getDownloader()->strain_set_calib_bias(bus,id, i, calib_bias[i], regset);
1483  }
1484 
1485  //full scale values
1486  filestr.getline (buffer,256);
1487  for (i=0;i<6; i++){
1488  filestr.getline (buffer,256);
1489  int fs = 0;
1490  sscanf (buffer,"%d", &fs);
1491  set.digital.fullscale[i] = static_cast<strain::dsp::FSC>(fs);
1492 // core->getDownloader()->strain_set_full_scale(bus,id, i, full_scale_const[index][i], regset);
1493  }
1494 
1495  reg.sets.push_back(set);
1496 
1497  }
1498 
1499  reg.set2useatbootstrap = 1;
1500 
1501 
1502  filestr.close();
1503  filestr.clear();
1504 
1505 #endif
1506 
1507  return ret;
1508  }
1509 
1510 } } // namespace strain { namespace regulation
1511 
1512 
1513 // - end-of-file (leave a blank line after)----------------------------------------------------------------------------
1514 
bool load_step1(const DiscreteGain g)
Definition: strain.cpp:546
bool load(const Registers &regs)
Definition: strain.cpp:823
virtual bool load(const void *data, const size_t size)
Definition: strain.cpp:707
bool load(const DiscreteGain g)
Definition: strain.cpp:508
void computeOffsetParams(const std::uint16_t vout, float &a, float &b, float &c)
Definition: strain.cpp:427
FSC convert(const double v, bool &saturated)
Definition: strain.cpp:1064
void exampleofuse(void)
Definition: strain.cpp:976
static const float mapofgains[static_cast< uint8_t >(DiscreteGain::maxnumberof)]
Definition: strain.cpp:657
void load(DiscreteGain _dg, Offset _o)
Definition: strain.h:148
out
Definition: sine.m:8
std::uint8_t ncols
Definition: strain.h:96
Q15 saturate(const Q15result x, bool &saturated)
Definition: strain.cpp:1181
STL namespace.
TransferFunctionConfig tsf
Definition: strain.cpp:492
void testIT(std::string &output)
Definition: strain.cpp:895
bool get(WideParams &wideparams)
Definition: strain.cpp:851
std::uint16_t Offset
Definition: strain.h:121
Q15 sub(Q15 &a, Q15 &b)
Definition: strain.cpp:1211
Gain convert(const DiscreteGain dg)
Definition: strain.cpp:662
bool multiply(const matrix &m1, const matrix &m2, matrix &res, bool &saturated)
Definition: strain.cpp:1300
std::vector< Set > sets
Definition: strain.h:327
const FSC max
Definition: strain.h:48
bool import(const Registers &regs, WideParams *wideparams=nullptr)
Definition: strain.cpp:797
static int v
Definition: iCub_Sim.cpp:42
Q15 U16toQ15(const std::uint16_t valU16)
Definition: strain.cpp:1130
std::int32_t Q15result
Definition: strain.h:42
void assign(const CFG0register &cfg0, const ZDACregister &zdac, const GDACregister &gdac)
Definition: strain.cpp:308
const FSC min
Definition: strain.h:49
Q15 mul(const Q15 a, const Q15 b)
Definition: strain.cpp:1228
void load(Gain _g, Offset _o)
Definition: strain.h:157
std::uint16_t Q15toU16(const Q15 valQ15)
Definition: strain.cpp:1162
bool add(const matrix &m1, const matrix &m2, matrix &res, bool &saturated)
Definition: strain.cpp:1269
void obtain(CFG0register &cfg0, ZDACregister &zdac, GDACregister &gdac) const
Definition: strain.cpp:300
bool get(Gain &gain, Offset &offset)
Definition: strain.cpp:631
bool read(const std::string fname, FullRegulation &reg)
Definition: strain.cpp:1355
Q15 div(const Q15 a, const Q15 b, bool &saturated)
Definition: strain.cpp:1234
Q15 opposite(const Q15 v)
Definition: strain.cpp:1172
std::uint16_t FSC
Definition: strain.h:40
std::uint8_t nrows
Definition: strain.h:95
const Q15 negOne
Definition: strain.h:61
const Offset midrangeOffset
Definition: strain.h:139
double sat(const double val, const double min, const double max)
Definition: utils.h:185
degrees offset
Definition: sine.m:4
CFG0register(std::uint8_t GO, std::uint8_t MUX, std::uint8_t GI, std::uint8_t OS)
Definition: strain.cpp:124
std::int16_t Q15
Definition: strain.h:41
const Q15 posOneNearly
Definition: strain.h:72
virtual bool fill(void *data, size_t &size)
Definition: strain.cpp:724
bool alignVOUT(const std::uint16_t vout, const std::uint16_t target, std::uint8_t &Y, std::uint16_t &Z)
Definition: strain.cpp:437
double convert(const Q15 v)
Definition: strain.cpp:1167
bool load(const DiscreteGain g, const Offset offset)
Definition: strain.cpp:584