Important changes to forums and questions
All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.
9 years, 8 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
1 Answer
9 years, 8 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 18 Mar 2015Hi 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 18 Mar 2015Thanks 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 19 Mar 2015Hi 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));
}
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 20 Mar 2015
IS this global scope? same translation unit? What does "does not work" mean ?
posted by Martin Kojtal 18 Mar 2015