8 years, 11 months ago.

SPI and DigitalOut creation order bug ?

Hello,

Can anybody explain why

int main()
{
DigitalOut cs(PTD4);
SPI spi(PTD6,PTD7,PTD5);
MPU9250 imu(spi, cs,PTC3,timer,&usbSerial);
imu.init();

}

is not behaving the a same as :

int main()
{
SPI spi(PTD6,PTD7,PTD5);
DigitalOut cs(PTD4);
MPU9250 imu(spi, cs,PTC3,timer,&usbSerial);

imu.init();
}

The later does not work : communication with magnetometer gives 1st case : AK8963Whoami= 72 ASAX=177 ASAY=176 ASAZ=166 CNTL1= 18

2nd case : AK8963Whoami= 0 ASAX=0 ASAY=0 ASAZ=0 CNTL1= 0

My new MPU9250 library header file with FIFO and interrupt


#ifndef mpu9250_h
#define mpu9250_h
#include "mbed.h"

#include "MPU9250Defs.h"

typedef struct
{
    uint8_t length;
    int32_t timeStamp;
    int32_t period;
    int16_t ax[MPU9250_CACHE_SAMPLE_NUMBER];
    int16_t ay[MPU9250_CACHE_SAMPLE_NUMBER];
    int16_t az[MPU9250_CACHE_SAMPLE_NUMBER];

    int16_t gx[MPU9250_CACHE_SAMPLE_NUMBER];
    int16_t gy[MPU9250_CACHE_SAMPLE_NUMBER];
    int16_t gz[MPU9250_CACHE_SAMPLE_NUMBER];

    int16_t mx[MPU9250_CACHE_SAMPLE_NUMBER];
    int16_t my[MPU9250_CACHE_SAMPLE_NUMBER];
    int16_t mz[MPU9250_CACHE_SAMPLE_NUMBER];


} MPU9250Result;


class MPU9250
{


public:
    //!
    //! \brief MPU9250
    //! \param spi : Spi objet
    //! \param cs : For some reason MUST BE INITIALIZED BEFORE spi
    //! \param interrupt interrupt pin name
    //! \param timer : Reference to time base
    //! \param debug : Serial port for printing / debugging
    //!
    MPU9250(SPI& spi, DigitalOut& cs, PinName interrupt, Timer &timer, Serial *debug=0);
    //!
    //! \brief interruptRaised each time a new sample is ready
    //!
    void interruptRaised();
    //!
    //! \brief interruptRaised each time a new sample is ready
    //!
    void init();
    //!
    //! \brief values of the sensor
    //! \param result
    //!
    void values(MPU9250Result& result);
    //!
    //! \brief readyRead when data are available
    //! \return
    //!
    bool readyRead();
    //!
    //! \brief resetFIFO in case of overflow
    //!
    void resetFIFO();
    // Setters Getters

    uint8_t gyroLPF() const;
    void setGyroLPF(const uint8_t &gyroLPF);

    uint8_t accelLPF() const;
    void setAccelLPF(const uint8_t &accelLPF);

    uint8_t gyroFullScale() const;
    void setGyroFullScale(const uint8_t &gyroFullScale);

    uint8_t accelFullScale() const;
    void setAccelFullScale(const uint8_t &accelFullScale);

    uint8_t fifoPacketSize() const;
    void setFifoPacketSize(const uint8_t &fifoPacketSize);

    uint16_t sampleRate() const;
    void setSampleRate(const uint16_t &sampleRate);

private:
    // Default init for 100Hz
    void select100HzConfig();
    // ----------------------------------------------------------------------------
    //!
    //! \brief dumpCompassConfig
    //!
    void dumpCompassConfig();
    void dumpConfig();
    void dumpFIFOConfig();

    // ----------------------------------------------------------------------------
    // HAL methods

    inline void select()
    {
        //Set CS low to start transmission (interrupts conversion)
        _cs = 0;
    }
    inline void deselect()
    {
        //Set CS high to stop transmission (restarts conversion)
        _cs = 1;
    }

    inline uint8_t   writeByte(uint8_t WriteAddr, uint8_t WriteData )
    {
        unsigned int temp_val;
        select();
        _spi.write(WriteAddr);
        temp_val=_spi.write(WriteData);
        deselect();
        return temp_val;
    }
    inline uint8_t  readByte( uint8_t WriteAddr, uint8_t WriteData )
    {
        return writeByte(WriteAddr | 0x80,WriteData);
    }

