Post

2 followers Follow
0
Avatar

Software Integration with SDK Functions for Real Time Application (Syncronization of Lidar, 2D camera and IMU/GPS)

I am attaching an updated C++ program from the example in your SDK package.  I can't find the reference for the function calls I need in the SDK.  The lines to look at are 177-187 and 200-203 which are shown below (i.e., Configuration Mode and Measurement Mode).  If you can guide me to where I can obtain this information that would be great. There are no clear examples of how to set the variables that we need to do.  Do you have the code for the GUI that sets up the callbacks in MT Viewer.  If I can see the setting and calling procedure I believe from the commented code sample I emailed to you that this can be put together in the correct sequence.

Here is what we wan to do:

Configuration Mode:

// Added this part. Also the Filter Profile and the AHS need to be included in the configuration
// For GeneralMag Filter Profile Set FilterProfile with a Filter Profile = 03 should go here but where is the reference for setting this up in the SDK
// For AHS SetOptionFlags should go here but where is the reference for setting this up in the SDK
// Reference: MTi Users Manual p. 23 and MTi Lowlevel Communications p. 15

Measurement Mode:

// Put the GyroBiasEstimation Function Here:
// SetNoRotation should go here with a time delay of 4s (Note: Is it a good idea to have a 4s delay in Measurement Mode). but where is the reference for setting this up in the SDK//
// Reference: MTi Lowlevel Communications p 47 and p 60

Code Segment:

