angle_compensator.cpp
Go to the documentation of this file.
1 
61 #ifdef _MSC_VER
62 #define _CRT_SECURE_NO_WARNINGS
63 #endif
64 
65 #ifndef _USE_MATH_DEFINES // to ensure that M_PI is defined
66 #define _USE_MATH_DEFINES
67 #endif
68 
69 
71 #include <string>
72 #include <vector>
73 #include <sstream>
74 #include <iostream>
75 #include <math.h>
76 #include <assert.h>
77 #include <string.h>
78 #include <memory.h>
79 
80 
81 using namespace std;
82 
83 /***************************************************************************************
84  *
85  * Background information is given in the Markdown file doc/angular_compensation.md
86  *
87  *
88  * The angle compensation compensates the given raw angle by using the formula
89  * compensated angle = [raw angle] + [ampl] * sin([raw_angle] + [phase_corr]) + [offset]
90  * The offset compensates a small offset deviation in [deg]
91  * The sine-wave compensation allows a compensation of a sine wave modulated deviation
92  * over 360 deg rotation
93  *
94  * Details:
95  *
96  * | xxxxx xxxxx
97  * .[phase_corr]. | xx ^ xx xx xx
98  * | xx | xx xx xx
99  * | x | x x x
100  * x x | x x x
101  * x x ----- x x x
102  * xx xx [ampl] xx xx xx xx
103  * xx xx xx xx xx xx
104  * xxxxx xxxxx xxxxx
105  *
106  * DC-Offset in [deg] corresponds to [offset]
107  *
108  ***************************************************************************************
109  */
110 
111 
116 double AngleCompensator::compensateAngleInRadFromRos(double angleInRadFromRos)
117 {
118  // this is a NAV3xx - X-Axis is the same like ROS
119  // but we rotate clockwise instead of counter clockwise
120  double angleInRadFromSickOrg = 0.0;
121  double angleInRadToRosCompensated = 0.0;
122  if (useNegSign)
123  {
124  // NAV310-Handling
125  // maps from clockwise and x-axis backwards to counter-clockwise and x-axis forwards
126  // [-Pi,Pi] --> [0, 2*Pi]
127  angleInRadFromSickOrg = -angleInRadFromRos + M_PI;
128  }
129  else // NAV2xx
130  {
131  // NAV2xx uses "standard" counter clockwise rotation like ROS, but X-Axis shows to the right
132  angleInRadFromSickOrg = angleInRadFromRos + M_PI/2.0;
133  }
134 
135  double angleInRadFromSickCompensated = compensateAngleInRad(angleInRadFromSickOrg);
136 
137  if (useNegSign) // NAV3xx
138  {
139  // NAV310-Handling
140  // maps from clockwise and x-axis backwards to counter-clockwise and x-axis forwards
141  // [0,2*pi] --> [-Pi, Pi]
142  angleInRadToRosCompensated = -angleInRadFromSickCompensated + M_PI;
143  }
144  else // NAV2xx
145  {
146  // NAV2xx uses "standard" counter clockwise rotation like ROS
147  angleInRadToRosCompensated = angleInRadFromSickCompensated - M_PI/2.0;
148  }
149  return(angleInRadToRosCompensated);
150 }
151 
156 double AngleCompensator::compensateAngleInRad(double angleInRad)
157 {
158  double deg2radFactor = 0.01745329252; // pi/180 deg - see for example: https://www.rapidtables.com/convert/number/degrees-to-radians.html
159  int sign = 1;
160  if (useNegSign)
161  {
162  sign = -1;
163  }
164  //angleInRad *= sign;
165  double angleCompInRad = angleInRad - sign * deg2radFactor * amplCorr * sin(angleInRad + sign * phaseCorrInRad)-sign * offsetCorrInRad;
166  return(angleCompInRad);
167 }
168 
173 double AngleCompensator::compensateAngleInDeg(double angleInDeg)
174 {
175  int sign = 1;
176  if (useNegSign)
177  {
178  sign = -1;
179  }
180  //angleInDeg*=sign;
181  // AngleComp =AngleRaw + AngleCompAmpl * SIN( AngleRaw + AngleCompPhase ) + AngleCompOffset
182  double angleCompInDeg;
183  double deg2radFactor = 0.01745329252; // pi/180 deg - see for example: https://www.rapidtables.com/convert/number/degrees-to-radians.html
184  double angleRawInRad = deg2radFactor * angleInDeg;
185  double phaseCorrInRad= deg2radFactor * phaseCorrInDeg;
186  angleCompInDeg = angleInDeg - sign * amplCorr * sin(angleRawInRad + sign * phaseCorrInRad) - sign * offsetCorrInDeg;
187  return(angleCompInDeg);
188 }
189 
194 int AngleCompensator::parseAsciiReply(const char *replyStr)
195 {
196  int retCode = 0;
197  std::stringstream ss(replyStr);
198  std::string token;
199  std::vector<std::string> cont;
200  cont.clear();
201  char delim = ' ';
202  while (std::getline(ss, token, delim))
203  {
204  cont.push_back(token);
205  }
206 
207 
208  // if (val > 32767) val -= 65536;
209 
210  std::string s = "fffefffe";
211  int x = std::stoul(s, nullptr, 16);
212 
213  // sizes in bit of the following values
214  // 16
215  // 32
216  // 16
217 
218  int32_t ampl10000th;
219  int32_t phase10000th;
220  int32_t offset10000th;
221 
222 
223  if (cont.size() == 5)
224  {
225  unsigned long helperArr[3] = {0};
226  for (int i = 0; i < 3; i++)
227  {
228  char ch = cont [2+i][0];
229  if ((ch == '+') || (ch == '-'))
230  {
231  sscanf(cont[2+i].c_str(),"%ld", &(helperArr[i]));
232  }
233  else
234  {
235  helperArr[i] = std::stoul(cont[2+i], nullptr, 16);
236  }
237  }
238 
239  if(useNegSign) // NAV310 uses 16/32/16 and subtracts given offset from sine-wave-compensation part
240  {
241  ampl10000th = (int32_t)((int16_t)(0xFFFF & helperArr[0]));
242  phase10000th = (int32_t)(0xFFFFFFFF & helperArr[1]); // check againt https://www.rapidtables.com/convert/number/hex-to-decimal.html
243  offset10000th = (int32_t)(int16_t)(0xFFFF & helperArr[2]);
244  }
245  else // NAV210 uses 32/32/32 and POSITIVE Sign for phase offset
246  {
247  ampl10000th = (int32_t)(0xFFFFFFFF & helperArr[0]);
248  phase10000th = (int32_t)(0xFFFFFFFF & helperArr[1]); // check againt https://www.rapidtables.com/convert/number/hex-to-decimal.html
249  offset10000th = (int32_t)(0xFFFFFFFF & helperArr[2]);
250 
251  }
252  }
253 
254  amplCorr = 1/10000.0 * ampl10000th;
255  phaseCorrInDeg = 1/10000.0 * phase10000th;
256  offsetCorrInDeg = 1/10000.0 * offset10000th;
257 
258  offsetCorrInRad = offsetCorrInDeg/180.0 * M_PI;
259  phaseCorrInRad = phaseCorrInDeg/180.0 * M_PI;
260  return (retCode);
261 }
262 
268 int AngleCompensator::parseReply(bool isBinary, std::vector<unsigned char>& replyVec)
269 {
270 
271  int retCode = 0;
272  std::string stmp;
273  int payLoadLen = 0;
274  if (isBinary) // convert binary message into the ASCII format to reuse parsing algorithm
275  {
276  stmp = "";
277  int sLen = (int)replyVec.size();
278  assert((sLen == 40) || (sLen == 36));
279 
280  switch(sLen)
281  {
282  case 36:
283  useNegSign = true; // phase compensation is negative for 310 - using the short telegram
284  payLoadLen = 8;
285  break;
286  case 40:
287  payLoadLen = 12;
288  break;
289  default:
290  assert(0);
291  break;
292  }
293 
294  int offset = sLen - payLoadLen - 1; // -1 for CRC
295  assert(replyVec[offset-1] == 0x20); // must be a space there - last test
296  int relCnt = 0;
297  for (int i = 8; i < sLen - 1; i++)
298  {
299  if (i < offset)
300  {
301  stmp += (char)replyVec[i];
302  }
303  else
304  {
305  char szTmp[3];
306  sprintf(szTmp,"%02X", replyVec[i]);
307  relCnt++;
308  stmp += szTmp;
309 
310  int posCutArr[2] = {4,8};
311  if (payLoadLen == 8)
312  {
313  posCutArr[0] = 2;
314  posCutArr[1] = 6;
315 
316  }
317  if (relCnt < payLoadLen)
318  {
319  for (int k = 0; k < 2; k++)
320  {
321  if (posCutArr[k] == relCnt)
322  {
323  stmp += ' ';
324  }
325  }
326  }
327  }
328  }
329  }
330  else
331  {
332  for (int i = 0; i < replyVec.size(); i++)
333  {
334  stmp += (char)replyVec[i];
335  }
336  }
337  parseAsciiReply(stmp.c_str());
338  return(retCode);
339 }
340 
341 
343 {
344  char szDummy[1024] = {0};
345  std::string s;
346  char szLidarFamily[255] = { 0 };
347  // useNegSign = True ---> NAV3xx
348  // useNegSign = False --> NAV2xx
349 
350  if (useNegSign == true)
351  {
352  strcpy(szLidarFamily, "NAV3xx");
353  }
354  else
355  {
356  strcpy(szLidarFamily, "NAV210/NAV245");
357 
358  }
359  sprintf(szDummy,"Formula allowed for: %-20s Angle[comp.] = Angle[Raw] %c %8.6lf * sin(Angle[Raw] %c %8.6lf [deg]) %c %8.6lf",
360 
361  szLidarFamily, useNegSign ? '+' : '-', amplCorr, useNegSign ? '-' : '+', phaseCorrInDeg, useNegSign ? '+' : '-', offsetCorrInDeg);
362 
363  s = szDummy;
364  return(s);
365 
366 }
371 {
372  std::vector<unsigned char> testVec;
373 
374  std::string s = string("sRA MCAngleCompSin ");
375 
376  for (int iLoop = 0; iLoop < 2; iLoop++)
377  {
378 
379 
380  bool bFlag = (iLoop == 0) ? false : true; // starte mit NAV2xx (iLoop = 0), //
381  AngleCompensator ac(bFlag);
382  testVec.clear(); // start with empty test vector
383  switch(iLoop)
384  {
385  case 0: // test for NAV2XX
386  {
387  unsigned char preFix[8] = {0x02,0x02,0x02,0x02,0x00,0x00,0x00,27+4};
388  for (int i = 0; i < 8; i++)
389  {
390  testVec.push_back(preFix[i]);
391  }
392  // TODO CHECK IT!! 0xFF appended
393  unsigned char dataArr[] = {0x00,0x00,0x07,0x65,0xff,0xfc,0xc9,0xb9,0xff,0xff,0xff,0x0b,0xFF};
394  for (int i = 0; i < s.length(); i++)
395  {
396  testVec.push_back(s[i]);
397  }
398  for (int i = 0; i < sizeof(dataArr)/sizeof(dataArr[0]); i++)
399  {
400  testVec.push_back(dataArr[i]);
401  }
402  break;
403  }
404  case 1: // test for NAV3XX
405  {
406  unsigned char preFix[8] = {0x02,0x02,0x02,0x02,0x00,0x00,0x00,27};
407  for (int i = 0; i < 8; i++)
408  {
409  testVec.push_back(preFix[i]);
410  }
411  for (int i = 0; i < s.length(); i++)
412  {
413  testVec.push_back(s[i]);
414  }
415  unsigned char dataArr[] = {0x03, 0x37, 0x00, 0x1d, 0x8e, 0x8d, 0x00, 0xe7, 0x87};
416  for (int i = 0; i < sizeof(dataArr)/sizeof(dataArr[0]); i++)
417  {
418  testVec.push_back(dataArr[i]);
419  }
420 
421  break;
422  }
423 
424  }
425  ac.parseReply(true,testVec);
426  printf("Formula used: %s\n", ac.getHumanReadableFormula().c_str());
427 
428  }
429 
430 
431 
432 
433 
434  AngleCompensator ac(true);
435 
436  testVec.clear();
437  s = "sRA MCAngleCompSin 765 FFFCC9B9 FFFFFF0B";
438  for (int i = 0; i < s.length(); i++)
439  {
440  testVec.push_back(s[i]);
441  }
442  ac.parseReply(false,testVec);
443  ac.parseAsciiReply("sRA MCAngleCompSin 765 FFFCC9B9 FFFFFF0B");
444  ac.parseAsciiReply("sRA MCAngleCompSin +1893 -210503 -245");
445  FILE *fout = fopen("angle_compensation_debug.csv","w");
446  fprintf(fout,"Formula used: %s\n", ac.getHumanReadableFormula().c_str());
447  fprintf(fout,"Input ;Output ;Correction\n");
448  for (int i =0; i <= 359; i++)
449  {
450  double rawAngle = (double)i;
451  double compAngle = ac.compensateAngleInDeg(rawAngle);
452  double compRadAngle = ac.compensateAngleInRad(rawAngle/180.0*M_PI);
453  double checkAngle = compRadAngle/M_PI*180.0;
454  fprintf(fout,"%10.6lf;%10.6lf;%10.6lf\n", rawAngle, compAngle, compAngle - rawAngle);
455  }
456  fclose(fout);
457 }
std::string getHumanReadableFormula(void)
double compensateAngleInRadFromRos(double angleInRadFromRos)
Compensate raw angle given in [RAD] in the ROS axis orientation system.
double compensateAngleInRad(double angleInRad)
Compensate raw angle given in [RAD].
double compensateAngleInDeg(double angleInDeg)
Compensate raw angle given in [DEG].
void testbed()
Testbed for angle compensation.
int parseAsciiReply(const char *asciiReply)
Parse ASCII reply.
static sick_scan::SickScanCommonTcp * s
int parseReply(bool isBinary, std::vector< unsigned char > &replyVec)
Parse reply of angle compensation values given the command MCAngleCompSin (see testbed) ...


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed Sep 7 2022 02:25:06