Using Sensor Fusion With C++ API

Eric

This discussion was created from comments split from: Building a .exe for a metawear C sensor.

tzimer

thanks! now i can connect to the device successfully, but need help with streaming the fusion sensor data.

i call the function below, but nothing is printed to the command line. what am i doing wrong?

        void stream_quaternions()

        {

            var quaternion = mbl_mw_sensor_fusion_get_data_signal(cppBoard, SensorFusion.Data.QUATERION);

            Fn_IntPtr sensorFusionHandlerDelegate = new Fn_IntPtr(dataPtr =>

            {

                var data = Marshal.PtrToStructure<Data>(dataPtr);

                var sensor_fusion_value = Marshal.PtrToStructure<Quaternion>(data.value);

                System.Diagnostics.Debug.WriteLine(DateTime.Now + " Fusion  " + Marshal.PtrToStructure<Quaternion>(data.value));

                var message = "Fussion " + Marshal.PtrToStructure<Quaternion>(data.value).ToString();

                Console.WriteLine(sensor_fusion_value.x + " " + sensor_fusion_value.y + "" + sensor_fusion_value.z + " " + sensor_fusion_value.w);

            });

            var sensor_fusion_signal = mbl_mw_sensor_fusion_get_data_signal(cppBoard, SensorFusion.Data.QUATERION);

            mbl_mw_datasignal_subscribe(sensor_fusion_signal, sensorFusionHandlerDelegate);

            mbl_mw_sensor_fusion_enable_data(cppBoard, SensorFusion.Data.QUATERION);

            mbl_mw_sensor_fusion_start(cppBoard);

  

        }

tzimer

after placing the Handler out the method, i can finally stream data, BUT, i am getting zero in all the values: x, y, z, w although i am moving the sensor, what am i doing wrong?

        Fn_IntPtr sensorFusionHandlerDelegate = new Fn_IntPtr(dataPtr =>

        {

            var data = Marshal.PtrToStructure<Data>(dataPtr);

            var sensor_fusion_value = Marshal.PtrToStructure<Quaternion>(data.value);

            Console.WriteLine(sensor_fusion_value.x + " " + sensor_fusion_value.y + " " + sensor_fusion_value.z + " " + sensor_fusion_value.w);

        });

        void stream_quaternion()

        {

            var quaternion = mbl_mw_sensor_fusion_get_data_signal(cppBoard, SensorFusion.Data.QUATERION);

            var sensor_fusion_signal = mbl_mw_sensor_fusion_get_data_signal(cppBoard, SensorFusion.Data.QUATERION);

            mbl_mw_datasignal_subscribe(sensor_fusion_signal, sensorFusionHandlerDelegate);

            mbl_mw_sensor_fusion_enable_data(cppBoard, SensorFusion.Data.QUATERION);

            mbl_mw_sensor_fusion_start(cppBoard);

  

        }  

Eric

Did you configure the sensor fusion algorithm before using it?

https://mbientlab.com/cppdocs/0.7.0/sensor_fusion.html

tzimer

Eric, 

i am facing two problems with streaming the data correctly:

1. the sampling rate is not constant, and data received at 64-114 Hz

2. the data is being update only on slow movement of the sensor, when i move the sensor not very slow (but not very fast) the data is not refreshed.

i generated a graph for the quaternions over the time, you can clearly see the when i moved the sensor slow, the data updated, but when i moved it faster the data stayed constant:

http://imgur.com/a/eat4M

 

i followed your link for configuring the sensor fusion: 

the flow is as below:

 mbl_mw_sensor_fusion_set_mode(cppBoard, SensorFusion.Mode.NDOF);

 mbl_mw_sensor_fusion_set_acc_range(cppBoard, SensorFusion.AccRange.AR_16G);

 mbl_mw_sensor_fusion_write_config(cppBoard);      

 sensor_fusion_signal = mbl_mw_sensor_fusion_get_data_signal(cppBoard, SensorFusion.Data.QUATERION);

 mbl_mw_datasignal_subscribe(sensor_fusion_signal, sensorFusionHandlerDelegate);

 mbl_mw_sensor_fusion_enable_data(cppBoard, SensorFusion.Data.QUATERION);

 mbl_mw_sensor_fusion_start(cppBoard);   

the sensorFusionHandlerDelegate :

        Fn_IntPtr sensorFusionHandlerDelegate = new Fn_IntPtr(dataPtr =>

        {

            Data data = Marshal.PtrToStructure<Data>(dataPtr);

            Quaternion sensor_fusion_value = Marshal.PtrToStructure<Quaternion>(data.value);

            DateTime epoch = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Local);         

            Console.WriteLine(epoch.AddMilliseconds(data.epoch) + " " + sensor_fusion_value.ToString());

        });       

am i missing any other configuration?

Eric

Rreduce the max connection interval to 7.5ms.  

http://community.mbientlab.com/discussion/comment/3825/#Comment_3825

tzimer

thanks, that solved the first issue. now the sampling rate is constant.

the second issue, however still take affect.

when moving the sensor slowly, the data is being update, but faster movements causing the sensor to not refresh the data.

please refer to the link below:

http://imgur.com/a/eat4M

the data is constant when the movement is faster than a very slow and gentle movements .

what should solve this?

Eric

I've never see that behavior before.  What do you mean by "faster movements" and do you see this same behavior when using the Android or iOS apps?

tzimer

i mean that i see the data refreshed only on a very gentle movement of the sensor, when moving it "normally", the data stucks and not refreshed.

we don't see this behavior when using the android app. where can i find the source code for this app? it will help me to verify that i'm setting everything correctly. 

Eric

https://github.com/mbientlab/Metawear-SampleAndroidApp

tzimer

thanks Eric, 

i was missing the following configuration:

     mbl_mw_sensor_fusion_set_gyro_range(cppBoard, SensorFusion.GyroRange.GR_2000DPS);

which is not documented in your cpp documentation page:

https://mbientlab.com/cppdocs/0.7.0/sensor_fusion.html

maybe you should add it?

thanks a lot! now i'm able to stream correctly!