how to get data packets periodically using MSCL

[Shihao-Feng-98]

trafficstars

I want to use mscl to get data packets in every sampling period I use the node.totalPackets() to check if there is data packets in the buffer, and use the node.getDataPackets() to get all data packets in the buffer. Is the latest data packet the last element of the vector?

I try to test the sampling frequency of the 3DMCV5-AHRS in ubuntu18, the code is as follows

// ======== main thread ========
int main()
{
    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        cout << "mlockall failed: %m\n"; 
        return -2;
    }

    cout << "main thread start\n";
    struct sched_param param;
    memset(&param, 0, sizeof(param)); 
    param.sched_priority = 49; 
    int ret = pthread_setschedparam(pthread_self(), SCHED_RR, &param);
    if (ret) 
    {
        cout << "main thread setschedpolicy and setinheritsched failed\n";
        return -1;
    }

    // ======= IMU ==============
    const string com_port = "/dev/ttyACM0"; 
    const uint32_t baud_rate = 115200;
    mscl::Connection connection = mscl::Connection::Serial(com_port, baud_rate);
    mscl::InertialNode node(connection);

    // print imu information
    cout << "Node Information: " << endl;
    cout << "Model Name: " << node.modelName() << endl;
    cout << "Model Number: " << node.modelNumber() << endl;
    cout << "Serial: " << node.serialNumber() << endl;
    cout << "Firmware: " << node.firmwareVersion().str() << endl << endl;

    // set congfig
    uint32_t samples_per_second = 500;
    if(node.features().supportsCategory(mscl::MipTypes::CLASS_AHRS_IMU))
    {
        mscl::MipChannels ahrsImuChs;
        // set channels
        ahrsImuChs.push_back(mscl::MipChannel(mscl::MipTypes::CH_FIELD_SENSOR_SCALED_ACCEL_VEC, 
                                                mscl::SampleRate::Hertz(samples_per_second))); // 0x8004
        ahrsImuChs.push_back(mscl::MipChannel(mscl::MipTypes::CH_FIELD_SENSOR_SCALED_GYRO_VEC, 
                                                mscl::SampleRate::Hertz(samples_per_second))); // 0x8005
        ahrsImuChs.push_back(mscl::MipChannel(mscl::MipTypes::CH_FIELD_SENSOR_EULER_ANGLES, 
                                                mscl::SampleRate::Hertz(samples_per_second))); // 0x800c
        //apply to the node
        node.setActiveChannelFields(mscl::MipTypes::CLASS_AHRS_IMU, ahrsImuChs);
    } 

    // start sampling
    node.enableDataStream(mscl::MipTypes::CLASS_AHRS_IMU);

    // loop 
    double time_since_run = 0.;
    double dt = 0.002;
    uint32_t valid_interation = 0;
    CTimer timer_step, timer_total; // us
    
    timer_total.reset();
    for (int i = 0; i < 10; i++)
    {
        while (time_since_run <= 1.) // check 1s
        {
            timer_step.reset();
            // wait for data packets in buffer
            // timeout 50us 
            while(timer_step.end() < 1000) 
            {
                if (node.totalPackets())
                {
                    node.getDataPackets(); // get all packets
                    valid_interation++;
                    break;
                }
            }

            time_since_run += dt;
            // wait the rest of the time
            while (timer_step.end() < dt*1000*1000); 
        }
        cout << "valid interation: " << valid_interation << endl;
        time_since_run = 0.;
        valid_interation = 0;
    }
    cout << "time total: " << timer_total.end() / 1000. / 1000. << " s\n";
    return 0;
}

and the output is as follows

  main thread start
  Node Information: 
  Model Name: 3DMCV5-AHRS
  Model Number: 6257-4260
  Serial: 6257.132180
  Firmware: 1.1.76
  
  valid interation: 316
  valid interation: 314
  valid interation: 314
  valid interation: 315
  valid interation: 310
  valid interation: 307
  valid interation: 245
  valid interation: 187
  valid interation: 181
  valid interation: 174
  time total: 10.0004 s

It looks like the actual sampling rate is less than the preset 500hz. The same code I test 3DMGX3-AHRS, can reach the preset 500hz. the output is as follows

main thread start
Node Information: 
Model Name: 3DM-GX3-25
Model Number: 6223-4220
Serial: 6223.12504
Firmware: 2.0.6

valid interation: 500
valid interation: 500
valid interation: 500
valid interation: 500
valid interation: 500
valid interation: 500
valid interation: 500
valid interation: 500
valid interation: 500
valid interation: 500
time total: 10.0004 s

I'm not sure if it's a problem with my code or with 3DMCV5-AHRS. Looking forward to some help @msclissa . Thank you.

[msclissa]

Is the latest data packet the last element of the vector? Yes, the last element in the vector returned from InertialNode::getDataPackets() is the packet most recently received/parsed.

I didn't step through your timing logic, but did note that's you're not actually counting packets received, just the iterations that at least one packet is received.

A couple follow-up questions/considerations:

• Is the device connected via USB or serial? USB connections have inherent buffering behavior that can cause multiple packets to be collected in the USB buffer then read into the application at the same time.
• If serial, what is the configured baud rate? Default is 115200.

Assuming the same setup is used between the devices I'm not sure why you would see different results. It's also strange that in the CV5 test the effective sample rate seems to be decreasing.

Could you try rerunning your test and counting the actual packets collected? I would recommend incrementing based on node.getDataPackets().size() instead of the result of node.totalPackets() on the off chance a packet is processed between those two calls.

[Shihao-Feng-98]

Thanks for your reply. I didn't step through your timing logic, but did note that's you're not actually counting packets received, just the iterations that at least one packet is received. My code is run in a real-time thread, and I want to confirm if it has updated data every 2ms (IMU sampling rate is set to 500hz). My test method is to read the cache every 2ms to see if there is data, and read all the data in the cache, only taking the last one (as you say, the last one is the most recent element).

I use the C-series connectivity board to connect the 3DMCV5-AHRS and the connection from the board to the PC is USB.

USB connections have inherent buffering behavior that can cause multiple packets to be collected in the USB buffer then read into the application at the same time. Does this mean there is blocking? Caused me to read multiple packets after multiple sampling cycles? instead of reading one packet per sampling period.

I replace the node.totalPackets() to node.getDataPackets().size(), and got the same result. My main problem is that when I read the data in the buffer according to the sampling frequency set by the IMU, sometimes I can't read the data, or read multiple data at a time. I think it should read at least one freshly sampled data every sampling period. Am I right to think so? If not, how should I read new data every 2ms? Looking forward for your early reply. @msclissa