Sensor fusion V7.0 8g settings

[kayathrim]

Hi all,

We are using Custom board KL46 with the sensor Compass FXOS8700CQ and gyroscope FXAS21002 integrated into it. Sensor fusion V7.0 is running on our custom board. Our application requires +-8g range.

Below are the changes made in existing V7.0 configuration.

FXOS8700_XYZ_DATA_CFG=0x02 (fs=10)

FXOS8700_CTRL_REG1=0x09 (lnoise =0)

FXOS8700_CTRL_REG2=0x02 (mods=10 high resolution) 

We are confused with the data sheet

"High resolution is achieved by setting the lnoise bit in register 0x2A. This improves the resolution (by lowering the noise), but be aware that the dynamic range becomes fixed at ±4 g when this bit is set. This will affect all internal embedded functions (scaling of thresholds,etc.) and reduce noise. Another method for improving the resolution of the data is through oversampling. One of the oversampling schemes of the output data can be activated when CTRL_REG2[mods] = 0b10 which will improve the resolution of the output data without affecting the internal embedded functions or fixing the dynamic range.

When CTRL_REG2[mods] = 0b10, the lowest power is achieved, at the expense of higher noise."

With the same register setting CTRL_REG2[mods] = 0b10  high resolution is mentioned.

We need the correct register setting for 8g. please guide me if my register setting is correct.

Regards,

Kayathri

[michaelestanley]

Kayathri,

Sorry for delayed response.  I'll have an answer for you later today.

MIke

[michaelestanley · Accepted solution]

Kayathri,

I believe your changes are correct.  You also need to change the

#define FXOS8700_COUNTSPERG     8192.0

to

#define FXOS8700_COUNTSPERG     4096.0

in driver_FXOS87000.c.

I need to point out that the serial interface to the SF GUI assumed a +/- 4g range, so you'll see apparent clipping at those values in the GUI.  But the values used for sensor fusion should be correct.

Here is the fully modified driver.  I've bolded lines which changed from the V7.00 baseline.

Regards,

Mike

/*
* Copyright (c) 2015 - 2016, Freescale Semiconductor, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* o Redistributions of source code must retain the above copyright notice, this list
*   of conditions and the following disclaimer.
*
* o 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.
*
* o Neither the name of Freescale Semiconductor, Inc. nor the names of its
*   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 HOLDER 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.
*/

/*! \file driver_FXOS8700.c
    \brief Provides init() and read() functions for the FXOS8700 6-axis accel plus mag
    This version of the driver is designed for +/- 8g operation.
*/

#include "board.h"                      // generated by Kinetis Expert.  Long term - merge sensor_board.h into this file
#include "sensor_fusion.h"              // Sensor fusion structures and types
#include "sensor_io_i2c.h"              // Required for registerreadlist_t / registerwritelist_t declarations
#include "fxos8700.h"                   // describes the FXOS8700 register definition and its bit mask
#include "fxos8700_drv.h"               // Low level IS-SDK prototype driver
#include "drivers.h"                    // Device specific drivers supplied by NXP (can be replaced with user drivers)
#include "status.h"

#define FXOS8700_ACCEL_FIFO_SIZE  32 ///< FXOS8700 (accel), MMA8652, FXLS8952 all have 32 element FIFO
#define FXOS8700_MAG_FIFO_SIZE    1 ///< FXOS8700 (mag), MAG3110 have no FIFO so equivalent to 1 element FIFO

// Command definition to read the WHO_AM_I value.
const registerreadlist_t    FXOS8700_WHO_AM_I_READ[] =
{
    { .readFrom = FXOS8700_WHO_AM_I, .numBytes = 1 }, __END_READ_DATA__
};

// Command definition to read the number of entries in the accel FIFO.
const registerreadlist_t    FXOS8700_F_STATUS_READ[] =
{
    { .readFrom = FXOS8700_STATUS, .numBytes = 1 }, __END_READ_DATA__
};