    inline void readBytes( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
    {
        unsigned int  i = 0;

        select();
        _spi.write(ReadAddr | 0x80);
        for(i=0; i<Bytes; i++)
            ReadBuf[i] = _spi.write(0x00);

        deselect();

    }
    uint8_t AK8963ReadByte(uint8_t reg);


private :
    InterruptIn _interrupt;
    // CS BEFORE SPI
    DigitalOut& _cs;
    SPI& _spi;

    Timer& _timer;
    Serial *_debug;

    float _compassAdjust[3];

    // Interuption management
    int _sampleNumber;
    bool _readyRead;

    uint16_t _sampleRate;

    uint32_t _firstTimestamp;

    uint8_t _gyroLPF;
    uint8_t _accelLPF;
    uint8_t _gyroFullScale;
    uint8_t _accelFullScale;

    uint8_t _fifoPacketSize;






};
#endif


My new MPU9250 library source file with FIFO and interrupt

#include "MPU9250.h"
#include "DebugPrint.h"



MPU9250::MPU9250(SPI &spi, DigitalOut &cs, PinName interrupt, Timer &timer, Serial *debug):
    _interrupt(interrupt),
    // INIT CS BEFORE SPI
    _cs(cs),
    _spi(spi),
    _timer(timer),
    _debug(debug),
    _sampleNumber(0),
    _readyRead(false),
    _sampleRate(100),
    _firstTimestamp(0),
    _gyroLPF(MPU9250_GYRO_LPF_184),
    _accelLPF(MPU9250_ACCEL_LPF_184),
    _gyroFullScale(MPU9250_GYROFSR_1000),
    _accelFullScale(MPU9250_ACCELFSR_2),
    _fifoPacketSize(20)
{
    _fifoPacketSize=20;
    _sampleNumber=0;
}

void MPU9250::interruptRaised()
{
    if(_sampleNumber==0)
        _firstTimestamp=_timer.read_ms();
    _sampleNumber++;

    if(_sampleNumber == MPU9250_CACHE_SAMPLE_NUMBER)
        _readyRead=true;

}

// Warning FCHOICE in GYRO_LPF config register (reg 27: 0x1B bit [1:0] !!)

//      FCHOICE  DLPF_CFG  Bandwidth(Hz)  Delay(ms)  Noise Density(ug/rtHz)  Rate(kHz)
//       x 1            X          8800          0.064           250             32
//       1 1            X          3600          0.11            250             32
//       0 0            0          250           0.97            250             8
//       0 0            1          184           2.90            250             1
//       0 0            2          92            3.90            250             1
//       0 0            3          41            5.90            250             1
//       0 0            4          20            9.90            250             1
//       0 0            5          10            17.85           250             1
//       0 0            6          5             33.48           250             1
//       0 0            7          3600          0.17            250             8

// ACCEL_FCHOICE  A_DLPF_CFG  Bandwidth(Hz)  Delay(ms)  Noise Density(ug/rtHz)  Rate(kHz)
//       1            X          1130          0.75            250                  4
//       0            0          460           1.94            250                  1
//       0            1          184           5.80            250                  1
//       0            2          92            7.80            250                  1
//       0            3          41            11.80           250                  1
//       0            4          20            19.80           250                  1
//       0            5          10            35.70           250                  1
//       0            6          5             66.96           250                  1
//       0            7          460           1.94            250                  1
void MPU9250::init()
{


    // reset device
    writeByte(MPU9250_PWR_MGMT_1, 0x80); // toggle reset device
    wait_ms(1000);


    // get stable time source; Auto select clock source to be PLL gyroscope reference if ready
    // else use the internal oscillator, bits 2:0 = 001
    writeByte(MPU9250_PWR_MGMT_2, 0x00);
    writeByte(MPU9250_PWR_MGMT_1, 0x01); // Turn on internal clock source
    wait_ms(500);


    select100HzConfig();
    dumpConfig();


    if(_sampleRate <= 100)
        dumpCompassConfig();

    // Configure FIFO to capture accelerometer and gyro data for bias calculation
    dumpFIFOConfig();

    _readyRead=false;
}
void MPU9250::dumpFIFOConfig()
{

    writeByte(MPU9250_USER_CTRL, 0x0C); // Reset FIFO and DMP

    // Configure FIFO to capture accelerometer and gyro data for bias calculation
    writeByte(MPU9250_INT_ENABLE   , 0x01 );
    writeByte(MPU9250_USER_CTRL, 0b01100000); // Enable FIFO

    writeByte(MPU9250_FIFO_EN,   0b01111010); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)


