sensor_epsonUart.c
Go to the documentation of this file.
1 //==============================================================================
2 //
3 // sensor_epsonUart.c - Epson IMU sensor protocol UART specific code
4 //
5 //
6 // THE SOFTWARE IS RELEASED INTO THE PUBLIC DOMAIN.
7 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
8 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, NONINFRINGEMENT,
9 // SECURITY, SATISFACTORY QUALITY, AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT
10 // SHALL EPSON BE LIABLE FOR ANY LOSS, DAMAGE OR CLAIM, ARISING FROM OR IN CONNECTION
11 // WITH THE SOFTWARE OR THE USE OF THE SOFTWARE.
12 //
13 //==============================================================================
14 #include "sensor_epsonCommon.h"
15 #include "hcl.h"
16 #include "hcl_gpio.h"
17 #include "hcl_uart.h"
18 
19 // These are declared by the main() application for UART IF
20 extern const char *IMUSERIAL; // COM port device name
21 extern int comPort; // COM port handle
22 unsigned char rxBuffer[256]; // COM port receive buffer
23 
24 
25 // UART Interface Timing
26 // TWRITERATE/TREADRATE = 200us min @ 460800 BAUD, 1 command = 3 bytes = 3 * 22us = 66us
27 // TSTALL = 200us - 66us = 134us
28 #define EPSON_STALL 134 // Microseconds
29 
30 // UART commands are always terminated with 0x0D
31 const unsigned char UART_DELIMITER = 0x0D; // Placed at the end of all UART cycles
32 
33 /*****************************************************************************
34 ** Function name: sensorDataByteLength
35 ** Description: Configures the IMU based on the parameters in the Epson Options struct.
36 ** Parameters: options - The struct describing how the IMU should be configured.
37 ** Return value: 1=success, 0=fail
38 *****************************************************************************/
39 unsigned int sensorDataByteLength(struct EpsonOptions options){
40  unsigned int length = 0;
41  if(options.flag_out)
42  length += 2;
43  if(options.temp_out){
44  if(options.temp_bit)
45  length += 4;
46  else
47  length += 2;
48  }
49  if(options.gyro_out){
50  if(options.gyro_bit)
51  length += 12;
52  else
53  length += 6;
54  }
55  if(options.accel_out){
56  if(options.accel_bit)
57  length += 12;
58  else
59  length += 6;
60  }
61  if(options.gyro_delta_out){
62  if(options.gyro_delta_bit)
63  length += 12;
64  else
65  length += 6;
66  }
67  if(options.accel_delta_out){
68  if(options.accel_delta_bit)
69  length += 12;
70  else
71  length += 6;
72  }
73  if(options.gpio_out)
74  length += 2;
75  if(options.count_out)
76  length += 2;
77  if(options.checksum_out)
78  length += 2;
79 
80  length += 2; // for start and end byte
81 
82  return length;
83 }
84 
85 /*****************************************************************************
86 ** Function name: sensorDataReady
87 ** Description: For UART interface check if comport recv buffer
88 ** contains a burst of data
89 ** Parameters: None
90 ** Return value: 1=success, 0=fail
91 *****************************************************************************/
92 int sensorDataReady(void)
93 {
94  // Sensor data is ready when UART recv buffer contains atleast 1 sensor sample
95  seDelayMS(1);
96  int count;
97  count = numBytesReadComPort(comPort);
98 
99  // G350/V340 One burst of sensor data: NDFLAG(2), Temp(2), Gx(2), Gy(2), Gz(2), Ax(2), Ay(2), Az(2), GPIO(2), Count(2), Header/Delimiter (2)
100  // G352/G262/G354/G364/G320 One burst of sensor data: Gx(4), Gy(4), Gz(4), Ax(4), Ay(4), Az(4), Count(2), CHKSM16(2), Header/Delimiter(2)
101  // BURST_LEN is defined in the specific IMU Model Header sensor_epsonXXXX.h
102  if (count >= BURSTLEN)
103  return TRUE;
104  else
105  return FALSE;
106 }
107 
108 int sensorDataReadyOptions(struct EpsonOptions options){
109  seDelayMS(1);
110  int count = numBytesReadComPort(comPort);
111 
112  if(count >= sensorDataByteLength(options))
113  return TRUE;
114  return FALSE;
115 }
116 
117 /*****************************************************************************
118 ** Function name: registerWriteByte
119 ** Description: Write Byte to Register = Set WIN_ID, Write Data
120 ** to Register
121 ** NOTE: G350/V340 does not have WINDOW_ID function
122 ** winNumber input parameter is ignored
123 ** Parameters: Window Number, Register Address, Register Write Byte,
124 ** Verbose Flag
125 ** Return value: None
126 *****************************************************************************/
127 void registerWriteByte(unsigned char winNumber, unsigned char regAddr, unsigned char regByte, unsigned int verbose)
128 {
129  unsigned char txData[3];
130 
131 #if !(defined G350 || defined V340)
132  txData[0] = ADDR_WIN_CTRL|0x80; //msb is 1b for register writes
133  txData[1] = winNumber;
134  txData[2] = UART_DELIMITER;
135  writeComPort(comPort, txData, 3);
136  EpsonStall();
137 #endif
138 
139  txData[0] = regAddr | 0x80; // msb is 1b for register writes
140  txData[1] = regByte;
141  txData[2] = UART_DELIMITER;
142  writeComPort(comPort, txData, 3);
143  EpsonStall();
144 
145  if (verbose)
146  {
147  printf("\r\nREG[0x%02X(W%01X)] < 0x%02X\t", regAddr, winNumber, regByte);
148  }
149 }
150 
151 /*****************************************************************************
152 ** Function name: registerRead16
153 ** Description: Read 16-bit from Register
154 ** NOTE: G350/V340 does not have WINDOW_ID function
155 ** winNumber input parameter is ignored
156 ** Parameters: Window Number, Register Address, Verbose Flag
157 ** Return value: Register Read Value 16-bit
158 *****************************************************************************/
159 unsigned short registerRead16(unsigned char winNumber, unsigned char regAddr, unsigned int verbose)
160 {
161  unsigned char response[4] = {0};
162  int size;
163  unsigned char txData[3];
164 
165 #if !(defined G350 || defined V340)
166  txData[0] = ADDR_WIN_CTRL|0x80;
167  txData[1] = winNumber;
168  txData[2] = UART_DELIMITER;
169  writeComPort(comPort, txData, 3);
170  EpsonStall();
171 #endif
172 
173  txData[0] = regAddr & 0x7E; // msb is 0b for register reads & address must be even
174  txData[1] = 0x00;
175  txData[2] = UART_DELIMITER;
176  writeComPort(comPort, txData, 3);
177  //purgeComPort(comPort);//flush port - Commented out because seemed to have no effect
178  EpsonStall();
179 
180  // Attempt to read 4 bytes from serial port
181  // Validation check: Should be atleast 4 bytes, First byte should be Register Address,
182  // Last byte should be delimiter
183  size = readComPort(comPort, &response[0], 4);
184  if ((size<4) || (response[0] != txData[0]) || (response[3] != UART_DELIMITER))
185  printf("Returned less data or unexpected data from previous command.\n");
186  EpsonStall();
187 
188  if (verbose)
189  {
190  printf("REG[0x%02X(W%01X)] > 0x%02X%02X\t", regAddr, winNumber, response[1], response[2]);
191  }
192  return (unsigned short)response[1]<<8|(unsigned short)response[2];
193 }
194 
195 
196 static unsigned char data[256];
197 #define START 0
198 #define DATA 1
199 #define END 2
200 static int state = START;
201 static int data_count = 0;
202 static int scale_factors_initialized = 0;
203 static double dv_scale_factors[16];
204 static double da_scale_factors[16];
205 
206 /*****************************************************************************
207 ** Function name: populateEpsonData
208 ** Description: Retrieves data from the IMU based on how it is configured.
209 ** Parameters: options - The struct describing how the IMU is configured.
210 ** epson_data - The struct that is filled with data from the IMU.
211 ** Return value: none
212 ** Notes:
213 ******************************************************************************/
214 void populateEpsonData(struct EpsonOptions options, struct EpsonData* epson_data){
215  if(!scale_factors_initialized){
216  da_scale_factors[0] = EPSON_DA_SF0;
217  da_scale_factors[1] = EPSON_DA_SF1;
218  da_scale_factors[2] = EPSON_DA_SF2;
219  da_scale_factors[3] = EPSON_DA_SF3;
220  da_scale_factors[4] = EPSON_DA_SF4;
221  da_scale_factors[5] = EPSON_DA_SF5;
222  da_scale_factors[6] = EPSON_DA_SF6;
223  da_scale_factors[7] = EPSON_DA_SF7;
224  da_scale_factors[8] = EPSON_DA_SF8;
225  da_scale_factors[9] = EPSON_DA_SF9;
226  da_scale_factors[10] = EPSON_DA_SF10;
227  da_scale_factors[11] = EPSON_DA_SF11;
228  da_scale_factors[12] = EPSON_DA_SF12;
229  da_scale_factors[13] = EPSON_DA_SF13;
230  da_scale_factors[14] = EPSON_DA_SF14;
231  da_scale_factors[15] = EPSON_DA_SF15;
232 
233  dv_scale_factors[0] = EPSON_DV_SF0;
234  dv_scale_factors[1] = EPSON_DV_SF1;
235  dv_scale_factors[2] = EPSON_DV_SF2;
236  dv_scale_factors[3] = EPSON_DV_SF3;
237  dv_scale_factors[4] = EPSON_DV_SF4;
238  dv_scale_factors[5] = EPSON_DV_SF5;
239  dv_scale_factors[6] = EPSON_DV_SF6;
240  dv_scale_factors[7] = EPSON_DV_SF7;
241  dv_scale_factors[8] = EPSON_DV_SF8;
242  dv_scale_factors[9] = EPSON_DV_SF9;
243  dv_scale_factors[10] = EPSON_DV_SF10;
244  dv_scale_factors[11] = EPSON_DV_SF11;
245  dv_scale_factors[12] = EPSON_DV_SF12;
246  dv_scale_factors[13] = EPSON_DV_SF13;
247  dv_scale_factors[14] = EPSON_DV_SF14;
248  dv_scale_factors[15] = EPSON_DV_SF15;
249  }
250 
251  int f1, f2;
252  int t1, t2, t3, t4;
253  int gx1, gx2, gx3, gx4;
254  int gy1, gy2, gy3, gy4;
255  int gz1, gz2, gz3, gz4;
256  int ax1, ax2, ax3, ax4;
257  int ay1, ay2, ay3, ay4;
258  int az1, az2, az3, az4;
259  int gdx1, gdx2, gdx3, gdx4;
260  int gdy1, gdy2, gdy3, gdy4;
261  int gdz1, gdz2, gdz3, gdz4;
262  int adx1, adx2, adx3, adx4;
263  int ady1, ady2, ady3, ady4;
264  int adz1, adz2, adz3, adz4;
265  int gp1, gp2;
266  int c1, c2;
267 
268  if(options.flag_out)
269  f1 = 0, f2 = 1;
270  else
271  f1 = 0, f2 = -1;
272 
273  if(options.temp_out)
274  t1 = f2 + 1, t2 = t1 + 1, t3 = t2 + 1, t4 = t3 + 1;
275  else
276  t1 = t2 = t3 = t4 = f2;
277  if(!options.temp_bit)
278  t3 = t4 = t2;
279 
280  if(options.gyro_out)
281  gx1 = t4 + 1, gx2 = gx1 + 1, gx3 = gx2 + 1, gx4 = gx3 + 1;
282  else
283  gx1 = gx2 = gx3 = gx4 = t4;
284  if(!options.gyro_bit)
285  gx3 = gx4 = gx2;
286 
287  if(options.gyro_out)
288  gy1 = gx4 + 1, gy2 = gy1 + 1, gy3 = gy2 + 1, gy4 = gy3 + 1;
289  else
290  gy1 = gy2 = gy3 = gy4 = t4;
291  if(!options.gyro_bit)
292  gy3 = gy4 = gy2;
293 
294  if(options.gyro_out)
295  gz1 = gy4 + 1, gz2 = gz1 + 1, gz3 = gz2 + 1, gz4 = gz3 + 1;
296  else
297  gz1 = gz2 = gz3 = gz4 = t4;
298  if(!options.gyro_bit)
299  gz3 = gz4 = gz2;
300 
301  if(options.accel_out)
302  ax1 = gz4 + 1, ax2 = ax1 + 1, ax3 = ax2 + 1, ax4 = ax3 + 1;
303  else
304  ax1 = ax2 = ax3 = ax4 = gz4;
305  if(!options.accel_bit)
306  ax3 = ax4 = ax2;
307 
308  if(options.accel_out)
309  ay1 = ax4 + 1, ay2 = ay1 + 1, ay3 = ay2 + 1, ay4 = ay3 + 1;
310  else
311  ay1 = ay2 = ay3 = ay4 = gz4;
312  if(!options.accel_bit)
313  ay3 = ay4 = ay2;
314 
315  if(options.accel_out)
316  az1 = ay4 + 1, az2 = az1 + 1, az3 = az2 + 1, az4 = az3 + 1;
317  else
318  az1 = az2 = az3 = az4 = gz4;
319  if(!options.accel_bit)
320  az3 = az4 = az2;
321 
322  if(options.accel_delta_out)
323  adx1 = az4 + 1, adx2 = adx1 + 1, adx3 = adx2 + 1, adx4 = adx3 + 1;
324  else
325  adx1 = adx2 = adx3 = adx4 = az4;
326  if(!options.accel_delta_bit)
327  adx3 = adx4 = adx2;
328 
329  if(options.accel_delta_out)
330  ady1 = adx4 + 1, ady2 = ady1 + 1, ady3 = ady2 + 1, ady4 = ady3 + 1;
331  else
332  ady1 = ady2 = ady3 = ady4 = az4;
333  if(!options.accel_delta_bit)
334  ady3 = ady4 = ady2;
335 
336  if(options.accel_delta_out)
337  adz1 = ady4 + 1, adz2 = adz1 + 1, adz3 = adz2 + 1, adz4 = adz3 + 1;
338  else
339  adz1 = adz2 = adz3 = adz4 = az4;
340  if(!options.accel_delta_bit)
341  adz3 = adz4 = adz2;
342 
343  if(options.gyro_delta_out)
344  gdx1 = adz4 + 1, gdx2 = gdx1 + 1, gdx3 = gdx2 + 1, gdx4 = gdx3 + 1;
345  else
346  gdx1 = gdx2 = gdx3 = gdx4 = adz4;
347  if(!options.gyro_delta_bit)
348  gdx3 = gdx4 = gdx2;
349 
350  if(options.gyro_delta_out)
351  gdy1 = adx4 + 1, gdy2 = gdy1 + 1, gdy3 = gdy2 + 1, gdy4 = gdy3 + 1;
352  else
353  gdy1 = gdy2 = gdy3 = gdy4 = adz4;
354  if(!options.gyro_delta_bit)
355  gdy3 = gdy4 = gdy2;
356 
357  if(options.gyro_delta_out)
358  gdz1 = ady4 + 1, gdz2 = gdz1 + 1, gdz3 = gdz2 + 1, gdz4 = gdz3 + 1;
359  else
360  gdz1 = gdz2 = gdz3 = gdz4 = adz4;
361  if(!options.gyro_delta_bit)
362  gdz3 = gdz4 = gdz2;
363 
364  if(options.gpio_out)
365  gp1 = gdz4 + 1, gp2 = gp1 + 1;
366  else
367  gp1 = gp2 = gdz4;
368 
369  if(options.count_out)
370  c1 = gp2 + 1, c2 = c1 + 1;
371  else
372  c1 = c2 = gp2;
373 
374  if(options.temp_bit){
375  int temp = (data[t1] << 8*3) + (data[t2] << 8*2) + (data[t3] << 8) + data[t4];
376  epson_data->temperature = (temp - 172621824)*EPSON_TEMP_SF/65536 + 25;
377  }
378  else{
379  short temp = (data[t1] << 8) + data[t2];
380  epson_data->temperature = (temp - 2634)*EPSON_TEMP_SF + 25;
381  }
382 
383  if(options.gyro_bit){
384  int gyro_x = (data[gx1] << 8*3) + (data[gx2] << 8*2) + (data[gx3] << 8)+data[gx4];
385  int gyro_y = (data[gy1] << 8*3) + (data[gy2] << 8*2) + (data[gy3] << 8)+data[gy4];
386  int gyro_z = (data[gz1] << 8*3) + (data[gz2] << 8*2) + (data[gz3] << 8)+data[gz4];
387  epson_data->gyro_x = (EPSON_GYRO_SF/65536)*3.14159/180.0 * gyro_x;
388  epson_data->gyro_y = (EPSON_GYRO_SF/65536)*3.14159/180.0 * gyro_y;
389  epson_data->gyro_z = (EPSON_GYRO_SF/65536)*3.14159/180.0 * gyro_z;
390  }
391  else{
392  short gyro_x = (data[gx1] << 8) + data[gx2];
393  short gyro_y = (data[gy1] << 8) + data[gy2];
394  short gyro_z = (data[gz1] << 8) + data[gz2];
395  epson_data->gyro_x = EPSON_GYRO_SF*3.14159/180.0 * gyro_x;
396  epson_data->gyro_y = EPSON_GYRO_SF*3.14159/180.0 * gyro_y;
397  epson_data->gyro_z = EPSON_GYRO_SF*3.14159/180.0 * gyro_z;
398  }
399 
400  if(options.accel_bit){
401  int accel_x = (data[ax1] << 8*3) + (data[ax2] << 8*2) +
402  (data[ax3] << 8) + data[ax4];
403  int accel_y = (data[ay1] << 8*3) + (data[ay2] << 8*2) +
404  (data[ay3] << 8) + data[ay4];
405  int accel_z = (data[az1] << 8*3) + (data[az2] << 8*2) +
406  (data[az3] << 8) + data[az4];
407  epson_data->accel_x = (EPSON_ACCL_SF/65536)*9.80665/1000.0 * accel_x;
408  epson_data->accel_y = (EPSON_ACCL_SF/65536)*9.80665/1000.0 * accel_y;
409  epson_data->accel_z = (EPSON_ACCL_SF/65536)*9.80665/1000.0 * accel_z;
410  }
411  else{
412  short accel_x = (data[ax1] << 8) + (data[ax2]);
413  short accel_y = (data[ay1] << 8) + (data[ay2]);
414  short accel_z = (data[az1] << 8) + (data[az2]);
415  epson_data->accel_x = (EPSON_ACCL_SF)*9.80665/1000.0 * accel_x;
416  epson_data->accel_y = (EPSON_ACCL_SF)*9.80665/1000.0 * accel_y;
417  epson_data->accel_z = (EPSON_ACCL_SF)*9.80665/1000.0 * accel_z;
418  }
419 
420  if(options.gyro_delta_bit){
421  int gyro_delta_x = (data[gdx1] << 8*3) + (data[gdx2] << 8*2) +
422  (data[gdx3] << 8)+data[gdx4];
423  int gyro_delta_y = (data[gdy1] << 8*3) + (data[gdy2] << 8*2) +
424  (data[gdy3] << 8)+data[gdy4];
425  int gyro_delta_z = (data[gdz1] << 8*3) + (data[gdz2] << 8*2) +
426  (data[gdz3] << 8)+data[gdz4];
427  double dv_sf = dv_scale_factors[options.dlt_range_ctrl];
428  epson_data->gyro_delta_x = dv_sf/65536 * gyro_delta_x;
429  epson_data->gyro_delta_y = dv_sf/65536 * gyro_delta_y;
430  epson_data->gyro_delta_z = dv_sf/65536 * gyro_delta_z;
431  }
432  else{
433  short gyro_delta_x = (data[gdx1] << 8) + data[gdx2];
434  short gyro_delta_y = (data[gdy1] << 8) + data[gdy2];
435  short gyro_delta_z = (data[gdz1] << 8) + data[gdz2];
436  double dv_sf = dv_scale_factors[options.dlt_range_ctrl];
437  epson_data->gyro_delta_x = dv_sf * gyro_delta_x;
438  epson_data->gyro_delta_y = dv_sf * gyro_delta_y;
439  epson_data->gyro_delta_z = dv_sf * gyro_delta_z;
440  }
441 
442  if(options.accel_delta_bit){
443  int accel_delta_x = (data[adx1] << 8*3) + (data[adx2] << 8*2) +
444  (data[adx3] << 8)+data[adx4];
445  int accel_delta_y = (data[ady1] << 8*3) + (data[ady2] << 8*2) +
446  (data[ady3] << 8)+data[ady4];
447  int accel_delta_z = (data[adz1] << 8*3) + (data[adz2] << 8*2) +
448  (data[adz3] << 8)+data[adz4];
449  double da_sf = da_scale_factors[options.dlt_range_ctrl];
450  epson_data->accel_delta_x = da_sf/65536 * accel_delta_x;
451  epson_data->accel_delta_y = da_sf/65536 * accel_delta_y;
452  epson_data->accel_delta_z = da_sf/65536 * accel_delta_z;
453  }
454  else{
455  short accel_delta_x = (data[adx1] << 8) + data[adx2];
456  short accel_delta_y = (data[ady1] << 8) + data[ady2];
457  short accel_delta_z = (data[adz1] << 8) + data[adz2];
458  double da_sf = da_scale_factors[options.dlt_range_ctrl];
459  epson_data->accel_delta_x = da_sf * accel_delta_x;
460  epson_data->accel_delta_y = da_sf * accel_delta_y;
461  epson_data->accel_delta_z = da_sf * accel_delta_z;
462  }
463 
464  int count = (data[c1] << 8) + data[c2];
465  epson_data->count = count*EPSON_COUNT_SF;
466 }
467 
468 int sensorDataReadBurstNOptions(struct EpsonOptions options, struct EpsonData* epson_data){
469  int byte_length = sensorDataByteLength(options);
470  int data_length = byte_length - 2;
471  unsigned char byte;
472  int i;
473  //printf("length: %d\n", byte_length);
474  while(readComPort(comPort, &byte, 1) > 0){
475  //printf("state: %d, byte: 0x%02X\n", state, byte);
476  switch(state){
477  case START:
478  if(byte == 0x80)
479  state = DATA;
480  break;
481  case DATA:
482  data[data_count] = byte;
483  data_count++;
484  if(data_count == byte_length - 2)
485  state = END;
486  break;
487  case END:
488  data_count = 0;
489  state = START;
490  if(byte == 0x0D){
491  unsigned short checksum = 0;
492  //for(i = 0; i < data_length; i++)
493  // printf("0x%02X ", data[i]);
494  //printf("\n");
495  for(i = 0; i < data_length-2; i += 2)
496  checksum += (data[i] << 8) + data[i+1];
497  unsigned short epson_checksum = (data[data_length - 2] << 8) +
498  data[data_length - 1];
499 
500  //printf("actual: 0x%04X, expected: 0x%04X\n", checksum, epson_checksum);
501 
502  if(checksum == epson_checksum){
503  populateEpsonData(options, epson_data);
504  return TRUE;
505  }
506  else
507  printf("checksum failed\n");
508  }
509  return FALSE;
510  break;
511  }
512  }
513  return FALSE;
514 }
515 
516 /*****************************************************************************
517 ** Function name: sensorDataReadBurstN
518 ** Description: Perform burst read to acquire sensor data
519 ** NOTE: Not supported for SPI IF G350/V340
520 ** Parameters: pointer to signed short array, size of array
521 ** Return value: none
522 ** Notes:
523 ** 1. The burst packet consists of 16-bit data units.
524 ** For 32-bit sensor output, Total = GyroXYZ(4*3) + AccelXYZ(4*3) + Count(2) + Chksum(2) = 28 bytes = 14 words
525 ** For 16-bit sensor output, Total = ND_FLAGS(2) + Temp(2) + GyroXYZ(2*3) + AccelXYZ(2*3) + GPIO(2) + Count(2) = 20 bytes = 10 words
526 ** 2. For SPI interface, call this function only after detecting DRDY is asserted.
527 ** For SPI interface, this function will send N+1 SPI commands to read N registers.
528 ** Maximum SPI clock is 1MHz for burst SPI reads.
529 ** 3. For UART interface, call this function only after detecting UART recv buffer contains atleast 1 burst of data
530 ** 4. For checksum16 validation, simply outputs error message if checksum mismatch detected (no other action performed)
531 **
532 ** To achieve high performance, we use this function to retrieve burst sensor data instead of calling registerRead16().
533 **
534 *****************************************************************************/
535 void sensorDataReadBurstN(signed short sensorReadData[], unsigned int readLen)
536 {
537  // rxBuffer (in bytes) is 2 * SENSOR_READ_LEN(in 16-bits) + Burst Byte + Delimiter Byte
538  int byteLength = (readLen*2) + 2;
539  int i, j;
540  int size;
541 
542  size = readComPort(comPort, &rxBuffer[0], byteLength);
543  if(size<=0) printf("!");
544  //for (i = 0; i < size; i++) printf("0x%02x,", rxBuffer[i]);
545  //printf("\t%2i",size);
546 
547  // Convert bytes in rxBuffer to 16-bit words stored in sensorReadData array
548  j = 0;
549  for (i = 1; i < (byteLength-1); i=i+2)
550  {
551  //printf("%u %u ", i, j);
552  unsigned short tmp = rxBuffer[i];
553  sensorReadData[j] = (tmp << 8) + rxBuffer[i+1];
554  j++;
555  }
556 
557  // debug
558  int k;
559  for(k = 0; k < byteLength-1; k++)
560  printf("%d: 0x%02x, ", k, rxBuffer[k]);
561  printf("\n");
562 
563  // Bypass for V340 or G350 since it does not support checksum16 function
564 #if !(defined G350 || defined V340)
565  unsigned short chksum16_calc = 0;
566  for (i = 0; i < readLen-1; i++)
567  chksum16_calc += (unsigned short)sensorReadData[i];
568 
569  /*printf("\r\n");
570  for (i = 0; i < readLen; i++) printf(",\t0x%04hX", sensorReadData[i]);
571  printf(",\t0x%04hX", chksum16_calc);
572  printf(" %i %i", chksum16_calc, sensorReadData[readLen-1]);*/
573 
574  if (chksum16_calc != (unsigned short)sensorReadData[readLen-1])
575  printf(" Checksum error detected!");
576 #endif
577 }
578 
EPSON_DA_SF15
#define EPSON_DA_SF15
Definition: sensor_epsonG364DC0.h:42
IMUSERIAL
const char * IMUSERIAL
Definition: main_csvlogger.c:28
EPSON_DA_SF11
#define EPSON_DA_SF11
Definition: sensor_epsonG364DC0.h:38
EPSON_DV_SF14
#define EPSON_DV_SF14
Definition: sensor_epsonG364DC0.h:58
EPSON_DA_SF13
#define EPSON_DA_SF13
Definition: sensor_epsonG364DC0.h:40
EPSON_DA_SF5
#define EPSON_DA_SF5
Definition: sensor_epsonG364DC0.h:32
EPSON_DA_SF12
#define EPSON_DA_SF12
Definition: sensor_epsonG364DC0.h:39
EPSON_DV_SF15
#define EPSON_DV_SF15
Definition: sensor_epsonG364DC0.h:59
sensorDataByteLength
unsigned int sensorDataByteLength(struct EpsonOptions options)
Definition: sensor_epsonUart.c:39
EPSON_DA_SF6
#define EPSON_DA_SF6
Definition: sensor_epsonG364DC0.h:33
rxBuffer
unsigned char rxBuffer[256]
Definition: sensor_epsonUart.c:22
da_scale_factors
static double da_scale_factors[16]
Definition: sensor_epsonUart.c:204
EPSON_DV_SF2
#define EPSON_DV_SF2
Definition: sensor_epsonG364DC0.h:46
BURSTLEN
#define BURSTLEN
Definition: sensor_epsonG320.h:31
FALSE
#define FALSE
Definition: hcl_uart.c:32
dv_scale_factors
static double dv_scale_factors[16]
Definition: sensor_epsonUart.c:203
START
#define START
Definition: sensor_epsonUart.c:197
scale_factors_initialized
static int scale_factors_initialized
Definition: sensor_epsonUart.c:202
TRUE
#define TRUE
Definition: hcl_uart.c:31
EPSON_DA_SF2
#define EPSON_DA_SF2
Definition: sensor_epsonG364DC0.h:29
sensorDataReadBurstN
void sensorDataReadBurstN(signed short sensorReadData[], unsigned int readLen)
Definition: sensor_epsonUart.c:535
sensorDataReady
int sensorDataReady(void)
Definition: sensor_epsonUart.c:92
ADDR_WIN_CTRL
#define ADDR_WIN_CTRL
Definition: sensor_epsonG320.h:120
EPSON_GYRO_SF
#define EPSON_GYRO_SF
Definition: sensor_epsonG320.h:23
numBytesReadComPort
int numBytesReadComPort(ComPortHandle comPort)
Definition: hcl_uart.c:207
EPSON_DV_SF10
#define EPSON_DV_SF10
Definition: sensor_epsonG364DC0.h:54
EPSON_DV_SF12
#define EPSON_DV_SF12
Definition: sensor_epsonG364DC0.h:56
END
#define END
Definition: sensor_epsonUart.c:199
EPSON_DA_SF14
#define EPSON_DA_SF14
Definition: sensor_epsonG364DC0.h:41
EPSON_DV_SF9
#define EPSON_DV_SF9
Definition: sensor_epsonG364DC0.h:53
EPSON_DV_SF8
#define EPSON_DV_SF8
Definition: sensor_epsonG364DC0.h:52
registerWriteByte
void registerWriteByte(unsigned char winNumber, unsigned char regAddr, unsigned char regByte, unsigned int verbose)
Definition: sensor_epsonUart.c:127
registerRead16
unsigned short registerRead16(unsigned char winNumber, unsigned char regAddr, unsigned int verbose)
Definition: sensor_epsonUart.c:159
EPSON_DA_SF10
#define EPSON_DA_SF10
Definition: sensor_epsonG364DC0.h:37
writeComPort
int writeComPort(ComPortHandle comPort, unsigned char *bytesToWrite, int size)
Definition: hcl_uart.c:194
data
static unsigned char data[256]
Definition: sensor_epsonUart.c:196
populateEpsonData
void populateEpsonData(struct EpsonOptions options, struct EpsonData *epson_data)
Definition: sensor_epsonUart.c:214
comPort
int comPort
Definition: epson_imu_driver_node.cpp:15
EPSON_DV_SF4
#define EPSON_DV_SF4
Definition: sensor_epsonG364DC0.h:48
EPSON_DA_SF8
#define EPSON_DA_SF8
Definition: sensor_epsonG364DC0.h:35
sensorDataReadBurstNOptions
int sensorDataReadBurstNOptions(struct EpsonOptions options, struct EpsonData *epson_data)
Definition: sensor_epsonUart.c:468
DATA
#define DATA
Definition: sensor_epsonUart.c:198
readComPort
int readComPort(ComPortHandle comPort, unsigned char *bytes, int bytesToRead)
Definition: hcl_uart.c:181
EPSON_DV_SF6
#define EPSON_DV_SF6
Definition: sensor_epsonG364DC0.h:50
EPSON_DA_SF4
#define EPSON_DA_SF4
Definition: sensor_epsonG364DC0.h:31
EPSON_DV_SF1
#define EPSON_DV_SF1
Definition: sensor_epsonG364DC0.h:45
EPSON_DV_SF5
#define EPSON_DV_SF5
Definition: sensor_epsonG364DC0.h:49
EPSON_DA_SF7
#define EPSON_DA_SF7
Definition: sensor_epsonG364DC0.h:34
EPSON_ACCL_SF
#define EPSON_ACCL_SF
Definition: sensor_epsonG320.h:22
EPSON_DA_SF3
#define EPSON_DA_SF3
Definition: sensor_epsonG364DC0.h:30
EPSON_COUNT_SF
#define EPSON_COUNT_SF
Definition: sensor_epsonG364DC0.h:25
sensorDataReadyOptions
int sensorDataReadyOptions(struct EpsonOptions options)
Definition: sensor_epsonUart.c:108
state
static int state
Definition: sensor_epsonUart.c:200
seDelayMS
void seDelayMS(uint32_t millis)
Definition: hcl_linux.c:54
EPSON_DV_SF0
#define EPSON_DV_SF0
Definition: sensor_epsonG364DC0.h:44
EPSON_TEMP_SF
#define EPSON_TEMP_SF
Definition: sensor_epsonG364DC0.h:24
EpsonStall
#define EpsonStall()
Definition: sensor_epsonCommon.h:48
data_count
static int data_count
Definition: sensor_epsonUart.c:201
EPSON_DV_SF13
#define EPSON_DV_SF13
Definition: sensor_epsonG364DC0.h:57
hcl_uart.h
EPSON_DA_SF1
#define EPSON_DA_SF1
Definition: sensor_epsonG364DC0.h:28
hcl_gpio.h
sensor_epsonCommon.h
EPSON_DV_SF11
#define EPSON_DV_SF11
Definition: sensor_epsonG364DC0.h:55
EPSON_DV_SF3
#define EPSON_DV_SF3
Definition: sensor_epsonG364DC0.h:47
EPSON_DA_SF0
#define EPSON_DA_SF0
Definition: sensor_epsonG364DC0.h:27
UART_DELIMITER
const unsigned char UART_DELIMITER
Definition: sensor_epsonUart.c:31
EPSON_DV_SF7
#define EPSON_DV_SF7
Definition: sensor_epsonG364DC0.h:51
EPSON_DA_SF9
#define EPSON_DA_SF9
Definition: sensor_epsonG364DC0.h:36

epson_g364_imu_driver
Author(s):
autogenerated on Mon Jun 10 2019 13:12:32