Changeset 74 in pacpussensors for trunk/Sick/SickLMSSensor.cpp


Ignore:
Timestamp:
01/16/15 17:01:19 (9 years ago)
Author:
DHERBOMEZ Gérald
Message:
  • Modification of Sick LMS sensor to get the RSSI signal (reflectivity of the laser)
  • Minor changes in data replay component of Sick LDMRS
  • CanGateway: Choose by default of Vector CAN XL card
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/Sick/SickLMSSensor.cpp

    r72 r74  
    110110//    sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
    111111//    S_socket->sendToServer(QString(buf));
    112 
    113 
    114 //    sprintf(buf, "%c%s%c", 0x02, "sWN LMDscandatacfg 03 00 1 1 0 00 00 0 0 0 0 1 1", 0x03);
     112//
     113//    //sprintf(buf, "%c%s%c", 0x02, "sMN mLMPsetscancfg 2500 1 2500 300000 1500000", 0x03);//chunlei to configure the angle of the sensor
     114//    //S_socket->sendToServer(QString(buf));
     115//
     116//    sprintf(buf, "%c%s%c", 0x02, "sWN LMDscandatacfg 01 01 1 1 0 00 00 0 0 0 0 1 1", 0x03);
    115117//    S_socket->sendToServer(QString(buf));
    116 
     118//
    117119////    sprintf(buf, "%c%s%c", 0x02, "sWN FREchoFilter 1", 0x03);
    118120////    S_socket->sendToServer(QString(buf));
    119 
     121//
     122//
    120123/////// SAVE PERMANENT
    121124//    sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
    122125//    S_socket->sendToServer(QString(buf));
    123 
     126//
    124127////    /// LOGOUT
    125128//    sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
    126129//    S_socket->sendToServer(QString(buf));
    127 
     130////
    128131
    129132    // Start measurement
    … …  
    367370    msg->data.rssi_len2 = 0;
    368371
    369 
    370372//    for(std::vector<std::string>::iterator it = msg->splitMessage->begin(); it != msg->splitMessage->end(); it++){
    371373//        printf("%s ", (*it).c_str());
    … …  
    397399    msg->data.scanFrequency = xstol(msg->splitMessage->at(16));
    398400    LOG_TRACE("(Parsing) Scan frequency "<< msg->data.scanFrequency <<" [1/100 Hz]");
    399 
     401        //LOG_INFO("(Parsing) Scan frequency "<< msg->data.scanFrequency <<" [1/100 Hz]");
    400402    //    17 //MeasurementFrequency
    401403
    … …  
    403405    int NumberEncoders = xstol(msg->splitMessage->at(18));
    404406    LOG_TRACE("(Parsing) Number Encoders "<< NumberEncoders);
    405 
     407        //LOG_INFO("(Parsing) Number Encoders "<< NumberEncoders);
    406408        for (int i = 0; i < NumberEncoders; i++) {
    407409    //         //EncoderPosition
    … …  
    449451                scalingFactor = 2;
    450452            LOG_TRACE("(Parsing 16bit channel #"<<i<<") Scaling factor x"<< scalingFactor);
    451 
     453                        //LOG_INFO("(Parsing 16bit channel #"<<i<<") Scaling factor x"<< scalingFactor);
    452454//            19+NumberEncoders*2+i*6+totalData16+3 //ScalingOffset
    453455//            19+NumberEncoders*2+i*6+totalData16+4 //Starting angle
    454456            msg->data.startAngle = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+4));
    455457            LOG_TRACE("(Parsing 16bit channel #"<<i<<") Start angle "<< msg->data.startAngle << " [1/10000 degree]");
    456 
     458                        //LOG_INFO("(Parsing 16bit channel #"<<i<<") Start angle "<< msg->data.startAngle << " [1/10000 degree]");
    457459//            19+NumberEncoders*2+i*6+totalData16+5 //Angular step width
    458460            msg->data.angleResolution = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+5));
    459461            LOG_TRACE("(Parsing 16bit channel #"<<i<<") Angular step width "<< msg->data.angleResolution<<" [1/10000 degree]");
    460 
     462                        //LOG_INFO("(Parsing 16bit channel #"<<i<<") Angular step width "<< msg->data.angleResolution<<" [1/10000 degree]");
    461463//            19+NumberEncoders*2+i*6+totalData16+6 //NumberData
    462464            NumberData = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+6));
    … …  
    501503        int NumberChannels8Bit = xstol(msg->splitMessage->at(19+NumberEncoders*2+NumberChannels16Bit*6+totalData16+1));
    502504        LOG_TRACE("(Processing) Number channels 8Bit : "<<NumberChannels8Bit);
    503 
     505                //LOG_INFO("(Processing) Number channels 8Bit : "<<NumberChannels8Bit);
    504506        LOG_INFO("(Parsing) 8bit channel not implemented yet !");
    505507
    506 /*
     508
    507509//        int totalData8 = 0;
    508 //        for (int i = 0; i < NumberChannels8Bit; i++) {
    509 
     510//        for (int i = 0; i < NumberChannels8Bit; i++)
     511//              {
     512//
    510513//            int type = -1;
    511 
     514//
    512515//            std::string content = msg->splitMessage->at(21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6);
    513516//            LOG_TRACE("(Parsing 8bit channel) Measured Data Content : " << content);
    514 //            if (content == "DIST1") {
     517//            if (content == "RSSI1") {
    515518//                type = 0;
    516 //            } else if (content=="DIST2") {
     519//            } else if (content=="RSSI2") {
    517520//                type = 1;
    518 //            } else if (content=="RSSI1") {
     521//            }
     522//              /*      else if (content=="RSSI1") {
    519523//                type = 2;
    520524//            } else if (content == "RSSI2") {
    521525//                type = 3;
    522526//            }
    523 
     527//                      */
     528//           
     529//                      LOG_INFO("(Parsing 8bit channel #"<<i<<") Scaling factor x"<< 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6+1);
     530////            19+NumberEncoders*2+i*6+totalData16+3 //ScalingOffset
     531////            19+NumberEncoders*2+i*6+totalData16+4 //Starting angle
     532//
     533//                      LOG_INFO("(Parsing 8bit channel #"<<i<<") Start angle "<< 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6+3 << " [1/10000 degree]");
     534////            19+NumberEncoders*2+i*6+totalData16+5 //Angular step width
     535//
     536//                      LOG_INFO("(Parsing 8bit channel #"<<i<<") Angular step width "<< 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6+4<<" [1/10000 degree]");
     537///*
    524538//            21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+1 //ScalingFactor
    525539//            21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+2 //ScalingOffset
    … …  
    527541//            21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+4 //Angular step width
    528542//            21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+5 //NumberData
    529 
     543//*/
    530544//            int NumberData = xstol(msg->splitMessage->at(21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6+5));
    531 //            LOG_TRACE("(Parsing 16bit channel) Number Data for "<<content<<" : "<<NumberData);
    532 
    533 //            sscanf(tok, "%X", &NumberData);
    534 
    535 //            LOG_TRACE("(Processing) Number data : "<< NumberData);
    536 
     545//            LOG_TRACE("(Parsing 8bit channel) Number Data for "<<content<<" : "<<NumberData);
     546//
     547//            //sscanf(tok, "%X", &NumberData);
     548//
     549//            LOG_TRACE("(Processing) Number data : "<< NumberData);    //19+NumberEncoders*2+i*6+totalData16+6
     550//                      LOG_INFO("(Parsing 8bit channel #"<<i<<") Number data "<< NumberData);
    537551//            if (type == 0) {
    538 //                msg->data.dist_len1 = NumberData;
     552//                msg->data.rssi_len1 = NumberData;
    539553//            } else if (type == 1) {
    540 //                msg->data.dist_len2 = NumberData;
    541 //            } else if (type == 2) {
    542 //                msg->data.rssi_len1 = NumberData;
    543 //            } else if (type == 3) {
    544554//                msg->data.rssi_len2 = NumberData;
     555//            }
     556//                     
     557//            uint8_t* remissionPoints = (uint8_t*) malloc(NumberData * sizeof(uint8_t));
     558//            for (int j = 0; j < NumberData; j++) {
     559//                remissionPoints[j] = xstol(msg->splitMessage->at(21 + NumberEncoders*2 + NumberChannels16Bit*6 + totalData16 + totalData8 + i*6 + 6 + j));    //19+NumberEncoders*2+i*6+totalData16+7+j
     560//            }                 
     561//            //for (int i = 0; i < NumberData; i++) {
     562//                //int dat;
     563//                //tok = strtok(NULL, " "); //data
     564//                //sscanf(tok, "%X", &dat);
     565//
     566//            if (type == 0) {
     567//                msg->data.rssi1_8 = remissionPoints;
     568//                              msg->data.rssi_len1_8 = NumberData;
     569//            } else if (type == 1) {
     570//                msg->data.rssi2_8 = remissionPoints;
     571//                              msg->data.rssi_len2_8 = NumberData;
    545572//            }
    546 //            for (int i = 0; i < NumberData; i++) {
    547 //                int dat;
    548 //                tok = strtok(NULL, " "); //data
    549 //                sscanf(tok, "%X", &dat);
    550 
    551 //                if (type == 0) {
    552 //                    msg->data.dist1[i] = dat;
    553 //                } else if (type == 1) {
    554 //                    msg->data.dist2[i] = dat;
    555 //                } else if (type == 2) {
    556 //                    msg->data.rssi1[i] = dat;
    557 //                } else if (type == 3) {
    558 //                    msg->data.rssi2[i] = dat;
    559 //                }
    560 //            }
    561         } // 8bit channel
    562 */
     573//            //}
     574//                      totalData8 += NumberData;
     575//        } // 8bit channel
     576//
    563577
    564578                return 0;
    … …  
    588602    entry.rssi_len2 = entry.dataPos_rssi2 = 0;
    589603
     604    //entry.rssi_len1_8 = entry.dataPos_rssi1_8 = 0;
     605    //entry.rssi_len2_8 = entry.dataPos_rssi2_8 = 0;
     606
    590607    if(msg.data.dist_len1){
    591608        entry.dist_len1 = msg.data.dist_len1;
    … …  
    658675        free(msg.data.rssi2);
    659676    }
     677        //if(msg.data.rssi_len1_8){
     678 //       entry.rssi_len1_8 = msg.data.rssi_len1_8;
     679 //       entry.dataPos_rssi1_8 = dataFile_.tellp();
     680
     681 //       for (unsigned int i = 0 ; i < msg.data.rssi_len1_8; ++i) {
     682 //           dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi1_8[i])), sizeof(uint8_t));
     683 //       }
     684
     685 //       free(msg.data.rssi1_8);
     686 //   }
     687 //   if(msg.data.rssi_len2_8){
     688 //       entry.rssi_len2_8 = msg.data.rssi_len2_8;
     689 //       entry.dataPos_rssi2_8 = dataFile_.tellp();
     690
     691 //       for (unsigned int i = 0 ; i < msg.data.rssi_len2_8; ++i) {
     692 //           dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi2_8[i])), sizeof(uint8_t));
     693 //       }
     694
     695 //       free(msg.data.rssi2_8);
     696 //   }
    660697    // add a magic word to delimit the block of data
    661698    int32_t utcMagicWord = UTC_MAGIC_WORD;
Note: See TracChangeset for help on using the changeset viewer.