    _interrupt.rise(this,&MPU9250::interruptRaised);
}
void MPU9250::select100HzConfig()
{
    setSampleRate(100);
    setGyroLPF(MPU9250_GYRO_LPF_184);
    setAccelLPF(MPU9250_ACCEL_LPF_184);

    setGyroFullScale(MPU9250_GYROFSR_1000);
    setAccelFullScale(MPU9250_ACCELFSR_2);
}



void MPU9250::dumpConfig()
{
    // divider only available if FS <=1000 Hz
    if(_sampleRate <= 1000)
    {
        uint8_t div=(1000/_sampleRate)-1;
        writeByte(MPU9250_SMPRT_DIV, div);

    }
    writeByte(MPU9250_GYRO_LPF, _gyroLPF);
    writeByte(MPU9250_ACCEL_LPF, _accelLPF);
    // Full scale

    writeByte(MPU9250_GYRO_CONFIG, _gyroFullScale /* | MPU9250_GYRO_LPF_8800*/);
    writeByte(MPU9250_ACCEL_CONFIG,_accelFullScale);

}

void MPU9250::dumpCompassConfig()
{
    debugPrint1("\n\nAK8963Whoami=%3d\n",AK8963ReadByte(MPU9250_WHO_AM_I_AK8963));
    uint8_t asa[3];
    asa[0]=AK8963ReadByte(MPU9250_AK8963_ASAX);
    asa[1]=AK8963ReadByte(MPU9250_AK8963_ASAY);
    asa[2]=AK8963ReadByte(MPU9250_AK8963_ASAZ);

    debugPrint1("ASAX=%3d\n",asa[0]);
    debugPrint1("ASAY=%3d\n",asa[1]);
    debugPrint1("ASAZ=%3d\n",asa[2]);

    debugPrint1("CNTL1=%3d\n",AK8963ReadByte(MPU9250_AK8963_CNTL1));

    _compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
    _compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
    _compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;

    // Reset magnometer
    writeByte(MPU9250_I2C_SLV0_ADDR,MPU9250_AK8963_ADDRESS);
    writeByte(MPU9250_I2C_SLV0_REG,MPU9250_AK8963_CNTL2);
    writeByte(MPU9250_I2C_SLV0_DO,0x01);
    writeByte(MPU9250_I2C_SLV0_CTRL,0x81);

    wait_ms(1500);

    // Set Mode and Sample Rate :
    writeByte(MPU9250_I2C_SLV0_ADDR,MPU9250_AK8963_ADDRESS);
    writeByte(MPU9250_I2C_SLV0_REG,MPU9250_AK8963_CNTL1);
    writeByte(MPU9250_I2C_SLV0_DO,0x12);
    writeByte(MPU9250_I2C_SLV0_CTRL,0x81); // Enable i2c and write 1 bytes

    writeByte(MPU9250_I2C_SLV1_ADDR,MPU9250_AK8963_ADDRESS|MPU9250_READ_FLAG); // Magneto address
    writeByte(MPU9250_I2C_SLV1_REG,MPU9250_AK8963_ST1); // Address of the first data register
    writeByte(MPU9250_I2C_SLV1_CTRL,0x88); // Enable reading and read 8 bytes
}



void MPU9250::resetFIFO()
{
    writeByte(MPU9250_INT_ENABLE,0x00);
    writeByte(MPU9250_FIFO_EN,   0x00);
    writeByte( MPU9250_USER_CTRL, 0x04);
    writeByte( MPU9250_USER_CTRL, 0x60);
    writeByte(MPU9250_FIFO_EN,   0b01111010);
    writeByte(MPU9250_INT_ENABLE,0x01);

    _sampleNumber=0;
    _readyRead=false;
}

void MPU9250::values(MPU9250Result &results)


