- case CAN_ADC_2:
- orte_eb2008->sharps.back_left = s_gp2d120_ir2mmShort((frame.data[4] << 8)| \
- (frame.data[5]))/1000.0;
- orte_eb2008->sharps.back_right = s_gp2d120_ir2mmShort((frame.data[6] << 8)| \
- (frame.data[7]))/1000.0;
- ORTEPublicationSend(orte_eb2008->publication_sharps);
- break;
- /* laser data */
- case CAN_LAS1:
-// printf("CAN: ");
-// for (i=0; i<frame.can_dlc; i++) {
-// printf("0x%02x ", frame.data[i]);
-// }
-// printf("can_dlc=%d\n", frame.can_dlc);
- orte_eb2008->laser_meas.cnt = frame.data[1];
- las_bcnt = orte_eb2008->laser_meas.cnt;
- last_id = frame.data[0];
- las_di = 4;
- /* rotation period */
- orte_eb2008->laser_meas.period = (frame.data[2]<<8)|(frame.data[3]);
-// printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
-// frame.can_id, orte_eb2008->laser_meas.cnt, orte_eb2008->laser_meas.period);
-
- for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
- switch (las_mi) {
- case 0: las_meas = &orte_eb2008->laser_meas.measures0; break;
- case 1: las_meas = &orte_eb2008->laser_meas.measures1; break;
- default: break;
- }
- *las_meas =
- (frame.data[las_di++]<<8)|(frame.data[las_di++]);
-// printf("0x%02x 0x%02x %u ",
-// frame.data[las_di-2], frame.data[las_di-1], *las_meas);
- }
-// printf("\n");
- break;
- case CAN_LAS2:
- case CAN_LAS3:
- case CAN_LAS4:
-// printf("CAN: ");
-// for (i=0; i<frame.can_dlc; i++) {
-// printf("0x%02x ", frame.data[i]);
-// }
-// printf("can_dlc=%d\n", frame.can_dlc);
-// printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
-// frame.can_id, orte_eb2008->laser_meas.cnt, orte_eb2008->laser_meas.period);
- if (frame.data[0] != (last_id+(frame.can_id-CAN_LAS1)))
- break;
- las_di = 2;
- while (las_di < 8 && las_bcnt > 0) {
- switch (las_mi) {
- case 2: las_meas = &orte_eb2008->laser_meas.measures2; break;
- case 3: las_meas = &orte_eb2008->laser_meas.measures3; break;
- case 4: las_meas = &orte_eb2008->laser_meas.measures4; break;
- case 5: las_meas = &orte_eb2008->laser_meas.measures5; break;
- case 6: las_meas = &orte_eb2008->laser_meas.measures6; break;
- case 7: las_meas = &orte_eb2008->laser_meas.measures7; break;
- case 8: las_meas = &orte_eb2008->laser_meas.measures8; break;
- case 9: las_meas = &orte_eb2008->laser_meas.measures9; break;
- default: break;
- }
- *las_meas =
- (frame.data[las_di++]<<8)|(frame.data[las_di++]);
-// printf("0x%02x 0x%02x %u ",
-// frame.data[las_di-2], frame.data[las_di-1], *las_meas);
- las_mi++;
- las_bcnt--;
- }
- printf("\n");
- if (las_bcnt == 0) {
-// printf("ORTEPublicationSend()\n");
- ORTEPublicationSend(orte_eb2008->publication_laser_meas);
- }
- break;