/* Copyright (c) 2003-2016 Xsens Technologies B.V. or subsidiaries worldwide.
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the names of the copyright holders nor the names of their contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

//--------------------------------------------------------------------------------
// Xsens device API example for an MTi / MTx / Mtmk4 device using the C++ API
//
//--------------------------------------------------------------------------------
#include <xsensdeviceapi.h> // The Xsens device API header

#include <iostream>
#include <list>
#include <iomanip>
#include <stdexcept>

#include <xsens/xstime.h>
#include <xsens/xsmutex.h>

#include "conio.h" // for non ANSI _kbhit() and _getch()

//--------------------------------------------------------------------------------
// CallbackHandler is an object derived from XsCallback that can be attached to
// an XsDevice using the XsDevice::setCallbackHandler method.
// Various virtual methods which are automatically called by the XsDevice can be
// overridden (See XsCallback)
// Only the onPostProcess(...) method is overridden here. This method is called
// when a new data packet is available.
// Note that this method will be called from within the thread of the XsDevice so
// proper synchronisation is required.
// It is recommended to keep the implementation of these methods fast; therefore
// the only action here is to copy the packet to a queue where it can be
// retrieved later by the main thread to be displayed. All access to the queue is
// protected by a critical section because multiple threads are accessing it.
//--------------------------------------------------------------------------------
class CallbackHandler : public XsCallback
{
public:
CallbackHandler(size_t maxBufferSize = 5) :
m_maxNumberOfPacketsInBuffer(maxBufferSize),
m_numberOfPacketsInBuffer(0)
{
}

virtual ~CallbackHandler() throw()
{
}

bool packetAvailable() const
{
XsMutexLocker lock(m_mutex);
return m_numberOfPacketsInBuffer > 0;
}

XsDataPacket getNextPacket()
{
assert(packetAvailable());
XsMutexLocker lock(m_mutex);
XsDataPacket oldestPacket(m_packetBuffer.front());
m_packetBuffer.pop_front();
--m_numberOfPacketsInBuffer;
return oldestPacket;
}

protected:
virtual void onLiveDataAvailable(XsDevice*, const XsDataPacket* packet)
{
XsMutexLocker lock(m_mutex);
assert(packet != 0);
while (m_numberOfPacketsInBuffer >= m_maxNumberOfPacketsInBuffer)
{
(void)getNextPacket();
}
m_packetBuffer.push_back(*packet);
++m_numberOfPacketsInBuffer;
assert(m_numberOfPacketsInBuffer <= m_maxNumberOfPacketsInBuffer);
}
private:
mutable XsMutex m_mutex;

size_t m_maxNumberOfPacketsInBuffer;
size_t m_numberOfPacketsInBuffer;
std::list<XsDataPacket> m_packetBuffer;
};

//--------------------------------------------------------------------------------
int main(void)
{
// Create XsControl object
std::cout << "Creating XsControl object..." << std::endl;
XsControl* control = XsControl::construct();
assert(control != 0);

try
{
// Scan for connected devices
std::cout << "Scanning for devices..." << std::endl;
XsPortInfoArray portInfoArray = XsScanner::scanPorts();

// Find an MTi / MTx / MTmk4 device
XsPortInfoArray::const_iterator mtPort = portInfoArray.begin();
while (mtPort != portInfoArray.end() && !mtPort->deviceId().isMt9c() && !mtPort->deviceId().isLegacyMtig() && !mtPort->deviceId().isMtMk4() && !mtPort->deviceId().isFmt_X000()) {++mtPort;}
if (mtPort == portInfoArray.end())
{
throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
}
std::cout << "Found a device with id: " << mtPort->deviceId().toString().toStdString() << " @ port: " << mtPort->portName().toStdString() << ", baudrate: " << mtPort->baudrate() << std::endl;

// Open the port with the detected device
std::cout << "Opening port..." << std::endl;
if (!control->openPort(mtPort->portName().toStdString(), mtPort->baudrate()))
{
throw std::runtime_error("Could not open port. Aborting.");
}

try
{
// Get the device object
XsDevice* device = control->device(mtPort->deviceId());
assert(device != 0);

// Print information about detected MTi / MTx / MTmk4 device
std::cout << "Device: " << device->productCode().toStdString() << " opened." << std::endl;

// Create and attach callback handler to device
CallbackHandler callback;
device->addCallbackHandler(&callback);

// Put the device in configuration mode
std::cout << "Putting device into configuration mode..." << std::endl;
if (!device->gotoConfig()) // Put the device into configuration mode before configuring the device
{
throw std::runtime_error("Could not put device into configuration mode. Aborting.");
}

// Configure the device. Note the differences between MTix and MTmk4
std::cout << "Configuring the device..." << std::endl;
if (device->deviceId().isMt9c() || device->deviceId().isLegacyMtig())
{
XsOutputMode outputMode = XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XOS_OrientationMode_Quaternion; // output orientation data as quaternion
XsDeviceMode deviceMode(100); // make a device mode with update rate: 100 Hz
deviceMode.setModeFlag(outputMode);
deviceMode.setSettingsFlag(outputSettings);

// set the device configuration
if (!device->setDeviceMode(deviceMode))
{
throw std::runtime_error("Could not configure MTmki device. Aborting.");
}
}
else if (device->deviceId().isMtMk4() || mtPort->deviceId().isFmt_X000())
{
XsOutputConfiguration quat(XDI_Quaternion, 0);
// Added this part. Also the Filter Profile and the AHS need to be included in the configuration
// For GeneralMag Filter Profile Set FilterProfile with a Filter Profile = 03 should go here but where is the reference for setting this up in the SDK
// For AHS SetOptionFlags should go here but where is the reference for setting this up in the SDK
// Reference: MTi Users Manual p. 23 and MTi Lowlevel Communications p. 15
XsOutputConfiguration mylatlong(XDI_LatLon, 0);
XsOutputConfiguration myvel(XDI_VelocityXYZ, 0);
XsOutputConfigurationArray configArray;
configArray.push_back(quat);
configArray.push_back(mylatlong);
configArray.push_back(myvel);
if (!device->setOutputConfiguration(configArray))
{

throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
}
else
{
throw std::runtime_error("Unknown device while configuring. Aborting.");
}

// Put the device in measurement mode
std::cout << "Putting device into measurement mode..." << std::endl;
// Put the GyroBiasEstimation Function Here:
// SetNoRotation should go here with a time delay of 4s but where is the reference for setting this up in the SDK
// Reference: MTi Lowlevel COmmunications p 47 and p 60
if (!device->gotoMeasurement())
{
throw std::runtime_error("Could not put device into measurement mode. Aborting.");
}

std::cout << "\nMain loop (press any key to quit)" << std::endl;
std::cout << std::string(79, '-') << std::endl;
while (!_kbhit())
{
if (callback.packetAvailable())
{
// Retrieve a packet
XsDataPacket packet = callback.getNextPacket();

// Get the quaternion data
XsQuaternion quaternion = packet.orientationQuaternion();
std::cout << "\r"
<< "q0:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.w()
<< ",q1:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.x()
<< ",q2:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.y()
<< ",q3:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.z()
;

// Convert packet to euler
XsEuler euler = packet.orientationEuler();
std::cout << ",Roll:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.roll()
<< ",Pitch:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.pitch()
<< ",Yaw:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.yaw()
;

std::cout << std::flush;
}
XsTime::msleep(0);
}
_getch();
std::cout << "\n" << std::string(79, '-') << "\n";
std::cout << std::endl;
}
catch (std::runtime_error const & error)
{
std::cout << error.what() << std::endl;
}
catch (...)
{
std::cout << "An unknown fatal error has occured. Aborting." << std::endl;
}

// Close port
std::cout << "Closing port..." << std::endl;
control->closePort(mtPort->portName().toStdString());
}
catch (std::runtime_error const & error)
{
std::cout << error.what() << std::endl;
}
catch (...)
{
std::cout << "An unknown fatal error has occured. Aborting." << std::endl;
}

// Free XsControl object
std::cout << "Freeing XsControl object..." << std::endl;
control->destruct();

std::cout << "Successful exit." << std::endl;

std::cout << "Press [ENTER] to continue." << std::endl; std::cin.get();

return 0;
}

Rick Tutwiler
1
Avatar

Hi Rick,


You can select a filter profile with XsDevice::setOnboardFilterProfile. Device option flags can be set or cleared through XsDevice::setDeviceOptionFlags. For your particular case I updated the code snippet for configuration:

 

...
XsOutputConfiguration quat(XDI_Quaternion, 0);
XsOutputConfiguration mylatlong(XDI_LatLon, 0);
XsOutputConfiguration myvel(XDI_VelocityXYZ, 0);
XsOutputConfiguration status(XDI_StatusWord, 0);
XsOutputConfigurationArray configArray;
configArray.push_back(quat);
configArray.push_back(mylatlong);
configArray.push_back(myvel);
configArray.push_back(status);
if (!device->setOutputConfiguration(configArray))
{
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}

//Selecting the GeneralMag filter profile
device->setOnboardFilterProfile(3);

//Setting the AHS option
device->setDeviceOptionFlags(XDOF_EnableMagAidedVru, XDOF_None);
...

Please note that I also enabled the status word output. This is for answering your next question.


To start a gyro bias estimation you indeed use the setNoRotation function with the amount of seconds as parameter. This is telling the device to estimate the gyro bias for the next X seconds. The function will not block
for this amount of time though. You have to observe the status byte (or word) output of the device for feedback (That is why I enabled it in the output configuration). The setNoRotation must be called when already in measurement
mode and during the estimation the device must lay still. To illustrate the no rotation procedure I provide the following code snippet. You can place this between the "device->gotoMeasurement" call and the main loop:

...
// Performing a gyro bias estimation through the NoRotation mechanism
XsTime::msleep(100); //Give the device a little bit of time to enter measurement mode
uint16_t biasEstimateDuration = 4;
device->setNoRotation(biasEstimateDuration);
bool noRotationStarted = false;
int64_t biasEstimateStart = XsTime::timeStampNow();
while (true)
{
if (callback.packetAvailable())
{
XsDataPacket packet = callback.getNextPacket();
uint32_t noRotationStatus = packet.status() & XSF_NoRotationMask;

if (!noRotationStarted)
{
if (noRotationStatus == XSF_NoRotationRunningNormally)
{
std::cout << "No rotation started"<< std::endl;
noRotationStarted = true;
}
}
else
{
if (noRotationStatus == 0)
{
std::cout << "No rotation ended successfully"<< std::endl;
break;
}
}

if (noRotationStatus == XSF_NoRotationAborted)
{
std::cout << "No rotation aborted" << std::endl;
break;
}

if (noRotationStatus == XSF_NoRotationSamplesRejected)
{
std::cout << "No rotation ended with rejected samples" << std::endl;
break;
}
}

//Check for timeout
int64_t timeout = ((int64_t)biasEstimateDuration * 1000 + 500);
if ( (XsTime::timeStampNow() - biasEstimateStart) > timeout)
{
if (!noRotationStarted)
std::cout << "No rotation did not start in time" << std::endl;
else
std::cout << "No rotation did not end in time" << std::endl;
break;
}
XsTime::msleep(0);
}
...

I hope this helps you further.

regards,
Tjerk

Tjerk Hofmeijer 1 vote
Comment actions Permalink
0
Avatar

Hi Tjerk,

 

I implemented your suggestions.  I was running SDK 4.3 and the SDK did not recognize the

setDeviceOptionFlags .  I upgraded to sdk 4.6.5. 

I had an original command in the code
//XsControl::setSerialKey(cfg.serialKey.toStdString().c_str());
I checked the documentation and it says that Upgrading from CMT or XDA 4.x
  • XsControl::setSerialKey has been removed
Is there an alternative command to setSerialKey .  I have built the project but when I try 
to execute I get an error when trying to start the IMU. Is there another function that needs
to be executed to setSerialKey 

Thank You for your help and assistance.

Rick



 

Rick Tutwiler 0 votes
Comment actions Permalink
0
Avatar

Hi Rick,

With the removal of the XsControl::setSerialKey we also removed the serial key requirement. So it is not necessary to find an equivalent function for it.

Can you elaborate on the specific error(s) you get?

Thanks,

Tjerk

p.s. We also released the 4.7 SDK just before Christmas. That version is the latest stable release and might be interesting to try. You can download it from here https://www.xsens.com/mt-software-suite/

 

Tjerk Hofmeijer 0 votes
Comment actions Permalink
0
Avatar

Hi Tjerk,

I do not think I can find the device.  I have built with 4.6.5.  When I start my program and  it can not locate the device.  I have downloaded 4.7 and am installing now but,

How do you connect to a specific IMU?  Without the serial key how is the application supposed to know its talking to the correct device?  Is there an enumeration of devices API?

Is there an example I can follow to see what has to be done.

Thanks

Rick

 

Rick Tutwiler 0 votes
Comment actions Permalink
0
Avatar

Hi Rick,

The xda_c_cpp example as packaged with the 4.7 release is adapted to the latest changes in our API. However I don't think it changed much since the 4.3 release.
For device enumeration the XsScanner class is used. The easiest is to use the XsScanner::scanPorts function. This will return an XsPortInfoArray containing an array of all detected devices. The information in this array can then be used with XsControl::openPort to actually open a connection with the device. What kind of connection you are using? If, for
example, you use a non-Xsens USB to serial converter, or a dedicated serial port of your platform, you have to set the ignoreNonXsensDevices parameter of XsScanner::scanPorts to 0. Does the device scan OK in MTManager?

The devices are identified by their unique device ID. If you want to connect to a specific IMU you scan first to see what devices are present and in the resulting XsPortInfoArray you lookup
the entry that matches the device ID. That entry you then use to open the port.

Regards,
Tjerk

Tjerk Hofmeijer 0 votes
Comment actions Permalink

Please sign in to leave a comment.

5 comments