{
    uint8_t data[512]; // data array to hold accelerometer and gyro x, y, z, data
    uint16_t ii, fifo_count;

    readBytes(MPU9250_FIFO_COUNT_H,data,2); // read FIFO sample count
    fifo_count = ((uint16_t)data[0] << 8) | data[1];

    if(fifo_count==512)
    {
        debugPrint("overflow\n");
        // Reset Fifo
        resetFIFO();
        return;
    }

    if(_readyRead)
    {
        _readyRead=false;
        _sampleNumber=0;

        readBytes(MPU9250_FIFO_R_W,  data,fifo_count);

        results.timeStamp=_firstTimestamp;
        results.period=100;
        results.length=MPU9250_CACHE_SAMPLE_NUMBER;
        for (ii = 0; ii < MPU9250_CACHE_SAMPLE_NUMBER; ii++)
        {
            int offset=ii*_fifoPacketSize;
            results.ax[ii] = (int16_t) (((int16_t)data[0+offset] << 8) | data[1+offset] ) ; // Form signed 16-bit integer for each sample in FIFO
            results.ay[ii] = (int16_t) (((int16_t)data[2+offset] << 8) | data[3+offset] ) ;
            results.az[ii] = (int16_t) (((int16_t)data[4+offset] << 8) | data[5+offset] ) ;

            results.gx[ii] = (int16_t) (((int16_t)data[6+offset] << 8) | data[7+offset] ) ;
            results.gy[ii] = (int16_t) (((int16_t)data[8+offset] << 8) | data[9+offset] ) ;
            results.gz[ii] = (int16_t) (((int16_t)data[10+offset] << 8) | data[11+offset]) ;

            if(_fifoPacketSize==20)
            {
                results.mx[ii] = (int16_t) (((int16_t)data[13+offset] << 8) | data[14+offset] ) ;
                results.my[ii] = (int16_t) (((int16_t)data[15+offset] << 8) | data[16+offset] ) ;
                results.mz[ii] = (int16_t) (((int16_t)data[17+offset] << 8) | data[18+offset]) ;
            }
            else
            {
                results.mx[ii] = 0;
                results.my[ii] = 0;
                results.mz[ii] = 0;
            }

        }

    }
}