// Command definition to read the number of entries in the accel FIFO.
registerreadlist_t          FXOS8700_DATA_READ[] =
{
    { .readFrom = FXOS8700_OUT_X_MSB, .numBytes = 6 }, __END_READ_DATA__
};

// Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
const registerwritelist_t   FXOS8700_Initialization[] =
{
    // Command definition to write 0000 0000 = 0x00 to CTRL_REG1 to place FXOS8700 into standby
    // [7-1] = 0000 000
    // [0]: active=0
    { FXOS8700_CTRL_REG1, 0x00, 0x00 }, // write 0100 0000 = 0x40 to F_SETUP to enable FIFO in continuous (circular) mode

    // [7-6]: f_mode[1-0]=01 for FIFO continuous mode
    // [5-0]: f_wmrk[5-0]=000000 for no FIFO watermark
    { FXOS8700_F_SETUP, 0x40, 0x00 },

    // write 0001 1111 = 0x1F to M_CTRL_REG1
    // [7]: m_acal=0: auto calibration disabled
    // [6]: m_rst=0: one-shot magnetic reset disabled
    // [5]: m_ost=0: one-shot magnetic measurement disabled
    // [4-2]: m_os=111=7: maximum oversampling to reduce magnetometer noise
    // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active
    { FXOS8700_M_CTRL_REG1, 0x1F, 0x00 },   // write 0000 0000 = 0x00 to M_CTRL_REG2

    // [7]: reserved
    // [6]: reserved
    // [5]: hyb_autoinc_mode=0 to ensure address wraparound to 0x00 to clear accelerometer FIFO in one read
    // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used
    // [3]: m_maxmin_dis_ths=0
    // [2]: m_maxmin_rst=0
    // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle
    { FXOS8700_M_CTRL_REG2, 0x00, 0x00 },

    // write 0000 0001= 0x01 to XYZ_DATA_CFG register
    // [7]: reserved
    // [6]: reserved
    // [5]: reserved
    // [4]: hpf_out=0
    // [3]: reserved
    // [2]: reserved
    // [1-0]: fs=10 for +/- 8g mode
    { FXOS8700_XYZ_DATA_CFG, 0x02, 0x00 },  // write 0000 0010 = 0x02 to CTRL_REG2 to set MODS bits

    // [7]: st=0: self test disabled
    // [6]: rst=0: reset disabled
    // [5]: unused
    // [4-3]: smods=00
    // [2]: slpe=0: auto sleep disabled
    // [1-0]: mods=10 for high resolution (maximum over sampling)
    { FXOS8700_CTRL_REG2, 0x02, 0x00 },

    // write 00XX X101 = 0x0D to accelerometer control register 1
    // since this is a hybrid sensor with identical accelerometer and magnetometer ODR the value for ACCEL_ODR_HZ is used
    // [7-6]: aslp_rate=00
    // [5-3]: dr=111 for 0.78Hz data rate giving 0x3D
    // [5-3]: dr=110 for 3.125Hz data rate giving 0x35
    // [5-3]: dr=101 for 6.25Hz data rate giving 0x2D
    // [5-3]: dr=100 for 25Hz data rate giving 0x25
    // [5-3]: dr=011 for 50Hz data rate giving 0x1D
    // [5-3]: dr=010 for 100Hz data rate giving 0x15
    // [5-3]: dr=001 for 200Hz data rate giving 0x0D
    // [5-3]: dr=000 for 400Hz data rate giving 0x05
    // [2]: lnoise=0 for normal mode (since we're in 8g mode)
    // [1]: f_read=0 for normal 16 bit reads
    // [0]: active=1 to take the part out of standby and enable sampling
#if (ACCEL_ODR_HZ <= 1)                     // select 0.78Hz ODR
    { FXOS8700_CTRL_REG1, 0x39, 0x00 },
#elif (ACCEL_ODR_HZ <= 3)                   // select 3.125Hz ODR
    { FXOS8700_CTRL_REG1, 0x31, 0x00 },
#elif (ACCEL_ODR_HZ <= 6)                   // select 6.25Hz ODR
    { FXOS8700_CTRL_REG1, 0x29, 0x00 },
#elif (ACCEL_ODR_HZ <= 30)                  // select 25Hz ODR
    { FXOS8700_CTRL_REG1, 0x21, 0x00 },
#elif (ACCEL_ODR_HZ <= 50)                  // select 50Hz ODR
    { FXOS8700_CTRL_REG1, 0x19, 0x00 },
#elif (ACCEL_ODR_HZ <= 100)                 // select 100Hz ODR
    { FXOS8700_CTRL_REG1, 0x11, 0x00 },
#elif (ACCEL_ODR_HZ <= 200)                 // select 200Hz ODR
    { FXOS8700_CTRL_REG1, 0x09, 0x00 },
#else // select 400Hz ODR
    { FXOS8700_CTRL_REG1, 0x01, 0x00 },
#endif
    __END_WRITE_DATA__
};

