iNemo Lite code's interface?

dannym · ‎2012-07-06

Posted on July 06, 2012 at 19:45

I have the iNEMO Lite code for its Extended Kalman filter. I have a board I made with an LSM303DLM and LSM330DL connected to the STM32F4 Discovery board for the IMU.

Is there any example? It looks like it's a library, and I don't know how to call it or calibrate it. Correct me if I'm wrong but I see 3 functions, an init, calc, and deinit function:

void iNEMO_AHRS_Init(iNEMO_SENSORDATA* pSensorData,

iNEMO_EULER_ANGLES* pAngle,

iNEMO_QUAT* pQuat);

void iNEMO_AHRS_Update(iNEMO_SENSORDATA* pSensorData,

iNEMO_EULER_ANGLES* pAngle,

iNEMO_QUAT* pQuat);

I'm not worried about collecting I2C samples, although it'd be nice to see what they sent it for sensor config. I guess my first questions:

1. Is there an example program, with a main(), anywhere? Or did I just not look in the right place? It'd also be good to see what config it gives to the accelerometers.

2. What is the scale of the sensor data? Once I get some I2C sample, I could give the accelerometer as 1=fullscale, I could change it to 1=1G, I could make it 1=1fps/s or =1ms/s. The magnetometer has a #define EMS in gauss, so I'm guessing the incoming vectors should be in gauss. But accel and gyro, what are they?

Hmm there is a line:

#define VAR_AX (0.00096231f) /*!< Accelerometer Variance on X-axis [m/s2] */

So I'm guessing accelerometer units are in m/s^2?

Then:

#define VAR_MX (0.000009f) /*!< Magnetometer Variance on X-axis [(0.1*g)^2] */

