time

    m_dhd_thread = thread([this]{
        do
        {
//            while (OpenDevices())
//            {
//                if (m_thread_kill)
//                {
//                    break;
//                }
//                m_timer_sleep.Sleep(3000);
//            }
            printf("sleep test\n");
            int policy;
            struct sched_param param;
            param.sched_priority = 99;
            sched_setscheduler(0, SCHED_RR, &param);
            pthread_setschedparam(pthread_self(), SCHED_RR, &param);

            policy =  sched_getscheduler(0);
            param.sched_priority = 0;
            pthread_getschedparam(pthread_self(),&policy,&param);
            if(policy == SCHED_OTHER)
              printf("SCHED_OTHER 1\n");
            if(policy == SCHED_RR)
            printf("SCHED_RR 1 \n");
            if(policy==SCHED_FIFO)
              printf("SCHED_FIFO 1\n");
            policy = SCHED_RR;
            printf("sched_priority %d\n", param.sched_priority);

            int64_t max_duration = 0;
            int64_t min_duration = 2000;
            int pack_count = 20000;
            int count = pack_count;
            bool first = true;
            int sleetime = 1000000;
            int64_t timebgein = GetNowTimeStamp();
            while (true)
            {
                if(count == pack_count)
                {
                    printf("begin %s \n", GetNowTime().c_str());                    
                }
                int64_t time1 = GetNowTimeStamp();
                if (m_thread_kill)
                {
                    break;
                }
#if 1
                //CheckReady();
                //CheckForceState();
                //GetOmegaData();
               // Gravity();
#if 1
               // Viscosity();
#else
                //Viscosity1();
#endif
#if USE_GET_FORCE_TORQUE
                GetForceAndTorque();
#endif
#endif
                // wenchuan send serial data to jingsong
                // send begin
                unsigned char txbuf[255] = {0};
                unsigned char tx_chksum = 0;
//                txbuf[0] = 0x77;
//                txbuf[1] = 0x88;
//                txbuf[2] = 0x20;
//                txbuf[3] = 0x10;
//                txbuf[4] = 0x2F;
//                if(CSerialControl::getInstance()->isOpen())
//                {
//                    bool sc = CSerialControl::getInstance()->SendComData(txbuf, 5);
//                    //printf("SendComData   %s\n", GetNowTime().c_str());
//                    if(!sc)
//                    {
//                        printf("guide error\n");
//                    }
//                }
                txbuf[0] = 0x55;
                txbuf[1] = 0x66;
                txbuf[2] = 0x20;
                txbuf[3] = 0x6B;
                txbuf[4] = 0x6B;
                txbuf[5] = 0x6B;
                txbuf[6] = 0x1C;
                if(CSerialControl::getInstance()->isOpen())
                {
                    bool suc = CSerialControl::getInstance()->SendComData(txbuf, 7);
                    if(!suc)
                    {
                        printf("tool error\n");
                    }
                }
                //todo
                // send end

                if(first)
                {
                    first = false;
                    sleetime = 2000000;
                }
                else if(!first)
                {
                    sleetime = 2000000;
                }
                int64_t spend_time = GetNowTimeStamp() - time1;
                if(spend_time > sleetime)
                {
                    printf("spend_time: %lld us\n", spend_time/1000);
                }
                std::this_thread::sleep_for(std::chrono::nanoseconds(sleetime-spend_time));

                int64_t duration = (GetNowTimeStamp() - time1) / 1000;
                if(duration > max_duration)
                {
                    max_duration = duration;
                }
                if(duration < min_duration)
                {
                    min_duration = duration;
                }
                if(--count==0)
                {
                    count = pack_count;
                    printf("end   %s\n", GetNowTime().c_str());
                    printf("max_duration: %lld us\n", max_duration);
                    printf("min_duration: %lld us\n", min_duration);
                    printf("%d pack data send completed\n", pack_count);
                    int64_t distance = GetNowTimeStamp() - timebgein;
                    printf("distance %lld ms\n", distance/1000000);
                    break;
                }
            }
        } while (false);

    });