• State Resolved
  • Locked Locked
  • Replies 10 replies
  • Subscribers 32 subscribers
  • Views 650 views
  • Users 0 members are here
Support feedback
Options
Options
Related

This thread has been locked.

If you have a related question, please click the "Ask a related question" button in the top right corner. The newly created question will be automatically linked to this question.

Original question:

CCS/TM4C123GH6PM: sample, accumulate, average function - CCS warning

CCS/TM4C123GH6PM: Unstable results while using "float" in my code

shai kon
Expert 1960 points
Part Number: TM4C123GH6PM


Tool/software: Code Composer Studio

Hello,

I use the following program to read , average and print on screen values from an MPU6050 sensor

////////////////////////////////////////////////////////////////////////////////////////////////////
#include <stdbool.h>
#include <stdint.h>
#include "driverlib/debug.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/pwm.h"
#include "driverlib/rom.h"
#include "driverlib/sysctl.h"
#include "inc/hw_gpio.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include <math.h>
#include <stddef.h> // For using the NULL pointer

#define PWM_FREQUENCY 55

////////////////////////////////////////////////////////////////////////////////////////////////////
// My I2C defines & includes
#include "driverlib/i2c.h"
#include "driverlib/interrupt.h"
#include "inc/hw_i2c.h"
#include "inc/hw_ints.h"
#include "sensorlib/i2cm_drv.h"
#include "sensorlib/mpu6050.h"
#include "sensorlib/hw_mpu6050.h"
#define DEBUG


////////////////////////////////////////////////////////////////////////////////////////////////////
// My UART defines & includes
#include "driverlib/uart.h"
#include "utils/uartstdio.h"

////////////////////////////////////////////////////////////////////////////////////////////////////
#include "definitions_mpu6050.h"

tI2CMInstance g_sI2CInst; // I2C master driver structure. "tI2CMInstance" is defined in the i2cm_drv.h file.
tMPU6050 g_sMPU6050Inst; // MPU6050 sensor driver structure. "tMPU6050" is defined in the mpu6050.h file.
volatile unsigned long g_vui8DataFlag; // Data ready flag
volatile unsigned long g_vui8ErrorFlag; // Error flag

void
ISL29023I2CIntHandler(void)
{
I2CMIntHandler(&g_sI2CInst);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
int main(void)
{
    float fAccel[3], fGyro[3] = {0.0} ;
    uint8_t register_mpu6050_o_pwr_mgmt_1 ; // Used for debug
    uint8_t register_mpu6050_o_who_am_i ; // Used for debug

    uint8_t index = 0 ;
    uint32_t counter = 0 ;
    int32_t i32IntegerPart = 0 ;
    int32_t i32FractionPart = 0 ;


    // Configure the clock
    SysCtlClockSet(SYSCTL_SYSDIV_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);

////////////////////////////////////////////////////////////////////////////////////////////////////
    // My I2C initialization code

    // Enable GPIO peripheral that contains I2C 2
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);

    // Enable I2C module 2
    SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C2);

    // Configure the pin muxing for I2C2 functions on port E4 and E5.
    GPIOPinConfigure(GPIO_PE4_I2C2SCL);
    GPIOPinConfigure(GPIO_PE5_I2C2SDA);

    // Select the I2C function for these pins.
    GPIOPinTypeI2CSCL(GPIO_PORTE_BASE, GPIO_PIN_4);
    GPIOPinTypeI2C(GPIO_PORTE_BASE, GPIO_PIN_5);

    IntMasterEnable();

//    USER_I2CMInit(&g_sI2CInst, I2C2_BASE, INT_I2C2, 0xFF, 0xFF, SysCtlClockGet()); // Set SCL to 100KHz instead of the original 400KHz.
    I2CMInit(&g_sI2CInst, I2C2_BASE, INT_I2C2, 0xFF, 0xFF, SysCtlClockGet());

    SysCtlDelay(SysCtlClockGet() / 3);

    // USER_MPU6050Callback will modify the g_bMPU6050Done variable to true if the functions that use it succeed.

    // Initialize the MPU6050
    g_bMPU6050Done = false;
    MPU6050Init(&g_sMPU6050Inst, &g_sI2CInst, MPU6050_I2C_ADDRESS, USER_MPU6050Callback, 0);
    while(!g_bMPU6050Done) { }
