Changeset 74 in pacpussensors


Ignore:
Timestamp:
Jan 16, 2015, 5:01:19 PM (10 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
Location:
trunk
Files:
7 edited

Legend:

Unmodified
Added
Removed

  • trunk/CMakeLists.txt

    r72 r74  
    8484# ========================================
    8585# add_subdirectory(Alasca)
    86 #add_subdirectory(CanGateway)
     86add_subdirectory(CanGateway)
    8787#add_subdirectory(PtGreyCameras)
    8888# add_subdirectory(Dualshock)
    … …  
    9191#add_subdirectory(StereoVisionDisparity)
    9292#add_subdirectory(SensorsApplication)
    93 # add_subdirectory(StdDbtPlayerComponents)
     93add_subdirectory(StdDbtPlayerComponents)
    9494# add_subdirectory(Wifibot)
    95 #add_subdirectory(Gps)
     95add_subdirectory(Gps)
     96#add_subdirectory(OpencvVideo)
    9697#add_subdirectory(SpanCPTComponent)
    97 #add_subdirectory(NMEA0183)
    98 #add_subdirectory(OpencvVideo)
     98add_subdirectory(NMEA0183)
     99add_subdirectory(VelodyneComponent)
    99100# ========================================
    100101# Summary

  • trunk/CanGateway/Win32CanInterface.h

    r63 r74  
    1717// we use the Vector driver
    1818#ifdef WIN32
    19 #   define CAN_DRIVER_TYPE       VECTOR_CAN_DRIVER
    20 //#   define CAN_DRIVER_TYPE       VECTOR_CANXL_DRIVER
     19//#   define CAN_DRIVER_TYPE       VECTOR_CAN_DRIVER
     20#   define CAN_DRIVER_TYPE       VECTOR_CANXL_DRIVER
    2121//#   define CAN_DRIVER_TYPE       PEAK_CAN_DRIVER_WIN
    2222#else

  • trunk/Sick/SickLMSData.h

    r61 r74  
    166166     */
    167167    int rssi_len1;
    168 
     168        //int rssi_len1_8;
    169169    /*!
    170170     * \brief Energy values for the first reflected pulse
    … …  
    173173    uint16_t* rssi1;
    174174
     175        //uint8_t* rssi1_8;
     176
    175177    /*!
    176178     * \brief Number of samples in rssi2.
    … …  
    178180     */
    179181    int rssi_len2;
    180 
     182        //int rssi_len2_8;
    181183    /*!
    182184     * \brief Energy values for the second reflected pulse
    … …  
    184186     */
    185187    uint16_t* rssi2;
     188        //uint8_t* rssi2_8;
    186189} scanData;
    187190
    … …  
    229232    int rssi_len2;                  //!< Number of energy values (2nd echo).
    230233    uint32_t dataPos_rssi2;         //!< Energy of the returned pulse (2nd echo).  @b LMS1xx @b only.
     234        //chunlei
     235        //int rssi_len1_8;                  //!< Number of energy values (1st echo).
     236        //uint32_t dataPos_rssi1_8;         //!< Energy of the returned pulse (1st echo). @b LMS1xx @b only. 8bit
     237        //int rssi_len2_8;
     238        //uint32_t dataPos_rssi2_8;         //!< Energy of the returned pulse (1st echo). @b LMS1xx @b only. 8bit
    231239}SickLMS_dbt;
    232240

  • 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;

  • trunk/StdDbtPlayerComponents/CMakeLists.txt

    r52 r74  
    33################################################################################
    44add_definitions( -DSTDDBTPLAYERCOMPONENTS_EXPORTS )
    5 
     5add_definitions( -DDBTPLYSICK_EXPORTS)
     6add_definitions( -DDBTPLYSICK_EXPORTS)
    67# ========================================
    78# Include directories
    … …  
    2223set(HDRS
    2324        StdDbtPlayerComponentsConfig.h
    24     # DbtPlySickLMSManager.h
    25     # DbtPlySickLDMRSManager.h
     25    DbtPlySickLMSManager.h
     26    DbtPlySickLDMRSManager.h
    2627)
    2728
    … …  
    3233set(
    3334    PROJECT_SRCS
    34     # DbtPlySickLMSManager.cpp
    35     # DbtPlySickLDMRSManager.cpp
     35    DbtPlySickLMSManager.cpp
     36    DbtPlySickLDMRSManager.cpp
    3637        ${HDRS}
    3738    ${PLUGIN_CPP}

  • trunk/StdDbtPlayerComponents/DbtPlySickLDMRSManager.cpp

    r50 r74  
    5959    DbtPlyFileManager::configureComponent(config);
    6060
    61     mDataFilename = param.getProperty("binFile");
     61    mDataFilename = config.getProperty("binFile");
    6262
    6363    return ComponentBase::CONFIGURED_OK;

  • trunk/StdDbtPlayerComponents/DbtPlySickLMSManager.cpp

    r43 r74  
    1717#include <string>
    1818
    19 #include "kernel/Log.h"
    20 #include "PacpusTools/ShMem.h"
    21 
     19#include "Pacpus/kernel/Log.h"
     20#include "Pacpus/PacpusTools/ShMem.h"
     21
     22#include <Pacpus/kernel/InputOutputInterface.h>
     23
     24#include <math.h>
    2225
    2326#define UTC_MAGIC_WORD  0x55544300
    24 
     27#define PI 3.1415936
    2528
    2629namespace pacpus {
    … …  
    5962    DbtPlyFileManager::configureComponent(config);
    6063
    61     mDataFilename = param.getProperty("binFile");
    62 
     64    //mDataFilename = param.getProperty("binFile");
     65        mDataFilename = config.getProperty("binFile");
    6366    return ComponentBase::CONFIGURED_OK;
    6467}
    … …  
    9699    mDataFile.close();
    97100}
     101
    98102
    99103//////////////////////////////////////////////////////////////////////////
    … …  
    115119    mSickDbt.scanFrequency = sickLMS_dbt->scanFrequency;
    116120    mSickDbt.angleResolution = sickLMS_dbt->angleResolution;
    117     mSickDbt.startAngle = sickLMS_dbt->startAngle;
     121    mSickDbt.startAngle = sickLMS_dbt->startAngle;
     122
     123        std::cout << "start angle is " << mSickDbt.startAngle  << std::endl;
     124        std::cout << "angle step is " << mSickDbt.angleResolution  << std::endl;
    118125    mSickDbt.time = t;
    119126    mSickDbt.timerange = tr;
    120127
    121 
    122     int sizes[7];
     128        /*int sizes[7];*/
     129        int sizes[9];
    123130    sizes[0] = mSickDbt.dist_len1 = sickLMS_dbt->dist_len1;
    124131    sizes[1] = mSickDbt.dist_len2 = sickLMS_dbt->dist_len2;
    … …  
    129136    sizes[6] = mSickDbt.rssi_len2 = sickLMS_dbt->rssi_len2;
    130137
    131     uint32_t pos[7];
     138    //sizes[7] = mSickDbt.rssi_len1 = sickLMS_dbt->rssi_len1_8;
     139    //sizes[8] = mSickDbt.rssi_len2 = sickLMS_dbt->rssi_len2_8;
     140    //uint32_t pos[7];
     141        uint32_t pos[9];
    132142    pos[0] = mSickDbt.dataPos_dist1 = sickLMS_dbt->dataPos_dist1;
    133143    pos[1] = mSickDbt.dataPos_dist2 = sickLMS_dbt->dataPos_dist2;
    … …  
    138148    pos[6] = mSickDbt.dataPos_rssi2 = sickLMS_dbt->dataPos_rssi2;
    139149
     150    //pos[7] = mSickDbt.dataPos_rssi1 = sickLMS_dbt->dataPos_rssi1_8;
     151    //pos[8] = mSickDbt.dataPos_rssi2 = sickLMS_dbt->dataPos_rssi2_8;
     152
    140153    LOG_TRACE("(ScanPointsInfo) dist1 points\tSize : " << mSickDbt.dist_len1 <<"\tPos. : "<< mSickDbt.dataPos_dist1);
    141154    LOG_TRACE("(ScanPointsInfo) dist2 points\tSize : " << mSickDbt.dist_len2 <<"\tPos. : "<< mSickDbt.dataPos_dist2);
    … …  
    147160
    148161
     162
    149163    uint16_t* data[7];
    150 
     164        uint8_t* remission_8[2];
    151165    int sumSizes = 0;
    152166
    153167    LOG_TRACE("Reading UTC file ... ");
    154     for(int i = 0; i<7; ++i){
    155 
    156         if(sizes[i]){
     168    for(int i = 0; i<7; ++i)
     169        {
     170
     171        if(sizes[i])
     172                {
    157173            mDataFile.seekg(pos[i]); // set the get pointer to the correct place
    158174            // // then copy the data contained in the binary file
    159175            data[i] = (uint16_t*) malloc(sizes[i]*sizeof(uint16_t));
    160             for (size_t j = 0; j < sizes[i]; ++j) {
     176            for (size_t j = 0; j < sizes[i]; ++j)
     177                        {
    161178                mDataFile.read(reinterpret_cast<char*>(data[i]+j), sizeof(uint16_t));
    162                 if(j%500==0){
     179                if(j%500==0)
     180                                {
    163181                    LOG_TRACE("Data : " << (uint16_t) *(data[i] + j));
    164182                }
     183                                if(i == 0)
     184                                {
     185
     186                                        //std::cout << mSickLMSScan.point[j].distance << std::endl;
     187                                }
     188                                if(i == 5)
     189                                {
     190
     191                                        std::cout <<  *(data[i]+j) << std::endl;
     192                                }
    165193            }
    166         }
    167         sumSizes += sizes[i];
    168     }
    169 
     194
     195                }
     196                sumSizes += sizes[i];
     197    }
     198       
     199 //   for(int i = 7; i<9; ++i)
     200        //{
     201
     202 //       if(sizes[i])
     203        //      {
     204 //           mDataFile.seekg(pos[i]); // set the get pointer to the correct place
     205 //           // // then copy the data contained in the binary file
     206 //           remission_8[i] = (uint8_t*) malloc(sizes[i]*sizeof(uint8_t));
     207 //           for (size_t j = 0; j < sizes[i]; ++j)
     208        //              {
     209 //               mDataFile.read(reinterpret_cast<char*>(remission_8[i]+j), sizeof(uint8_t));
     210 //               if(j%500==0)
     211        //                      {
     212 //                   LOG_TRACE("Data : " << (uint16_t) *(remission_8[i] + j));
     213 //               }
     214        //                      if(i == 0)
     215        //                      {
     216
     217        //                              //std::cout << mSickLMSScan.point[j].distance << std::endl;
     218        //                      }
     219        //                      if(i == 7)
     220        //                      {
     221
     222        //                              std::cout << "the 8 bit remission" << *(remission_8[i]+j) << std::endl;
     223        //                      }
     224 //           }
     225
     226        //      }
     227        //      sumSizes += sizes[i];
     228 //   }
     229 //       
    170230
    171231    // verify that the last value is the UTC magic word
    … …  
    181241
    182242        /**********************************/
    183         /*      TODO : Send data !        */
     243
     244
    184245        /**********************************/
    185246    }
    … …  
    198259
    199260
    200     for(int i=0; i<7; ++i){
    201         free(data[i]);
     261    for(int i=0; i<7; ++i)
     262        {
     263                if(sizes[i])
     264                {
     265                 free(data[i]);
     266                }
    202267    }
    203268}
Note: See TracChangeset for help on using the changeset viewer.