#define FXOS8700_COUNTSPERG     4096.0
#define FXOS8700_COUNTSPERUT    10

// All sensor drivers and initialization functions have the same prototype
// sensor = pointer to linked list element used by the sensor fusion subsystem to specify required sensors

// sfg = pointer to top level (generally global) data structure for sensor fusion
int8_t FXOS8700_Init(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
    int32_t status;
    uint8_t reg;

    status = Register_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_WHO_AM_I, 1, &reg);

    if (status==SENSOR_ERROR_NONE) {
#if F_USING_ACCEL
       sfg->Accel.iWhoAmI = reg;
       sfg->Accel.iCountsPerg = FXOS8700_COUNTSPERG;
       sfg->Accel.fgPerCount = 1.0F / FXOS8700_COUNTSPERG;
#endif
#if F_USING_MAG
       sfg->Mag.iWhoAmI = reg;
       sfg->Mag.iCountsPeruT = FXOS8700_COUNTSPERUT;
       sfg->Mag.fCountsPeruT = (float) FXOS8700_COUNTSPERUT;
       sfg->Mag.fuTPerCount = 1.0F / FXOS8700_COUNTSPERUT;
#endif
       if (reg != FXOS8700_WHO_AM_I_PROD_VALUE) {
          return SENSOR_ERROR_INIT;  // The whoAmI did not match
       }
    } else {
        // whoAmI will rettain default value of zero
        // return with error
        return status;
    }

    // Configure and start the fxos8700 sensor.  This does multiple register writes
    // (see FXOS8700_Initialization definition above)
    status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_Initialization );
    sensor->isInitialized = F_USING_ACCEL | F_USING_MAG;
#if F_USING_ACCEL
    sfg->Accel.isEnabled = true;
#endif
#if F_USING_MAG
    sfg->Mag.isEnabled = true;
#endif

    return (status);
}