So that reinforces the theory that the magnetometer is in gauss (g wouldn't be G-force).

The rate gyro would likely be in fraction of fullscale, deg/s, radians/s, or rotations/s. I found a function ''iNEMO_WrapAround'' which looks to take in a radians argument, but that function isn't even used.

The SENSORDATA construct has a ''ScaleFactor'' and ''m_fOffset'' for all axes, but none of them are used in the code.

3. I see there is a ''m_fDeltaTime'', that gets initialized to 0.01f. So I guess that means 100Hz sample rate is intended there? That's an available sample rate option for the accel/mag/gyro. I guess I answered my own question there.

4. I see the magnetic ellipse parameters are hardcoded in, which isn't valid from application to application. There's a reference to http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/APPLICATION_NOTE/CD00269797.pdf for a calibration procedure, using some PC interface software. How can this be done without that software? Correct me if I'm wrong, but isn't this a matter of rotating it randomly for awhile, away from external magnetic materials, and looking for the highest and lowest magnetic values on the 3 axes?

5. I see some mysterious state vector initialization:

fSV[0] = 0.5375;

fSV[1] = 0.04915;

fSV[2] = -0.0418;

fSV[3] = -0.8408;

for (i=4; i < 7; ++i)

fSV[i] = 0;

What does this do? There's a commented-out version which lists these as all-zero. I don't know the significance of these numbers, if they're something I need to derive from calibration or a mathematical reason for being what they are?

6. What are these accel/mag ''VARIANCE'' figures?:

#define VAR_AX (0.00096231f) /*!< Accelerometer Variance on X-axis [m/s2] */

Again, do they need to be calibrated or just accepted as-is?

7. Is the iNEMO app protocol openly documented? It may be useful to use the iNEMO PC application, and it seems fairly simple to make STM32 code to feed the data out once the filter is working properly. Could just send it through a UART and pick it up with a UART-USB dongle bridge and I see where the software takes in a COM port so it seems simple to set up.

dannym · ‎2012-08-03

Posted on August 03, 2012 at 21:49

Just blogging my discoveries here (which are only ''mostly'' certain):

iNemo Lite contains no main(). It is a library with no documentation on usage, but it's only 3 tasks- structure creation and initialization, the ''do it'' call for every sample frame, and an object deconstructor.

accel is in m/s^2

mag is in gauss

gyro is in RADIANS/sec

m_nDeltaTime is in seconds

There are numerous #define substitutions which obfuscate the code. For example, the creation task iNEMO_fMatCreateZero() is actually a macro substitution for the iNEMO_fMatCreateInit() task. In fact most of the fields inside the input and output data structures are replaced this way, so be prepared for this when searching for text strings.

The iNEMO_SENSORDATA structure ''pExtSensorData''is your hardware sensor input pointer. Note the tasks take in and manipulate this pointer as ''pSensorData''.

Inside iNEMO_SENSORDATA, m_nDeltaTime is in seconds, this is your sample period between calls to iNEMO_AHRS_Update(), you need to set it. There is also m_fScaleFactor[9] and m_fOffset[9], which are NOT used, I believe the code expects the user's sensor-reading code to use them. Note they're just 9-deep arrays with no declared positions for which sensor and which axis. I recommend replacing them with, say, m_fAccelScaleFactor[3], but remember this is in the iNemo library and updates would put your mods in peril.

iNemo_Engine_Lite.c's init task has two ''Options'' for state vector and ''angle initialization'', one is to zero, the other is to give arbitrary values which don't seem to have any significance. I don't know if starting from 0 somehow stalls its adaptation or what, I wish I knew. The default is arbitrary values; I changed it to the zeroes option and it works well enough.

The code requires a GREAT DEAL of heap- about 2K- all the structures are dynamically created via malloc(), I believe this is so it can be dynamically run as as app. But for whatever reason, the code does NOT check that malloc() returns null because it failed to find space. If that happens, your code will act unpredictably and throw a HARD_FAULT trap at some point.

There are 8 structures are created dynamically. These contain small 2-dimensional arrays of up to 7x7 values. The matrix inside is just a single pointer created dynamically, but each row is created separately, so what we actually create is a pointer to a structure containing a pointer to an array of row pointers.

This is actually not unusual for C++ programming. But there are 2 caveats you must take into account here- one, it is NOT currently sufficient to check the return value of the STRUCTURE creation task (iNEMO_fMatCreateZero(), etc), because if creation of any of the array of row pointers or any of the rows themselves fails to malloc(), it will not forward this error along and the overall structure pointer will be valid, but some of its pointers inside may be null.

Second, it's hard to find a null pointer! If an array row fails to allocate, AFAIK the start of the row is 0 (null) but the code will access column 3 as [0+3], which I believe is a legal location, but in the control registers. The Keil IDE is surprisingly unhelpful at this, because pulling up Watch will show you if the overall structure pointer is null, and it could tell you if the single pointer to the array of row pointers to the matrix is null, but it can't display the array of row pointers themselves because it doesn't know the dimensions of the array since it was not determined at compile-time. Thus if any row failed to create, the Watch window cannot display that.

Bottom line being that it's vital to modify the code to check that all malloc() succeeds. If it does not, I see no way to recover, it should bomb and display a debug printf() and while(1) there. You could test to see that no array creation returns null and if so stop and return null for the entire structure, modify AHRS_init to verify structure pointers and return 0 if they're null- however, there's still no way to recover from that. Even if there were a fallback method to get a valid allocation, we don't readily have a way to deconstruction the allocations in the broken structure which has some valid pointers and some invalid ones.

The output is in two formats, the iNEMO_EULER_ANGLES and iNEMO_QUAT. The Euler angles are in radians. The quaternion is just an array and we can only guess what the ordering of the fields are.

The iNemo_Engine_Lite code does not have any provisions for calibrating sensors. As I said, it includes parameters for sensor offset and scale which are not used for iNemo calculations within this code. There's also Variances for the accel and magnetometer, which is a noise parameter which goes into pMat_Ra and pMat_Rm. I don't know if those are an arbitrary matrix initialization or of something which should be measured and recoded.

In iNemo_Engine_Lite.h you need to set the Earth's total mag field for your location. The overall magnitude is easily looked up by the link they handily included in the code and you need to set that.

There's also Ellipsoid radius and center coordinates for the magnetometer, with cartesian XYZ dimensions of each. These are cited as the ''Hard Iron'' calibration where they are used:

fHICx = (ERX * iNEMO_MagX) - ECCX; /* Hard - Iron Calibration X */

The term ''ellipsoid radius and center'' comes from a calibration

http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/APPLICATION_NOTE/CD00269797.pdf

, it is the shape of distortion you get when you thoroughly rotate the entire assembly along with its soft and hard iron errors in the Earth's fixed magnetic field. Ideally the plotted values would create a sphere centered on {0,0,0}, but hard iron (nearby permanent magnets) creates a fixed offset error on each axis to move the center and soft iron (magnetizes only in the presence of an external field) distorts the lines and creates errors in the SCALE on each axis (and also translates some flux from one axis to another, but this isn't represented). So ERX is soft iron SCALE error, ECCX is hard iron OFFSET error.

The important note is that this is redundant with any offset/scale adjustments you might make in your iNEMO_SENSORDATA structure. They're just offset/scale compensations; use one or the other, not both.