jr3_driver.cpp
Go to the documentation of this file.
1 /*
2  * ResMgr and Message Server Process
3  *
4  * This program is for JR3/Nitta Force moment sensor.
5  * Copyright(C) by Waseda University, Nitta Coropration. 2002.
6  *
7  * Modified by Kei Okada, 2014
8  */
9 
10 #include <stdio.h>
11 #include <unistd.h>
12 #include <stdlib.h>
13 #include <errno.h>
14 #include <string.h>
15 #include <sys/time.h>
16 #include <sys/neutrino.h>
17 #include <sys/iofunc.h>
18 #include <sys/dispatch.h>
19 #include <sys/mman.h>
20 #include <hw/pci.h>
21 
22 #define min(a, b) ((a) < (b) ? (a) : (b))
23 
24 int io_read(resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb); //
25 static char *buffer = (char *) "Hello world\n"; //
26 
27 void wait_t(void); // wait for 3 sec.
28 
29 #include "QNX101/jr3pci3.idm"
30 
31 #define VENDORID 0x1762 /* Vendor ID of JR3 */
32 // #define DEVICEID 0x3114 /* for 4ch board */
33 // #define DEVICEID 0x3113 /* for 3ch board */
34 #define DEVICEID 0x3112 /* for 2ch board */
35 // #define DEVICEID 0x1111 /* for 1ch board */
36 
37 #define Jr3ResetAddr 0x18000
38 #define Jr3NoAddrMask 0x40000
39 #define Jr3DmAddrMask 0x6000
40 unsigned long MappedAddress;
41 
42 /* Convert a DSP bus address to a PCI bus address */
43 #define ToJr3PciAddrH(addr) (((int)(addr) << 2) + Jr3BaseAddressH)
44 #define ToJr3PciAddrL(addr) (((int)(addr) << 2) + Jr3BaseAddressL)
45 
46 /* Read and Write from the program memory */
47 #define ReadJr3Pm(addr) (*(uint16_t volatile *)(ToJr3PciAddrH((addr))) << 8 |\
48  *(uint8_t volatile *)(ToJr3PciAddrL((addr))))
49 
50 #define WriteJr3Pm2(addr,data,data2) (*(int volatile *)ToJr3PciAddrH((addr)) = (int)(data));(*(int volatile *)ToJr3PciAddrL((addr)) = (int)(data2))
51 
52 #define WriteJr3Pm(addr,data) WriteJr3Pm2((addr),(data) >> 8, (data))
53 
54 /* Read and Write from the data memory using bus relative addressing */
55 #define ReadJr3Dm(addr) (*(uint16_t volatile *)(ToJr3PciAddrH((addr))))
56 #define WriteJr3Dm(addr,data) *(int volatile *)(ToJr3PciAddrH((addr))) = (int)(data)
57 
58 /* Read and Write from the data memory using 0 relative addressing */
59 #define ReadJr3(addr) (ReadJr3Dm((addr) | Jr3DmAddrMask))
60 #define WriteJr3(addr,data) WriteJr3Dm((addr) | Jr3DmAddrMask,data)
61 
62 static resmgr_connect_funcs_t ConnectFuncs; //
63 static resmgr_io_funcs_t IoFuncs; //
64 static iofunc_attr_t IoFuncAttr; //
65 
66 /* Jr3 Base address */
67 volatile uint32_t Jr3BaseAddressH;
68 volatile uint32_t Jr3BaseAddressL;
69 volatile uint32_t Jr3BaseAddress0H;
70 volatile uint32_t Jr3BaseAddress0L;
71 volatile uint32_t Jr3BaseAddress1H;
72 volatile uint32_t Jr3BaseAddress1L;
73 
74 /* Jr3 memory map */
75 /*
76  For the following structure definitions :
77  int: signed 16 bit value
78  unsigned int: unsigned 16-bit value
79  bit fields are shown with the lsb first
80  */
81 
82 struct raw_channel {
83  uint16_t raw_time;
84  uint16_t dummy1;
85  int16_t raw_data;
86  int16_t dummy2;
87  int16_t reserved[2];
88  int16_t dummy3[2];
89 };
90 
91 struct force_array {
92  int16_t fx;
93  int16_t dummy1;
94  int16_t fy;
95  int16_t dummy2;
96  int16_t fz;
97  int16_t dummy3;
98  int16_t mx;
99  int16_t dummy4;
100  int16_t my;
101  int16_t dummy5;
102  int16_t mz;
103  int16_t dummy6;
104  int16_t v1;
105  int16_t dummy7;
106  int16_t v2;
107  int16_t dummy8;
108 };
109 
111  int16_t fx;
112  int16_t dummy1;
113  int16_t fy;
114  int16_t dummy2;
115  int16_t fz;
116  int16_t dummy3;
117  int16_t mx;
118  int16_t dummy4;
119  int16_t my;
120  int16_t dummy5;
121  int16_t mz;
122  int16_t dummy6;
123 };
124 
125 struct vect_bits {
126  uint8_t fx :1;
127  uint8_t fy :1;
128  uint8_t fz :1;
129  uint8_t mx :1;
130  uint8_t my :1;
131  uint8_t mz :1;
132  uint8_t changedV1 :1;
133  uint8_t changedV2 :1;
134  int16_t dummy;
135 };
136 
137 struct warning_bits {
138  uint8_t fx_near_set :1;
139  uint8_t fy_near_set :1;
140  uint8_t fz_near_set :1;
141  uint8_t mx_near_set :1;
142  uint8_t my_near_set :1;
143  uint8_t mz_near_set :1;
144  uint8_t reserved :8;
145  //uint8_t reserved : 10; // this will use 3 byte
146  int16_t dummy;
147 };
148 
149 struct error_bits {
150  uint8_t fx_sat :1;
151  uint8_t fy_sat :1;
152  uint8_t fz_sat :1;
153  uint8_t mx_sat :1;
154  uint8_t my_sat :1;
155  uint8_t mz_sat :1;
156  uint8_t researved :2;
157  //uint8_t researved : 4; // this will use 3 byte
158  uint8_t memry_error :1;
159  uint8_t sensor_charge :1;
160  uint8_t system_busy :1;
161  uint8_t cal_crc_bad :1;
162  uint8_t watch_dog2 :1;
163  uint8_t watch_dog :1;
164  int16_t dummy1;
165 };
166 
168  { (char *) "pound, inch*pound, inch*1000",
169 
170  (char *) "Newton, Newton*meter*10, mm*10",
171  (char *) "kilogram-force*10, kilogram-Force*cm, mm*10",
172  (char *) "kilopound, kiloinch*pound, inch*1000" };
173 
183 };
184 
186  int16_t data_address;
187  int16_t dummy1;
188  int16_t threshold;
189  int16_t dummy2;
190  int16_t bit_pattern;
191  int16_t dummy3;
192 };
193 
194 struct le_struct {
195  int16_t latch_bits;
196  int16_t dummy1;
198  int16_t dummy2;
200  int16_t dummy3;
201  thresh_struct thresholds[4];
202  int16_t reserved;
203  int16_t dummy4;
204 };
205 
208 };
209 
210 struct links {
212  int16_t dummy1;
213  int16_t link_amount;
214  int16_t dummy2;
215 };
216 
217 struct transform {
218  links link[8];
219 };
220 
221 
223  raw_channel raw_channels[16]; /* offset 0x0000 */
224  char copyright[0x18 * 4]; /* offset 0x0040 */
225  int16_t reserved1[0x08 * 2]; /* offset 0x0058 */
226  six_axis_array shunts; /* offset 0x0060 */
227  int16_t reserved2[2]; /* offset 0x0066 */
228  int16_t dummy1[2];
229  six_axis_array default_FS; /* offset 0x0068 */
230  int16_t reserved3; /* offset 0x006e */
231  int16_t dummy2;
232  int16_t load_envelope_num; /* offset 0x006f */
233  int16_t dummy3;
234  six_axis_array min_full_scale; /* offset 0x0070 */
235  int16_t reserved4; /* offset 0x0076 */
236  int16_t dummy4;
237  int16_t transform_num; /* offset 0x0077 */
238  int16_t dummy5;
239  six_axis_array max_full_scale; /* offset 0x0078 */
240  int16_t reserved5; /* offset 0x007e */
241  int16_t dummy6;
242  int16_t peak_address; /* offset 0x007f */
243  int16_t dummy7;
244  force_array full_scale; /* offset 0x0080 */
245  six_axis_array offsets; /* offset 0x0088 */
246  int16_t offset_num; /* offset 0x008e */
247  int16_t dummy8;
248  vect_bits vect_axes; /* offset 0x008f */
249  force_array filter0; /* offset 0x0090 */
250  force_array filter1; /* offset 0x0098 */
251  force_array filter2; /* offset 0x00a0 */
252  force_array filter3; /* offset 0x00a8 */
253  force_array filter4; /* offset 0x00b0 */
254  force_array filter5; /* offset 0x00b8 */
255  force_array filter6; /* offset 0x00c0 */
256  force_array rate_data; /* offset 0x00c8 */
257  force_array minimum_data; /* offset 0x00d0 */
258  force_array maximum_data; /* offset 0x00d8 */
259  int16_t near_sat_value; /* offset 0x00e0 */
260  int16_t dummy9;
261  int16_t sat_value; /* offset 0x00e1 */
262  int16_t dummy10;
263  int16_t rate_address; /* offset 0x00e2 */
264  int16_t dummy11;
265  uint16_t rate_divisor; /* offset 0x00e3 */
266  uint16_t dummy12;
267  uint16_t rate_count; /* offset 0x00e4 */
268  uint16_t dummy13;
269  int16_t command_word2; /* offset 0x00e5 */
270  int16_t dummy14;
271  int16_t command_word1; /* offset 0x00e6 */
272  int16_t dummy15;
273  int16_t command_word0; /* offset 0x00e7 */
274  uint16_t dummy16;
275  uint16_t count1; /* offset 0x00e8 */
276  uint16_t dummy17;
277  uint16_t count2; /* offset 0x00e9 */
278  uint16_t dummy18;
279  uint16_t count3; /* offset 0x00ea */
280  uint16_t dummy19;
281  uint16_t count4; /* offset 0x00eb */
282  uint16_t dummy20;
283  uint16_t count5; /* offset 0x00ec */
284  uint16_t dummy21;
285  uint16_t count6; /* offset 0x00ed */
286  uint16_t dummy22;
287  uint16_t error_count; /* offset 0x00ee */
288  uint16_t dummy23;
289  uint16_t count_x; /* offset 0x00ef */
290  uint16_t dummy24;
291  warning_bits warnings; /* offset 0x00f0 */
292  error_bits errors; /* offset 0x00f1 */
293  int16_t threshold_bits; /* offset 0x00f2 */
294  int16_t dummy25;
295  int16_t last_crc; /* offset 0x00f3 */
296  int16_t dummy26;
297  int16_t eeprom_ver_no; /* offset 0x00f4 */
298  int16_t dummy27;
299  int16_t software_ver_no; /* offset 0x00f5 */
300  int16_t dummy28;
301  int16_t software_day; /* offset 0x00f6 */
302  int16_t dummy29;
303  int16_t software_year; /* offset 0x00f7 */
304  int16_t dummy30;
305  uint16_t serial_no; /* offset 0x00f8 */
306  uint16_t dummy31;
307  uint16_t model_no; /* offset 0x00f9 */
308  uint16_t dummy32;
309  int16_t cal_day; /* offset 0x00fa */
310  int16_t dummy33;
311  int16_t cal_year; /* offset 0x00fb */
312  int16_t dummy34;
313  force_units units; /* offset 0x00fc */
314  int16_t bit; /* offset 0x00fd */
315  int16_t dummy35;
316  int16_t channels; /* offset 0x00fe */
317  int16_t dummy36;
318  int16_t thickness; /* offset 0x00ff */
319  int16_t dummy37;
320  le_struct load_envelopes[0x10]; /* offset 0x0100 */
321  transform transforms[0x10]; /* offset 0x0200 */
322 };
323 
324 typedef struct {
325  uint16_t msg_no;
326  char msg_data[255];
327 } server_msg_t;
328 
329 int download(unsigned int base0, unsigned int base1) {
330  printf("Download %x %x\n", base0, base1);
331 
332  int count;
333  int index = 0;
334  unsigned int Jr3BaseAddressH;
335  unsigned int Jr3BaseAddressL;
336 
337  Jr3BaseAddressH = base0;
338  Jr3BaseAddressL = base1;
339 
340  /* The first line is a line count */
341  count = dsp[index++];
342 
343  /* Read in file while the count is no 0xffff */
344  while (count != 0xffff) {
345  int addr;
346 
347  /* After the count is the address */
348  addr = dsp[index++];
349 
350  /* loop count times and write the data to the dsp memory */
351  while (count > 0) {
352  /* Check to see if this is data memory or program memory */
353  if (addr & 0x4000) {
354  int data = 0;
355 
356  /* Data memory is 16 bits and is on one line */
357  data = dsp[index++];
358  WriteJr3Dm(addr, data);
359  count--;
360  if (data != ReadJr3Dm(addr)) {
361  printf("data addr: %4.4x out: %4.4x in: %4.4x\n", addr,
362  data, ReadJr3Dm(addr));
363  }
364  } else {
365  int data, data2;
366  int data3;
367 
368  /* Program memory is 24 bits and is on two lines */
369  data = dsp[index++];
370  data2 = dsp[index++];
371  WriteJr3Pm2(addr, data, data2);
372  count -= 2;
373 
374  /* Verify the write */
375  if (((data << 8) | (data2 & 0xff))
376  != (data3 = ReadJr3Pm(addr))) {
377  // printf("pro addr: %4.4x out: %6.6x in: %6.6x\n", addr, ((data << 8)|(data2 & 0xff)), /* ReadJr3Pm(addr) */ data3);
378  }
379  }
380  addr++;
381  }
382  count = dsp[index++];
383  }
384 
385  return 0;
386 
387 }
388 
389 void get_force_sensor_info(force_sensor_data * data, char *msg) {
390  struct tm t1, *soft_day, *cal_day;
391  ;
392  time_t t2;
393  t1.tm_sec = t1.tm_min = t1.tm_hour = 0;
394  t1.tm_isdst = -1;
395  t1.tm_mon = 0;
396  t1.tm_year = data->software_year - 1900;
397  t1.tm_mday = data->software_day;
398  t2 = mktime(&t1);
399  soft_day = localtime(&t2);
400  t1.tm_sec = t1.tm_min = t1.tm_hour = 0;
401  t1.tm_isdst = -1;
402  t1.tm_mon = 0;
403  t1.tm_year = data->cal_year - 1900;
404  t1.tm_mday = data->cal_day;
405  t2 = mktime(&t1);
406  cal_day = localtime(&t2);
407  snprintf(msg, 128, "rom # %d, soft # %d, %d/%d/%d\n"
408  "serial # %d, model # %d, cal day %d/%d/%d\n"
409  "%s, %d bits, %d ch\n"
410  "(C) %c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n\n",
411  data->eeprom_ver_no, data->software_ver_no, soft_day->tm_mday,
412  soft_day->tm_mon + 1, soft_day->tm_year + 1900, data->serial_no,
413  data->model_no, cal_day->tm_mday, cal_day->tm_mon + 1,
414  cal_day->tm_year + 1900, force_units_str[1], data->bit,
415  data->channels, data->copyright[0 * 4 + 1],
416  data->copyright[1 * 4 + 1], data->copyright[2 * 4 + 1],
417  data->copyright[3 * 4 + 1], data->copyright[4 * 4 + 1],
418  data->copyright[5 * 4 + 1], data->copyright[6 * 4 + 1],
419  data->copyright[7 * 4 + 1], data->copyright[8 * 4 + 1],
420  data->copyright[9 * 4 + 1], data->copyright[10 * 4 + 1],
421  data->copyright[11 * 4 + 1], data->copyright[12 * 4 + 1],
422  data->copyright[13 * 4 + 1], data->copyright[14 * 4 + 1],
423  data->copyright[15 * 4 + 1], data->copyright[16 * 4 + 1],
424  data->copyright[17 * 4 + 1], data->copyright[18 * 4 + 1],
425  data->copyright[19 * 4 + 1], data->copyright[20 * 4 + 1],
426  data->copyright[21 * 4 + 1], data->copyright[22 * 4 + 1],
427  data->copyright[23 * 4 + 1]);
428 }
429 
430 int message_callback(message_context_t * ctp, int type, unsigned flags,
431  void *handle) {
432  server_msg_t *msg;
433  int num;
434  char msg_reply[255];
435 
436  /* cast a pointer to the message data */
437  msg = (server_msg_t *) ctp->msg;
438 
439  /* Print out some usefull information on the message */
440  //printf( "\n\nServer Got Message:\n" );
441  //printf( " type: %d\n" , type );
442  //printf( " data: %s\n\n", msg->msg_data );
443  force_sensor_data *data_0;
444  force_sensor_data *data_1;
445  data_0 = (force_sensor_data *) (Jr3BaseAddress0H + (Jr3DmAddrMask << 2));
446  data_1 = (force_sensor_data *) (Jr3BaseAddress1H + (Jr3DmAddrMask << 2));
447 
448  /* Build the reply message */
449  num = type - _IO_MAX;
450  switch (num) {
451  case 1: // get data
452  {
453  float tmp[12] = { -1.0 * (float) data_0->filter0.fx
454  / (float) data_0->full_scale.fx, -1.0
455  * (float) data_0->filter0.fy / (float) data_0->full_scale.fy,
456  -1.0 * (float) data_0->filter0.fz
457  / (float) data_0->full_scale.fz, -1.0
458  * (float) data_0->filter0.mx
459  / (float) data_0->full_scale.mx * 0.1, // Newton*meter*10
460  -1.0 * (float) data_0->filter0.my
461  / (float) data_0->full_scale.my * 0.1, -1.0
462  * (float) data_0->filter0.mz
463  / (float) data_0->full_scale.mz * 0.1, -1.0
464  * (float) data_1->filter0.fx
465  / (float) data_1->full_scale.fx, -1.0
466  * (float) data_1->filter0.fy
467  / (float) data_1->full_scale.fy, -1.0
468  * (float) data_1->filter0.fz
469  / (float) data_1->full_scale.fz, -1.0
470  * (float) data_1->filter0.mx
471  / (float) data_1->full_scale.mx * 0.1, -1.0
472  * (float) data_1->filter0.my
473  / (float) data_1->full_scale.my * 0.1, -1.0
474  * (float) data_1->filter0.mz
475  / (float) data_1->full_scale.mz * 0.1 };
476  memcpy(msg_reply, tmp, sizeof(float) * 12);
477  }
478  break;
479  case 2: // update offset
480  data_0->offsets.fx += data_0->filter0.fx;
481  data_0->offsets.fy += data_0->filter0.fy;
482  data_0->offsets.fz += data_0->filter0.fz;
483  data_0->offsets.mx += data_0->filter0.mx;
484  data_0->offsets.my += data_0->filter0.my;
485  data_0->offsets.mz += data_0->filter0.mz;
486  data_1->offsets.fx += data_1->filter0.fx;
487  data_1->offsets.fy += data_1->filter0.fy;
488  data_1->offsets.fz += data_1->filter0.fz;
489  data_1->offsets.mx += data_1->filter0.mx;
490  data_1->offsets.my += data_1->filter0.my;
491  data_1->offsets.mz += data_1->filter0.mz;
492  break;
493  case 3: // set filter
494  break;
495  case 4: // get data
496  get_force_sensor_info(data_0, msg_reply);
497  get_force_sensor_info(data_1, msg_reply + strlen(msg_reply));
498  break;
499  }
500 
501  /* Send a reply to the waiting (blocked) client */
502  MsgReply(ctp->rcvid, EOK, msg_reply, 256);
503  return 0;
504 }
505 
506 int main(int argc, char **argv) {
507  resmgr_attr_t resmgr_attr;
508  message_attr_t message_attr;
509  dispatch_t *dpp;
510  dispatch_context_t *ctp, *ctp_ret;
511  int resmgr_id, message_id;
512 
513  /* Create the Dispatch Interface */
514  dpp = dispatch_create();
515  if (dpp == NULL) {
516  fprintf(stderr, "dispatch_create() failed: %s\n", strerror(errno));
517  return EXIT_FAILURE;
518  }
519 
520  memset(&resmgr_attr, 0, sizeof(resmgr_attr));
521  resmgr_attr.nparts_max = 1;
522  resmgr_attr.msg_max_size = 2048;
523 
524  /* Setup the default I/O functions to handle open/read/write/... */
525  iofunc_func_init(_RESMGR_CONNECT_NFUNCS, &ConnectFuncs, _RESMGR_IO_NFUNCS,
526  &IoFuncs);
527 
528  IoFuncs.read = io_read;
529 
530  /* Setup the attribute for the entry in the filesystem */
531  iofunc_attr_init(&IoFuncAttr, S_IFNAM | 0666, 0, 0);
532  IoFuncAttr.nbytes = strlen(buffer) + 1; //
533 
534  resmgr_id = resmgr_attach(dpp, &resmgr_attr, "/dev/jr3q", _FTYPE_ANY, 0,
536  if (resmgr_id == -1) {
537  fprintf(stderr, "resmgr_attach() failed: %s\n", strerror(errno));
538  return EXIT_FAILURE;
539  }
540 
541  /* Setup PCI */
542  unsigned int flags = 0;
543  unsigned bus, function, index = 0;
544  unsigned devid, venid;
545  int result;
546  unsigned lastbus, version, hardware;
547  unsigned int address0;
548  void *bufptr;
549  unsigned int addr;
550 
551  pci_attach(flags);
552  result = pci_present(&lastbus, &version, &hardware);
553  if (result == -1) {
554  printf("PCI BIOS not present!\n");
555  }
556  devid = DEVICEID;
557  venid = VENDORID;
558  result = pci_find_device(devid, venid, index, &bus, &function);
559  printf("result PCI %d\n", result);
560  printf("bus %d\n", bus);
561  printf("function %d\n", function);
562  pci_read_config_bus(bus, function, 0x10, 1, sizeof(unsigned int), bufptr);
563 
564  address0 = *(unsigned int *) bufptr;
565  buffer = (char *) bufptr;
566  printf("Board addr 0x%x\n", *(unsigned int *) buffer);
567 
568  Jr3BaseAddress0H = (volatile uint32_t) mmap_device_memory(NULL, 0x100000,
569  PROT_READ | PROT_WRITE | PROT_NOCACHE, 0, address0);
570  Jr3BaseAddress0L = (volatile uint32_t) mmap_device_memory(NULL, 0x100000,
571  PROT_READ | PROT_WRITE | PROT_NOCACHE, 0, address0 + Jr3NoAddrMask);
572  Jr3BaseAddressH = (volatile uint32_t) mmap_device_memory(NULL, 0x100000,
573  PROT_READ | PROT_WRITE | PROT_NOCACHE, 0, address0);
574  WriteJr3Dm(Jr3ResetAddr, 0); // Reset DSP
575  wait_t();
577  wait_t();
578 
579  Jr3BaseAddress1H = (volatile uint32_t) mmap_device_memory(NULL, 0x100000,
580  PROT_READ | PROT_WRITE | PROT_NOCACHE, 0, address0 + 0x80000);
581  Jr3BaseAddress1L = (volatile uint32_t) mmap_device_memory(NULL, 0x100000,
582  PROT_READ | PROT_WRITE | PROT_NOCACHE, 0, address0 + 0x80000 +
583  Jr3NoAddrMask);
585  wait_t();
586  /*
587  Jr3BaseAddress2H = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x100000);
588  Jr3BaseAddress2L = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x100000 + Jr3NoAddrMask);
589  download(Jr3BaseAddress2H, Jr3BaseAddress2L);
590  wait_t();
591 
592  Jr3BaseAddress3H = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x180000);
593  Jr3BaseAddress3L = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x180000 + Jr3NoAddrMask);
594  download(Jr3BaseAddress3H, Jr3BaseAddress3L);
595  wait_t();
596  */
597 
598  //reset
599  *(uint16_t *) (Jr3BaseAddress0H + ((0x0200 | Jr3DmAddrMask) << 2)) =
600  (uint16_t) 0;
601  *(uint16_t *) (Jr3BaseAddress0H + ((0x00e7 | Jr3DmAddrMask) << 2)) =
602  (uint16_t) 0x0500;
603 
604  /* Setup our message callback */
605  memset(&message_attr, 0, sizeof(message_attr));
606  message_attr.nparts_max = 1;
607  message_attr.msg_max_size = 4096;
608 
609  /* Attach a callback (handler) for two message types */
610  message_id = message_attach(dpp, &message_attr, _IO_MAX + 1, _IO_MAX + 10,
611  message_callback, NULL);
612  if (message_id == -1) {
613  fprintf(stderr, "message_attach() failed: %s\n", strerror(errno));
614  return EXIT_FAILURE;
615  }
616 
617  /* Setup a context for the dispatch layer to use */
618  ctp = dispatch_context_alloc(dpp);
619  if (ctp == NULL) {
620  fprintf(stderr, "dispatch_context_alloc() failed: %s\n",
621  strerror(errno));
622  return EXIT_FAILURE;
623  }
624 
625  /* The "Data Pump" - get and process messages */
626  while (1) {
627  ctp_ret = dispatch_block(ctp);
628  if (ctp_ret) {
629  dispatch_handler(ctp);
630  } else {
631  fprintf(stderr, "dispatch_block() failed: %s\n", strerror(errno));
632  return EXIT_FAILURE;
633  }
634  }
635 
636  return EXIT_SUCCESS;
637 }
638 
639 int io_read(resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb) {
640  int nleft;
641  int nbytes;
642  int nparts;
643  int status;
644 
645  if ((status = iofunc_read_verify(ctp, msg, ocb, NULL)) != EOK)
646  return (status);
647 
648  if ((msg->i.xtype & _IO_XTYPE_MASK) != _IO_XTYPE_NONE)
649  return (ENOSYS);
650 
651  /*
652  * on all reads (first and subsequent) calculate
653  * how many bytes we can return to the client,
654  * based upon the number of bytes available (nleft)
655  * and the client's buffer size
656  */
657 
658  nleft = ocb->attr->nbytes - ocb->offset;
659  nbytes = min(msg->i.nbytes, nleft);
660 
661  if (nbytes > 0) {
662  /* set up the return data IOV */
663  SETIOV(ctp->iov, buffer + ocb->offset, nbytes);
664 
665  /* set up the number of bytes (returned by client's read()) */
666  _IO_SET_READ_NBYTES(ctp, nbytes);
667 
668  /*
669  * advance the offset by the number of bytes
670  * returned to the client.
671  */
672 
673  ocb->offset += nbytes;
674  nparts = 1;
675  } else {
676  /* they've adked for zero bytes or they've already previously
677  * read everything
678  */
679 
680  _IO_SET_READ_NBYTES(ctp, 0);
681  nparts = 0;
682  }
683 
684  /* mark the access time as invalid (we just accessed it) */
685 
686  if (msg->i.nbytes > 0)
687  ocb->attr->flags |= IOFUNC_ATTR_ATIME;
688 
689  return (_RESMGR_NPARTS(nparts));
690 
691 }
692 
693 void wait_t(void) {
694  struct timeval tv, tv1;
695 
696  gettimeofday(&tv1, NULL);
697 
698  do {
699  gettimeofday(&tv, NULL);
700  } while (tv.tv_sec - tv1.tv_sec < 3); // wait for 3 sec.
701 
702  return;
703 }
six_axis_array default_FS
Definition: jr3_driver.cpp:229
int16_t dummy2
Definition: jr3_driver.cpp:198
vect_bits vect_axes
Definition: jr3_driver.cpp:248
int16_t dummy1
Definition: jr3_driver.cpp:164
uint8_t changedV1
Definition: jr3_driver.cpp:132
volatile uint32_t Jr3BaseAddressH
Definition: jr3_driver.cpp:67
int16_t dummy1
Definition: jr3_driver.cpp:187
int16_t dummy1
Definition: jr3_driver.cpp:93
png_infop int transforms
uint8_t my_near_set
Definition: jr3_driver.cpp:142
#define Jr3ResetAddr
Definition: jr3_driver.cpp:37
int16_t number_of_ge_thresholds
Definition: jr3_driver.cpp:197
int16_t threshold
Definition: jr3_driver.cpp:188
static char * buffer
Definition: jr3_driver.cpp:25
uint16_t rate_divisor
Definition: jr3_driver.cpp:265
int16_t dummy2
Definition: jr3_driver.cpp:86
static resmgr_connect_funcs_t ConnectFuncs
Definition: jr3_driver.cpp:62
int16_t my
Definition: jr3_driver.cpp:100
int16_t dummy6
Definition: jr3_driver.cpp:103
int16_t bit_pattern
Definition: jr3_driver.cpp:190
static iofunc_attr_t IoFuncAttr
Definition: jr3_driver.cpp:64
uint8_t mx_sat
Definition: jr3_driver.cpp:153
error_bits errors
Definition: jr3_driver.cpp:292
uint8_t my
Definition: jr3_driver.cpp:130
#define WriteJr3Dm(addr, data)
Definition: jr3_driver.cpp:56
#define VENDORID
Definition: jr3_driver.cpp:31
volatile uint32_t Jr3BaseAddress1L
Definition: jr3_driver.cpp:72
uint8_t my_sat
Definition: jr3_driver.cpp:154
uint8_t fz_near_set
Definition: jr3_driver.cpp:140
char * force_units_str[]
Definition: jr3_driver.cpp:167
force_array filter1
Definition: jr3_driver.cpp:250
int16_t fz
Definition: jr3_driver.cpp:96
int16_t dummy4
Definition: jr3_driver.cpp:203
force_array full_scale
Definition: jr3_driver.cpp:244
#define EXIT_SUCCESS
uint8_t cal_crc_bad
Definition: jr3_driver.cpp:161
six_axis_array max_full_scale
Definition: jr3_driver.cpp:239
uint8_t mx_near_set
Definition: jr3_driver.cpp:141
#define Jr3DmAddrMask
Definition: jr3_driver.cpp:39
data
stderr
#define DEVICEID
Definition: jr3_driver.cpp:34
int16_t raw_data
Definition: jr3_driver.cpp:85
int16_t number_of_le_thresholds
Definition: jr3_driver.cpp:199
png_uint_32 int flags
force_array filter3
Definition: jr3_driver.cpp:252
force_array rate_data
Definition: jr3_driver.cpp:256
int16_t latch_bits
Definition: jr3_driver.cpp:195
int16_t dummy3
Definition: jr3_driver.cpp:191
static resmgr_io_funcs_t IoFuncs
Definition: jr3_driver.cpp:63
#define ReadJr3Dm(addr)
Definition: jr3_driver.cpp:55
int16_t dummy2
Definition: jr3_driver.cpp:189
force_units
Definition: jr3_driver.cpp:174
int16_t near_sat_value
Definition: jr3_driver.cpp:259
int16_t dummy5
Definition: jr3_driver.cpp:101
int gettimeofday(struct timeval *tv, struct timezone *tz)
int16_t dummy
Definition: jr3_driver.cpp:134
link_types
Definition: jr3_driver.cpp:206
unsigned int index
int16_t dummy2
Definition: jr3_driver.cpp:95
#define ReadJr3Pm(addr)
Definition: jr3_driver.cpp:47
status
int16_t dummy3[2]
Definition: jr3_driver.cpp:88
uint8_t memry_error
Definition: jr3_driver.cpp:158
uint8_t mz_near_set
Definition: jr3_driver.cpp:143
int16_t v2
Definition: jr3_driver.cpp:106
int16_t dummy1
Definition: jr3_driver.cpp:196
force_units units
Definition: jr3_driver.cpp:313
int16_t fx
Definition: jr3_driver.cpp:92
uint8_t mz_sat
Definition: jr3_driver.cpp:155
uint8_t reserved
Definition: jr3_driver.cpp:144
six_axis_array shunts
Definition: jr3_driver.cpp:226
six_axis_array offsets
Definition: jr3_driver.cpp:245
#define min(a, b)
Definition: jr3_driver.cpp:22
int16_t threshold_bits
Definition: jr3_driver.cpp:293
uint8_t system_busy
Definition: jr3_driver.cpp:160
force_array filter0
Definition: jr3_driver.cpp:249
uint8_t changedV2
Definition: jr3_driver.cpp:133
force_array filter6
Definition: jr3_driver.cpp:255
int16_t dummy3
Definition: jr3_driver.cpp:97
uint16_t error_count
Definition: jr3_driver.cpp:287
int message_callback(message_context_t *ctp, int type, unsigned flags, void *handle)
Definition: jr3_driver.cpp:430
int16_t v1
Definition: jr3_driver.cpp:104
int16_t dummy8
Definition: jr3_driver.cpp:107
force_array filter2
Definition: jr3_driver.cpp:251
version
int main(int argc, char **argv)
Definition: jr3_driver.cpp:506
uint8_t watch_dog
Definition: jr3_driver.cpp:163
warning_bits warnings
Definition: jr3_driver.cpp:291
void wait_t(void)
Definition: jr3_driver.cpp:693
int16_t dummy
Definition: jr3_driver.cpp:146
volatile uint32_t Jr3BaseAddress0L
Definition: jr3_driver.cpp:70
uint8_t researved
Definition: jr3_driver.cpp:156
uint8_t fx
Definition: jr3_driver.cpp:126
uint8_t fy
Definition: jr3_driver.cpp:127
uint8_t fy_near_set
Definition: jr3_driver.cpp:139
int16_t mz
Definition: jr3_driver.cpp:102
volatile uint32_t Jr3BaseAddressL
Definition: jr3_driver.cpp:68
int16_t data_address
Definition: jr3_driver.cpp:186
int16_t dummy4
Definition: jr3_driver.cpp:99
uint16_t dummy1
Definition: jr3_driver.cpp:84
uint8_t fx_sat
Definition: jr3_driver.cpp:150
char copyright[0x18 *4]
Definition: jr3_driver.cpp:224
int16_t load_envelope_num
Definition: jr3_driver.cpp:232
volatile uint32_t Jr3BaseAddress0H
Definition: jr3_driver.cpp:69
int16_t reserved
Definition: jr3_driver.cpp:202
int16_t dummy7
Definition: jr3_driver.cpp:105
int num
int io_read(resmgr_context_t *ctp, io_read_t *msg, RESMGR_OCB_T *ocb)
Definition: jr3_driver.cpp:639
#define WriteJr3Pm2(addr, data, data2)
Definition: jr3_driver.cpp:50
int16_t mx
Definition: jr3_driver.cpp:98
uint8_t fz
Definition: jr3_driver.cpp:128
force_array filter4
Definition: jr3_driver.cpp:253
int16_t fy
Definition: jr3_driver.cpp:94
uint16_t raw_time
Definition: jr3_driver.cpp:83
void get_force_sensor_info(force_sensor_data *data, char *msg)
Definition: jr3_driver.cpp:389
#define EXIT_FAILURE
int download(unsigned int base0, unsigned int base1)
Definition: jr3_driver.cpp:329
uint8_t sensor_charge
Definition: jr3_driver.cpp:159
uint8_t watch_dog2
Definition: jr3_driver.cpp:162
force_array minimum_data
Definition: jr3_driver.cpp:257
uint8_t fy_sat
Definition: jr3_driver.cpp:151
#define Jr3NoAddrMask
Definition: jr3_driver.cpp:38
force_array filter5
Definition: jr3_driver.cpp:254
int16_t software_ver_no
Definition: jr3_driver.cpp:299
six_axis_array min_full_scale
Definition: jr3_driver.cpp:234
uint8_t fz_sat
Definition: jr3_driver.cpp:152
uint8_t mx
Definition: jr3_driver.cpp:129
uint8_t mz
Definition: jr3_driver.cpp:131
int16_t dummy3
Definition: jr3_driver.cpp:200
int16_t reserved[2]
Definition: jr3_driver.cpp:87
unsigned long MappedAddress
Definition: jr3_driver.cpp:40
force_array maximum_data
Definition: jr3_driver.cpp:258
uint8_t fx_near_set
Definition: jr3_driver.cpp:138
volatile uint32_t Jr3BaseAddress1H
Definition: jr3_driver.cpp:71


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15