#if F_USING_ACCEL
int8_t FXOS8700_ReadAccData(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
    uint8_t                     I2C_Buffer[6 * FXOS8700_ACCEL_FIFO_SIZE];    // I2C read buffer
    int32_t                     status;         // I2C transaction status
    int8_t                      j;              // scratch
    uint8_t                     sensor_fifo_count;
    int16_t                     sample[3];

    if(!(sensor->isInitialized & F_USING_ACCEL))
    {
       return SENSOR_ERROR_INIT;
    }

    // read the F_STATUS register (mapped to STATUS) and extract number of measurements available (lower 6 bits)
    status =  Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_F_STATUS_READ, I2C_Buffer );
    if (status != SENSOR_ERROR_NONE) return(status);
    else {
#ifdef SIMULATOR_MODE
      sensor_fifo_count = 1;
#else
      sensor_fifo_count = I2C_Buffer[0] & 0x3F;
#endif
      // return if there are no measurements in the sensor FIFO.
      // this will only occur when the FAST_LOOP_HZ equals or exceeds ACCEL_ODR_HZ
      if  (sensor_fifo_count == 0) return(SENSOR_ERROR_READ);
    }

    FXOS8700_DATA_READ[0].readFrom = FXOS8700_OUT_X_MSB;
    FXOS8700_DATA_READ[0].numBytes = 6 * sensor_fifo_count;
    status =  Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_DATA_READ, I2C_Buffer );

    if (status==SENSOR_ERROR_NONE) {
        for (j = 0; j < sensor_fifo_count; j++) {
            sample[CHX] = (I2C_Buffer[6 * j]     << 8) | (I2C_Buffer[6 * j + 1]); // decode X
            sample[CHY] = (I2C_Buffer[6 * j + 2] << 8) | (I2C_Buffer[6 * j + 3]); // decode Y
            sample[CHZ] = (I2C_Buffer[6 * j + 4] << 8) | (I2C_Buffer[6 * j + 5]); // decode Z
            conditionSample(sample);  // truncate negative values to -32767
            // place the 6 bytes read into the 16 bit accelerometer structure
            addToFifo((union FifoSensor*) &(sfg->Accel), ACCEL_FIFO_SIZE, sample);
        }
    }

    return (status);
}
#endif
#if F_USING_MAG
// read FXOS8700 magnetometer over I2C
int8_t FXOS8700_ReadMagData(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
    uint8_t                     I2C_Buffer[6];  // I2C read buffer
    int32_t                     status;         // I2C transaction status
    int16_t                     sample[3];

    if(!(sensor->isInitialized & F_USING_MAG))
    {
        return SENSOR_ERROR_INIT;
    }

    // read the six sequential magnetometer output bytes
    FXOS8700_DATA_READ[0].readFrom = FXOS8700_M_OUT_X_MSB;
    FXOS8700_DATA_READ[0].numBytes = 6;
    status =  Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_DATA_READ, I2C_Buffer );
    if (status==SENSOR_ERROR_NONE) {
        // place the 6 bytes read into the magnetometer structure
        sample[CHX] = (I2C_Buffer[0] << 8) | I2C_Buffer[1];
        sample[CHY] = (I2C_Buffer[2] << 8) | I2C_Buffer[3];
        sample[CHZ] = (I2C_Buffer[4] << 8) | I2C_Buffer[5];
        conditionSample(sample);  // truncate negative values to -32767
        addToFifo((union FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);
    }
    return status;
}
#endif
// This is the composite read function that handles both accel and mag portions of the FXOS8700

// It returns the first failing status flag
int8_t FXOS8700_Read(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
    int8_t  sts1 = 0;
    int8_t  sts2 = 0;
#if F_USING_ACCEL
        sts1 = FXOS8700_ReadAccData(sensor, sfg);
#endif

#if F_USING_MAG
        sts2 = FXOS8700_ReadMagData(sensor, sfg);
#endif

    if (sts1)
        return (sts1);
    else
        return (sts2);
}

// Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
const registerwritelist_t   FXOS8700_FULL_IDLE[] =
{
  // Set ACTIVE = other bits unchanged
  { FXOS8700_CTRL_REG1, 0x00, 0x01 },
    __END_WRITE_DATA__
};

// FXOS8700_Idle places the entire sensor into STANDBY mode (wakeup time = 1/ODR+1ms)
// This driver is all-on or all-off. It does not support mag or accel only.
// If you want that functionality, you can write your own using the initialization
// function in this file as a starting template.  We've chosen not to cover all
// permutations in the interest of simplicity.
int8_t FXOS8700_Idle(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
    int32_t     status;
    if(sensor->isInitialized == (F_USING_ACCEL|F_USING_MAG)) {
        status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_FULL_IDLE );
        sensor->isInitialized = 0;
#if F_USING_ACCEL
        sfg->Accel.isEnabled = false;
#endif
#if F_USING_MAG
        sfg->Mag.isEnabled = false;
#endif
    } else {
      return SENSOR_ERROR_INIT;
    }
    return status;
}

[kayathrim]

Hi Mike,

We have tested the 8g with the changes recommended in the previous post