//    SysCtlDelay(SysCtlClockGet() / 3);

    g_bMPU6050Done = false;
    MPU6050Read(&g_sMPU6050Inst, MPU6050_O_WHO_AM_I , &register_mpu6050_o_who_am_i, 1 ,USER_MPU6050Callback , 0);
    while(!g_bMPU6050Done) { }
    SysCtlDelay(SysCtlClockGet() / 3);

    g_bMPU6050Done = false;
    MPU6050Read(&g_sMPU6050Inst, MPU6050_O_PWR_MGMT_1 , &register_mpu6050_o_pwr_mgmt_1, 1 ,USER_MPU6050Callback , 0); // Someone forgot to take the device out of sleep.
    while(!g_bMPU6050Done) { }
    SysCtlDelay(SysCtlClockGet() / 3);

    g_bMPU6050Done = false;

//  Writing 0x01 wakes up the device from sleep and also makes the main clock PLL with X axis gyroscope reference.
    MPU6050Write(&g_sMPU6050Inst, MPU6050_O_PWR_MGMT_1 , 0x01 , 1 , USER_MPU6050Callback , 0);
    while(!g_bMPU6050Done) { }
    SysCtlDelay(SysCtlClockGet() / 3);

    g_bMPU6050Done = false;
    MPU6050Read(&g_sMPU6050Inst, MPU6050_O_PWR_MGMT_1 , &register_mpu6050_o_pwr_mgmt_1 , 1 ,USER_MPU6050Callback , 0);
    while(!g_bMPU6050Done) { }
    SysCtlDelay(SysCtlClockGet() / 3);

    g_bMPU6050Done = false;
    // Configure the MPU6050 for +/- 4 g accelerometer range.
    MPU6050ReadModifyWrite(&g_sMPU6050Inst, MPU6050_O_ACCEL_CONFIG, ~MPU6050_ACCEL_CONFIG_AFS_SEL_M, MPU6050_ACCEL_CONFIG_AFS_SEL_2G, USER_MPU6050Callback, 0);
    while(!g_bMPU6050Done) { }
    SysCtlDelay(SysCtlClockGet() / 3);

////////////////////////////////////////////////////////////////////////////////////////////////////
    // My UART initialization code
    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    GPIOPinConfigure(GPIO_PA0_U0RX);
    GPIOPinConfigure(GPIO_PA1_U0TX);
    GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
    UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);
    UARTStdioConfig(0, 115200, 16000000);
    UARTprintf("The UART Works\n");

    while(1)
    {
        g_bMPU6050Done = false;

        while ( counter < 1000000 )
        {
            counter ++ ;
        }
        counter = 0 ;

        // Get the new accelerometer and gyroscope readings.
//        MPU6050DataAccelGetFloat(&g_sMPU6050Inst, &fAccel[0], &fAccel[1], &fAccel[2]);
//        MPU6050DataGyroGetFloat(&g_sMPU6050Inst, &fGyro[0], &fGyro[1], &fGyro[2]);


        sample_accumulate_average
        (
            &g_sMPU6050Inst ,
            8               , 
            fAccel          ,
            fGyro
        ) ;

        // Convert floating point to an integer and send it via UART

        for ( index = 0 ; index <= 2 ; index ++ )
        {
            i32IntegerPart = (int32_t)fAccel[index];
            i32FractionPart = (int32_t)(fAccel[index] * 1000.0f);
            i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);

            if(i32FractionPart < 0)
            {
                i32FractionPart *= -1;
            }

            if ( index == 0 )
            {
                UARTprintf("Accelerometer X: %3d.%03d\t\n", i32IntegerPart, i32FractionPart);
            }

            if ( index == 1 )
            {
                UARTprintf("Accelerometer Y: %3d.%03d\t\n", i32IntegerPart, i32FractionPart);
            }

            if ( index == 2 )
            {
                UARTprintf("Accelerometer Z: %3d.%03d\t\n", i32IntegerPart, i32FractionPart);
            }
        }

        for ( index = 0 ; index <= 2 ; index ++ )
        {
            i32IntegerPart = (int32_t)fGyro[index];
            i32FractionPart = (int32_t)(fGyro[index] * 1000.0f);
            i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);

            if(i32FractionPart < 0)
            {
                i32FractionPart *= -1;
            }

            if ( index == 0 )
            {
                UARTprintf("Gyro X: %3d.%03d\t\n", i32IntegerPart, i32FractionPart);
            }

            if ( index == 1 )
            {
                UARTprintf("Gyro Y: %3d.%03d\t\n", i32IntegerPart, i32FractionPart);
            }

            if ( index == 2 )
            {
                UARTprintf("Gyro Z: %3d.%03d\t\n\n\n\n\n", i32IntegerPart, i32FractionPart);
            }
        }

        // Clear the accumulated values to prepare for a new cycle.    
        fAccel[0] = 0.0 ;
        fAccel[1] = 0.0 ;
        fAccel[2] = 0.0 ;
        fGyro[0] = 0.0 ;
        fGyro[1] = 0.0 ;
        fGyro[2] = 0.0 ;
    }
}