uint8_t MPU9250::AK8963ReadByte(uint8_t reg)
{
    uint8_t response;
    writeByte(MPU9250_I2C_SLV0_ADDR,MPU9250_AK8963_ADDRESS|MPU9250_READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
    writeByte(MPU9250_I2C_SLV0_REG, reg); //I2C slave 0 register address from where to begin data transfer
    writeByte(MPU9250_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
    wait_ms(100);
    response=writeByte(MPU9250_EXT_SENS_DATA_00|MPU9250_READ_FLAG, 0x00);    //Read I2C
    return response;
}


uint16_t MPU9250::sampleRate() const
{
    return _sampleRate;
}

void MPU9250::setSampleRate(const uint16_t &sampleRate)
{
    _sampleRate = sampleRate;
}

uint8_t MPU9250::fifoPacketSize() const
{
    return _fifoPacketSize;
}

void MPU9250::setFifoPacketSize(const uint8_t &fifoPacketSize)
{
    _fifoPacketSize = fifoPacketSize;
}

uint8_t MPU9250::accelFullScale() const
{
    return _accelFullScale;
}

void MPU9250::setAccelFullScale(const uint8_t &accelFullScale)
{
    _accelFullScale = accelFullScale;
}

uint8_t MPU9250::gyroFullScale() const
{
    return _gyroFullScale;
}

void MPU9250::setGyroFullScale(const uint8_t &gyroFullScale)
{
    _gyroFullScale = gyroFullScale;
}

uint8_t MPU9250::accelLPF() const
{
    return _accelLPF;
}

void MPU9250::setAccelLPF(const uint8_t &accelLPF)
{
    _accelLPF = accelLPF;
}

uint8_t MPU9250::gyroLPF() const
{
    return _gyroLPF;
}

void MPU9250::setGyroLPF(const uint8_t &gyroLPF)
{
    _gyroLPF = gyroLPF;
}


bool MPU9250::readyRead()
{
    return _readyRead;
}

Thanks for your clue because I don't have any ...

BTW: I am using a K22F board connected to a Invensense MPU9250

IS this global scope? same translation unit? What does "does not work" mean ?

posted by Martin Kojtal 18 Mar 2015

1 Answer

8 years, 11 months ago.

I suspect you have found a bug with DigitalOut's initialisation for that platform.

For the first "working" case you initialize DigitalOut on an apparently unrelated pin first. You then Initialize the spi object which will configure the pinmux correctly for use by the spi module.
This overrides any misconfiguration by DigitalOut.

In the second (faulty) example you initialze the spi object, which at that time would work just fine, but....
When you initialize the DigitalOut the pinmux for one of the spi pins gets altered when it shouldn't.

I don't have a K22F to do any testing with, but Martin does so perhaps he can have a quick look.

If you have the time, have a look at the datasheet for your device and try doing a printf of the involved pinmux registers for both a working and failing example.

I suspect that you won't even need the rest of your code, just initialize the spi and digital out, then in main print out your pinmux registers.

+1 for the printing out the pinmux. that can be what goes wrong there. Althought it is using KSDK and I havent seen any issues regarding mux, but might be wrong..

posted by Martin Kojtal 18 Mar 2015

Hi Martin, Any chance you can do a quick test with something like (untested and uncompiled)

#include "mbed.h"
#include "spi.h"
#define PinMux "Insert PinMux Register Location Here"
int main() {
    DigitalOut cs(PTD4);
    SPI spi(PTD6,PTD7,PTD5);
    printf("Working pinmux: %x",PinMux);
}

and

#include "mbed.h"
#include "spi.h"
#define PinMux "Insert PinMux Register Location Here"
int main() {
    SPI spi(PTD6,PTD7,PTD5);
    DigitalOut cs(PTD4);
    printf("Working pinmux: %x",PinMux);
}

I would be interested to see the results :)

posted by David Godfrey 18 Mar 2015

Thanks for your help,

I am not sure I did what you suggested here is what I have done

#include "mbed.h"
#include "MPU9250.h"

#define PinMux PORTD_PCR4

int main() {

    DigitalOut cs(PTD4);
    SPI spi(PTD6,PTD7,PTD5);



    Serial usbSerial(USBTX, USBRX);
    Timer timer;
    usbSerial.baud(115200);

    MPU9250 imu(spi, cs,PTC3,timer,&usbSerial);
    imu.init();

    printf("Pinmux: %x\n",PinMux);

    while(1)
    {}


}

the result is

AK8963Whoami=  0
ASAX=  0
ASAY=  0
ASAZ=  0
CNTL1=  0
Pinmux: 100


AK8963Whoami= 72
ASAX=177
ASAY=176
ASAZ=166
CNTL1= 18
Pinmux: 100

Is it the test you wanted me to do ???

posted by Jacques Charreyron 19 Mar 2015

Hi Jacques,

You almost have it, my code sample was written without checking any datasheets so I didn't realise that each pin has a 32bit Pin Control Register (PCR).

What you actually need to print out is the PCR for each pin involved in the SPI instance. For example

#include "mbed.h"
#include "spi.h"
#define PinMux "Insert PinMux Register Location Here"
int main() {
    SPI spi(PTD6,PTD7,PTD5);
    DigitalOut cs(PTD4);

    printf("Pin Control Register PORTD_PCR5: 0x%8x   MUX Bits: 0x%1x\r\n", PORTD_PCR5, ((PORTD_PCR5>>8)&0x07));
    printf("Pin Control Register PORTD_PCR6: 0x%8x   MUX Bits: 0x%1x\r\n", PORTD_PCR6, ((PORTD_PCR6>>8)&0x07));
    printf("Pin Control Register PORTD_PCR7: 0x%8x   MUX Bits: 0x%1x\r\n", PORTD_PCR7, ((PORTD_PCR7>>8)&0x07));
}
posted by David Godfrey 19 Mar 2015

David,

Thanks for your help, Here are the results :

AK8963Whoami= 72
ASAX=177
ASAY=176
ASAZ=166
CNTL1= 18
Pin Control Register PORTD_PCR4: 0x     100   MUX Bits: 0x1
Pin Control Register PORTD_PCR5: 0x     700   MUX Bits: 0x7
Pin Control Register PORTD_PCR6: 0x     700   MUX Bits: 0x7
Pin Control Register PORTD_PCR7: 0x     700   MUX Bits: 0x7


AK8963Whoami=  0
ASAX=  0
ASAY=  0
ASAZ=  0
CNTL1=  0
Pin Control Register PORTD_PCR4: 0x     100   MUX Bits: 0x1
Pin Control Register PORTD_PCR5: 0x     700   MUX Bits: 0x7
Pin Control Register PORTD_PCR6: 0x     700   MUX Bits: 0x7
Pin Control Register PORTD_PCR7: 0x     700   MUX Bits: 0x7

I am afraid it won't help ...

posted by Jacques Charreyron 20 Mar 2015

Hmm, I am not going to be of much more help here as I don't have one of those devices so I can't actually test any code. Martin, do you have any thoughts or the time to try and debug this?.

posted by David Godfrey 20 Mar 2015