Before your previous reply, I have made change in this function processAccelData()  in sensor_fusion.c as given below apart from the sensor register setting.

    #define FXOS8700_COUNTSPERG     8192.0

    sfg->Accel.iGs[j] = (int16)(iSum[j] / (int32) sfg->Accel.iFIFOCount);

    sfg->Accel.iGs[j] = sfg->Accel.iGs[j] * 2;  for 8g

The initial observation of roll, pitch & compass with sensor axis -y facing north direction are almost 0.

After the above reply i changed to #define FXOS8700_COUNTSPERG  4096.0  & commented   //sfg->Accel.iGs[j] = sfg->Accel.iGs[j] * 2.

I have observed roll values are not zero with this change #define FXOS8700_COUNTSPERG  4096.0

I am not sure the issue due to magnetic interference or precision calibration

FXOS8700_COUNTSPERG  are used in  fInvertAccelCal() calibration too

    pthisAccel->iGc[i] = (int16_t) (pthisAccel->fGc[i] * pthisAccel->iCountsPerg);

 Why this pthisAccel->iCountsPerg variable is used.

Our application based on the initial position of the sensor (roll pitch & yaw) 

Regards,

Kayathri

[michaelestanley]

Kayathri,

I ran my example again and checked the code.  The changes I gave appear to be correct.  Recall in sensor fusion.h we have:

 float fGs[3];           ///< averaged measurement (g)
 float fGc[3];    ///< averaged precision calibrated measurement (g)
 float fgPerCount;   ///< g per count
 float fCountsPerg;   ///< counts per g
 int16_t iGs[3];    ///< averaged measurement (counts)
 int16_t iGc[3];    ///< averaged precision calibrated measurement (counts)
 int16_t iCountsPerg;   ///< counts per g

You'll notice we have both integeter and floating versions of our sensor readings, as well as averaged calibrated and not.

In function processAccelData, we have:

            sfg->Accel.iGs[j] = (int16)(iSum[j] / (int32) sfg->Accel.iFIFOCount);
            sfg->Accel.fGs[j] = (float)sfg->Accel.iGs[j] * sfg->Accel.fgPerCount;

In function fInvertAccelCal, we have:

        pthisAccel->iGc[i] = (int16_t) (pthisAccel->fGc[i] * pthisAccel->iCountsPerg);

The latter is recomputing the integer number of accelerometer counts from the calibrated floating point accelerometer values.  This is correct.

The fusion library is designed specifically so that the only scale factors we need to enter are done in the driver.  They are used consistently elsewhere.

It IS possible for the magnetic calibration to introduce a rotation.  We calibrate the magnetometer based on the assumption of a constant magnetic vector magnitude, regardless of sensor orientation.  Theoretically,  you could rotate the inverse soft iron matrix and still meet the assumption of constant vector magnitude.  In most cases we've seen, the residual (uncorrected) rotation is modest.

You don't say how far off your roll numbers are from ideal.  Precision inclinometer projects would not include a magnetometer because the gain in accelerometer precision will be more than negatively offset by errors from the magnetic readings.  Applications which violate a constant Acc=1g will benefit from the mag, but you have to realize there are limits to dynamic performance estimates.

I generally advise people to assume they can hit about +/- 5 degrees with magnetic calibration.  If your error is above that, you may have a problem elsewhere.  If less, it may be something you just have to accept.

To prove to yourself that I'm correct, go back to one of the provided example programs and just swap in the driver changes.  You should see readings at 0 and 1g in the sensor fusion gui for a stationary device.  And the roll value will be small.  You can also compare accel+gyro with 9-axis results to convince yourself the errors you are seeing are due to the mag.

Regards,

Mike

[kayathrim]

Dear Mike ,

Sorry for my delayed response.

You were correct. Roll drift is +-10. Yaw value keeps changing from 0 to 360 in same place. The place where i am testing has more magnetic interference. But evaluation board gives good result.

So, We have designed new board with all guidelines followed.

Regards,

Kayathri