Here is the code for the "sample_accumulate_average" function

void sample_accumulate_average ( tMPU6050 * g_sMPU6050Inst , int number_of_samples , float *accelerometer_average , float *gyro_average) // the function receives as an argument pointer to arrays of 3 elements
{
    float accelerometer_reading[3], gyro_reading[3] = {0.0};
    int sample_counter ;
    int axis_number ;
    for ( sample_counter = 0 ; sample_counter < number_of_samples ; sample_counter ++ )
    {
        MPU6050DataRead(g_sMPU6050Inst, USER_MPU6050Callback, 0);
        while(!g_bMPU6050Done) { }
        MPU6050DataAccelGetFloat(g_sMPU6050Inst, &accelerometer_reading[0], &accelerometer_reading[1], &accelerometer_reading[2]);
        MPU6050DataGyroGetFloat(g_sMPU6050Inst, &gyro_reading[0], &gyro_reading[1], &gyro_reading[2]);
        for ( axis_number = 0 ; axis_number < 3 ; axis_number ++ )
        {
            accelerometer_average [ axis_number ] = accelerometer_average [ axis_number ] + accelerometer_reading [ axis_number ] ; // accumulate accelerometer readings
            gyro_average [ axis_number ] = gyro_average [ axis_number ] + gyro_reading [ axis_number ] ; // accumulate gyro readings
        }
    }
    for ( axis_number = 0 ; axis_number < 3 ; axis_number ++ )
    {
        accelerometer_average [ axis_number ] = accelerometer_average [ axis_number ] / number_of_samples ;
        gyro_average [ axis_number ] = gyro_average [ axis_number ] / number_of_samples ;
    }
}

The program runs and prints values on the screen. But it's VERY sensitive to the number of samples that I choose to average (8 in the above code).

When I set the value to 1 (no averaging) - correct values appear.

When I increase the value (to induce averaging) - for example to 8. Erroneous values are printed.

I think it has something to do with the fact that I'm using floats and accumulate them - perhaps an overflow of some kind.

