iCub-main
strain.h
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 // - include guard ----------------------------------------------------------------------------------------------------
20 
21 #ifndef _STRAIN_H_
22 #define _STRAIN_H_
23 
24 #include <cmath>
25 #include <cstdint>
26 #include <cstring>
27 #include <string>
28 #include <vector>
29 
30 namespace strain {
31 
32  enum class Board { strain1 = 1, strain2 = 2 };
33 
34  const std::uint8_t numberofchannels = 6;
35 
36 } // namespace strain {
37 
38 namespace strain { namespace dsp {
39 
40  using FSC = std::uint16_t;
41  using Q15 = std::int16_t;
42  using Q15result = std::int32_t;
43 
44 } } // namespace strain { namespace dsp {
45 
46 namespace strain { namespace dsp { namespace fsc {
47 
48  const FSC max = 64*1024-1;
49  const FSC min = 0;
50 
51  FSC convert(const double v, bool &saturated);
52  double convert(const FSC v);
53 
54  bool convert(const std::vector<double> &in, std::vector<FSC> &out);
55  bool convert(const std::vector<FSC> &in, std::vector<double> &out);
56 
57 } } } // namespace strain { namespace dsp { namespace fsc {
58 
59 namespace strain { namespace dsp { namespace q15 {
60 
61  const Q15 negOne = 0x8000;
62  const Q15 negOneNearly = 0x8001; // -1+2^(-15) = -0.999969482421875
63  const Q15 negOneHalf = 0xC000; // -1+2^(-1) = -1.00+0.50 = -0.50
64  const Q15 negOneFourth = 0xE000; // -1+2^(-1)+2^(-2) = -1.00+0.50+0.25 = -0.25
65  const Q15 negOneEigth = 0xF000; // -1+2^(-1)+2^(-2)+2^(-3) = -1.00+0.50+0.25+0.125 = -0.125
66  const Q15 negEPSILON = 0xFFFF; // -1+sum_(i=-1,..,-15)(2^i) = -0.000030517578125
67  const Q15 zero = 0;
68  const Q15 posEPSILON = 0x0001; // 2^(-15) = 0.000030517578125
69  const Q15 posOneHalf = 0x4000; // 2^(-1) = 0.5
70  const Q15 posOneFourth = 0x2000; // 2^(-2) = 0.25
71  const Q15 posOneEigth = 0x4000; // 2^(-3) = 0.125
72  const Q15 posOneNearly = 0x7FFF; // sum_(i=-1,..,-15)(2^i) = 1-2^(-15) = 0.999969482421875
73 
74  Q15 convert(const double v, bool &saturated);
75  double convert(const Q15 v);
76 
77  bool convert(const std::vector<double> &in, std::vector<Q15> &out);
78  bool convert(const std::vector<Q15> &in, std::vector<double> &out);
79 
80  Q15 U16toQ15(const std::uint16_t valU16); // transforms a value in range [0, 64k-1] into a Q15
81  std::uint16_t Q15toU16(const Q15 valQ15); // transforms a Q15 into range [0, 64k-1]
82 
83  Q15 opposite(const Q15 v); // opposite of negOne is posOneNearly
84  Q15 saturate(const Q15result x, bool &saturated);
85  Q15 add(const Q15 a, const Q15 b);
86  Q15 add(const Q15 a, const Q15 b, bool &saturated);
87  Q15 sub(const Q15 a, const Q15 b);
88  Q15 mul(const Q15 a, const Q15 b);
89  Q15 mul(const Q15 a, const Q15 b, bool &saturated);
90  Q15 div(const Q15 a, const Q15 b, bool &saturated);
91 
92 
93  struct matrix
94  {
95  std::uint8_t nrows;
96  std::uint8_t ncols;
97  Q15* data; // organised by row
98 
99  void load(std::uint8_t r, std::uint8_t c, Q15* d) { nrows = r; ncols = c; data = d; }
100  matrix() { load(0, 0, nullptr); }
101  matrix(std::uint8_t r, std::uint8_t c, Q15* d) { load(r, c, d); }
102  Q15 get(std::uint8_t r, std::uint8_t c) { if((r<nrows) && (c<ncols) && (nullptr != data)) { return data[c + r*ncols]; } else { return 0; } }
103  void set(std::uint8_t r, std::uint8_t c, Q15 v) { if((r<nrows) && (c<ncols) && (nullptr != data)) { data[c + r*ncols] = v; } }
104  void clear() { if(nullptr != data) { std::memset(data, 0, sizeof(Q15)*ncols*nrows); } }
105  void diagonal(Q15 v) { clear(); if(nullptr != data) { std::uint8_t min = (ncols<nrows) ? (ncols) : (nrows); for(int i=0; i<min; i++) data[i+i*ncols] = v; } }
106  void fill(Q15 v) { clear(); if(nullptr != data) { for(int r=0; r<nrows; r++) for(int c=0; c<nrows; c++) data[c+r*ncols] = v; } }
107  };
108 
109  bool multiply(const matrix &m1, const matrix &m2, matrix &res, bool &saturated);
110  bool add(const matrix &m1, const matrix &m2, matrix &res, bool &saturated);
111 
112 } } } // namespace strain { namespace dsp { namespace q15 {
113 
114 
115 
116 namespace strain { namespace amplifier {
117 
118  // according to formula: Vout = gain * Vin + offset
119  // gain is a float, and offset is a positive integer in range [0, 64k)
120  using Gain = float;
121  using Offset = std::uint16_t;
122 
123  // but gain cannot be any real number because it is given by a limited combination of registers.
124  // the exact definition of the possible values is possible but very complicated, hence we decide
125  // to simplify and allow only a limited number of possible values whcih have shown to be useful.
126  // they are the DiscreteGain
127  enum class DiscreteGain { val48 = 0, val36 = 1, val24 = 2, val20 = 3, val16 = 4, val10 = 5, val08 = 6, val06 = 7, val04 = 8, none = 32, maxnumberof = 9 };
128 
129  // with this we convert from the enum to the real value (should you use it in some debug message)
131 
132  // with this we convert a float gain into a discrete one. but only if the float gain is exactly equal.
133  // as an example g = 48.0f will be succesfully converted to dg = DiscreteGain::val48 and teh funtion will return true,
134  // but g = 47.0f will cause funtion to return false and dg = DiscreteGain::none
135  bool convert(const Gain g, DiscreteGain &dg);
136 
137  // we have some special offsets: the minimum, the midrange, the maximum
139  const Offset midrangeOffset = 32*1024-1;
140  const Offset maximumOffset = 64*1024-1;
141 
142  // we group in here the discrete gain + offset
144  {
147  DiscreteParams() : dg(DiscreteGain::none), o(minimumOffset) {}
148  void load(DiscreteGain _dg, Offset _o) { dg = _dg; o = _o; }
149  };
150 
151  // we group in here the float gain + offset
152  struct WideParams
153  {
156  WideParams() : g(1.0f), o(minimumOffset) {}
157  void load(Gain _g, Offset _o) { g = _g; o = _o; }
158  void load(DiscreteGain _dg, Offset _o) { g = convert(_dg); o = _o; }
159  void load(const DiscreteParams &_dp) { g = convert(_dp.dg); o = _dp.o; }
160  };
161 
162  // and in here we can convert from wide to discrete. it returns true only if conversion of Gain to DiscreteGain is possible
163  bool convert(const WideParams &wp, DiscreteParams &dp);
164 
165  // in here we define a virtual interface for the registers
166  class IFregs
167  {
168  public:
169  virtual ~IFregs() {}
170  virtual bool load(const void *data, const size_t size) = 0; // import from a stream (e.g., a can frame)
171  virtual bool fill(void *data, size_t &size) = 0; // export to a stream (e.g., a can frame)
172  virtual std::uint8_t size() = 0;
173  };
174 
175  // and now we have a class which manages a particular amplifier: the PGA308.
176  // transformations between discrete params of the amplifier in the form of DiscreteParams
177  // towards the registers of the PGA308. And also the transformation from the registers towards full range Gain+Offset
178  class PGA308
179  {
180  public:
181 
182  class Registers: public IFregs
183  {
184  public:
185  std::uint16_t GD; // it is a gain register
186  std::uint8_t GI : 4; // it is a gain register
187  std::uint8_t S : 1; // it is a sign of gain register (keep it always 0)
188  std::uint8_t GO : 3; // it is a gain register
189  std::uint8_t Voffsetcoarse; // it is an offset register
190  std::uint16_t Vzerodac; // it is an offset register
191 
192  static const std:: uint8_t sizeofregisters = 6;
193 
194  static const std::uint8_t defval[sizeofregisters];// = {0x00, 0x40, 0x46, 0x1f, 0xb1, 0x7f}; // gain = 48, offset = midrangeOffset
195 
196  Registers() : GD(0), GI(0), S(0), GO(0), Voffsetcoarse(0), Vzerodac(0) {}
197  Registers(void *data, size_t size) { load(data, size); }
198  virtual bool load(const void *data, const size_t size);
199  virtual bool fill(void *data, size_t &size);
200  virtual std::uint8_t size() { return sizeofregisters; }
201  };
202 
203 
204  PGA308();
205  ~PGA308();
206 
207  // usage: there are two possible modes:
208  // 1. import registers and retrieve gain+offset into WideParams
209  // 2. load discrete-gain+ofsfet and retrieve regs into Registers.
210 
211  bool import(const Registers &regs, WideParams *wideparams = nullptr);
212  bool get(WideParams &wideparams);
213 
214  bool import(const DiscreteParams &discreteparams, Registers *regs = nullptr);
215  bool get(Registers &regs);
216 
217 
218 
219  protected:
220 
221  // in here we put a ... protected interface
222 
223  // loads into the PGA308 the basic registers which define its behaviour in terms of gain-offset
224  // that will cause the PGA to have values of gain and offset
225  bool load(const Registers &regs);
226 
227  // attempts to load into the PGA308 a pair discretegain-offset, which produce a given Registers.
228  // the attempt can cause sligthly different values
229  bool load(const DiscreteGain g, const Offset offset = midrangeOffset);
230  bool load(const DiscreteParams &discreteparams);
231 
232  // retrieve the pair gain-offset which is effectively in use after a load operation
233  bool get(Gain &gain, Offset &offset);
234 
235 
236  bool get(DiscreteParams &discreteparams);
237 
238 
239 
240  private:
241  struct Impl;
242  Impl *pImpl;
243  };
244 
245 
246  void testIT(std::string &output);
247 
248 
249 }} // namespace strain { namespace amplifier
250 
251 
252 
253 class cDownloader;
254 
255 namespace strain { namespace regulation {
256 
257 
258  const std::uint8_t maxSets = 3;
259 
260  enum class Version { two = 2, three = 3, four = 4 };
261 
262  struct Analog1
263  { // for strain1
265  Analog1() { clear(); }
266  void clear() { std::memset(offset, 0, sizeof(offset)); }
267  };
268 
269  struct Analog
270  { // for strain2
272  Analog() { clear(); }
273  void clear() {
274  for(int i=0; i<strain::numberofchannels; i++)
275  {
277  }
278  }
279  };
280 
281 
282 
283  struct Digital
284  {
285  std::uint16_t tare[strain::numberofchannels];
288  Digital() { clear(); }
289  void clear() {
290  std::memset(tare, 0, sizeof(tare));
291  std::memset(fullscale, strain::dsp::fsc::max, sizeof(fullscale));
294  }
295  };
296 
297  struct Set1
298  { // for strain1
301  };
302 
303  struct Set
304  { // for strain2
307  Set() { clear(); }
308  void clear() { analog.clear(); digital.clear(); }
309  };
310 
311 
312 
314  {
317  std::string serial;
319  };
320 
322  {
325  std::string serial;
326  std::uint8_t set2useatbootstrap; // 1, 2 or 3
327  std::vector<Set> sets;
328  FullRegulation() { clear(); }
329  void clear() { version = Version::four; board = Board::strain2; serial = "SN666"; set2useatbootstrap = 1; sets.resize(0); }
330  };
331 
332  // it just reads from file and fills FullRegulation for use outside of this module
333  bool read(const std::string filename, FullRegulation &reg);
334 
335  // it just reads from file and fills FullRegulation for use outside of this module
336  bool write(const std::string filename, const FullRegulation &reg);
337  bool apply(cDownloader *down, const FullRegulation &reg);
338 
339 } } // namespace strain { namespace regulation {
340 
341 
342 
343 
344 
345 #endif // include-guard
346 
347 
348 // - end-of-file (leave a blank line after)----------------------------------------------------------------------------
349 
350 
351 
352 
void load(std::uint8_t r, std::uint8_t c, Q15 *d)
Definition: strain.h:99
const Q15 negOneHalf
Definition: strain.h:63
virtual std::uint8_t size()
Definition: strain.h:200
void load(DiscreteGain _dg, Offset _o)
Definition: strain.h:148
Q15 sub(const Q15 a, const Q15 b)
out
Definition: sine.m:8
std::uint8_t ncols
Definition: strain.h:96
Q15 saturate(const Q15result x, bool &saturated)
Definition: strain.cpp:1181
void testIT(std::string &output)
Definition: strain.cpp:895
std::uint16_t Offset
Definition: strain.h:121
void load(const DiscreteParams &_dp)
Definition: strain.h:159
const std::uint8_t numberofchannels
Definition: strain.h:34
Gain convert(const DiscreteGain dg)
Definition: strain.cpp:662
bool write(const std::string filename, const FullRegulation &reg)
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
const Q15 posOneHalf
Definition: strain.h:69
static int v
Definition: iCub_Sim.cpp:47
Q15 U16toQ15(const std::uint16_t valU16)
Definition: strain.cpp:1130
std::int32_t Q15result
Definition: strain.h:42
const Q15 negOneNearly
Definition: strain.h:62
const FSC min
Definition: strain.h:49
bool apply(cDownloader *down, const FullRegulation &reg)
unsigned int offset[6]
void load(Gain _g, Offset _o)
Definition: strain.h:157
std::uint16_t Q15toU16(const Q15 valQ15)
Definition: strain.cpp:1162
const Q15 posEPSILON
Definition: strain.h:68
Q15 div(const Q15 a, const Q15 b, bool &saturated)
Definition: strain.cpp:1234
void fill(Q15 v)
Definition: strain.h:106
Board
Definition: strain.h:32
const Offset maximumOffset
Definition: strain.h:140
Q15 opposite(const Q15 v)
Definition: strain.cpp:1172
matrix(std::uint8_t r, std::uint8_t c, Q15 *d)
Definition: strain.h:101
Q15 add(const Q15 a, const Q15 b)
Definition: strain.cpp:1198
std::uint16_t FSC
Definition: strain.h:40
void diagonal(Q15 v)
Definition: strain.h:105
static const std::uint8_t sizeofregisters
Definition: strain.h:192
Q15 mul(const Q15 a, const Q15 b, bool &saturated)
Definition: strain.cpp:1216
std::uint8_t nrows
Definition: strain.h:95
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
const Q15 negOne
Definition: strain.h:61
const Q15 negOneFourth
Definition: strain.h:64
const Offset midrangeOffset
Definition: strain.h:139
std::int16_t Q15
Definition: strain.h:41
Registers(void *data, size_t size)
Definition: strain.h:197
const Q15 posOneNearly
Definition: strain.h:72
const Q15 negOneEigth
Definition: strain.h:65
void load(DiscreteGain _dg, Offset _o)
Definition: strain.h:158
const std::uint8_t maxSets
Definition: strain.h:258
const Q15 zero
Definition: strain.h:67
unsigned int matrix[6][6]
const Q15 posOneFourth
Definition: strain.h:70
const Q15 negEPSILON
Definition: strain.h:66
static const std::uint8_t defval[sizeofregisters]
Definition: strain.h:194
const Q15 posOneEigth
Definition: strain.h:71
const Offset minimumOffset
Definition: strain.h:138