What do you think ?

  • 0
    TI__Guru**** 191906 points
    How much is off? Can you show a table of the average value vs. the expected with the number of samples from 1 to 8?
  • 0
    Guru 56143 points

    Hi shai,

    shai kon said:
    float accelerometer_reading[3], gyro_reading[3] = {0.0};

    I find it odd you float the array values, if both are i2c FIFO POP into any C+ array the RAW data would likely be 16 bit integers? Perhaps Charles might explain syntax is/not proper to float array values versus the FPU preforming math on say algorithm variables read from the array index. It would seem the compiler call (float) would have to do several magic tricks upon the array data.

  • 0 in reply to Genatco
    Expert 1960 points
    I use floats because this is what the sensorlib MPU6050DataAccelGetFloat and MPU6050DataGyroGetFloat give me...
  • 0 in reply to Genatco
    Expert 1960 points

    I had some progress debugging this problem.

    It doesn't seem to happen if I add a long enough delay between

    while(!g_bMPU6050Done) { }

    and

    MPU6050DataAccelGetFloat(g_sMPU6050Inst, &accelerometer_reading[0], &accelerometer_reading[1], &accelerometer_reading[2]);

    This is the code with the delay:

    void sample_accumulate_average ( tMPU6050 * g_sMPU6050Inst , int number_of_samples , float *accelerometer_average , float *gyro_average) // the function receives as an argument pointer to arrays of 3 elements
{
    float accelerometer_reading[3], gyro_reading[3] = {0.0};
    int sample_counter ;
    int axis_number ;
    int counter = 0 ;
    for ( sample_counter = 0 ; sample_counter < number_of_samples ; sample_counter ++ )
    {
        MPU6050DataRead(g_sMPU6050Inst, USER_MPU6050Callback, 0);
        while(!g_bMPU6050Done) { }

        while ( counter < 1000 ) // This delay seems to solve the problem
        {
            counter ++ ;
        }
        counter = 0 ;

        MPU6050DataAccelGetFloat(g_sMPU6050Inst, &accelerometer_reading[0], &accelerometer_reading[1], &accelerometer_reading[2]);
        MPU6050DataGyroGetFloat(g_sMPU6050Inst, &gyro_reading[0], &gyro_reading[1], &gyro_reading[2]);
        for ( axis_number = 0 ; axis_number < 3 ; axis_number ++ )
        {
            accelerometer_average [ axis_number ] = accelerometer_average [ axis_number ] + accelerometer_reading [ axis_number ] ; // accumulate accelerometer readings
            gyro_average [ axis_number ] = gyro_average [ axis_number ] + gyro_reading [ axis_number ] ; // accumulate gyro readings
        }
    }
    for ( axis_number = 0 ; axis_number < 3 ; axis_number ++ )
    {
        accelerometer_average [ axis_number ] = accelerometer_average [ axis_number ] / number_of_samples ;
        gyro_average [ axis_number ] = gyro_average [ axis_number ] / number_of_samples ;
    }
}
    Do you understand why ?
    How can it be possible that although the I2C transaction completes - data still isn't ready ?
    I thought that the I2C functions are blocking and don't return till data is ready...
  • 0 in reply to shai kon
    TI__Guru**** 191906 points
    Can you do a UARTprintf on the g_bMPU6050Done before calling MPU6050DataAccelGetFloat to see if the MPU6050 is really done?
  • 0 in reply to Charles Tsai
    Expert 1960 points
    mmm...I can - but what's the motivation behind such a test ??
    if g_bMPU6050Done is false the code must be stuck in the while loop - not printing anything...can you think of a case when it g_bMPU6050Done is false and the code isn't stuck in the while loop ??

    Please look at the example of the ISL29023 sensor inside the TM4C Launchpad's workshop.
    www.cse.iitb.ac.in/.../TM4C123G_LaunchPad_Workshop_Workbook.pdf
    Page 24
    The function ISL29023AppI2CWait is used after every I2C transaction - maybe it's a clue...
    The mpu6050 example inside the Sensorlib PDF however, doesn't use such a function.

    What do you think?
  • 0 in reply to shai kon
    TI__Guru**** 191906 points
    Don't you need to first set the g_bMPU6050Done to false before calling MPU6050DataRead? Otherwise the second time you call MPU6050DataRead the g_bMPU6050Done was always true left from the last time.
  • 0 in reply to Charles Tsai
    Expert 1960 points

    oops.

    Missed that - I'll add the clearing of g_bMPU6050Done and remove the waiting loop to see if it the problem is solved.

  • 0 in reply to shai kon
    Guru 56143 points

    shai kon said:
    It doesn't seem to happen if I add a long enough delay between while(!g_bMPU6050Done) { }



    Is the g_bMPU6050Done defined as Boolean switch? I don't see where though was being set false many times. If actually a variable or register flag, logical (!) may not work well as bitwise &(~g_bMPU6050Done)

    About the float array[] it was never discussed C+ syntax array structures being capable to handle floating integers. For that reason it may be wiser to &variable returned from reads of 6050. That to me is not a call back simply a variable returned from that float query call. One has to enter CCS debug check on float variables to see if the decimal point is actually being added by the FPU, it will show up in mouse hover. If there is no decimal point, the float directive is not working.

    Best of luck!

  • 0 in reply to Genatco
    Expert 1960 points

    As Charles suggested, the problem was that I forgot to  set g_bMPU6050Done to false after doing the call.  I works well now.

    I didn't understand what you meant by:

    "About the float array[] it was never discussed C+ syntax array structures being capable to handle floating integers."

    Are you suggesting C isn't designed to handle arrays of floating point numbers ??

    Please elaborate.