APM 2.6 and MPU6000 gyro - any custom firmware out there for this?

[JP B]

Hi,

Just wondering if anyone in the community has developed their own test/library code for utilising the onboard MPU6000 gyro?  

Currently developing my own custom rover firmware and am now looking at obtaining gyro information from the MPU6000, hoping there may be a sketch out there before I go hacking the standard AP_InertialSensor library.

Many thanks.

[Michael Shimniok]

On 11/05/2013 05:05 AM, JP B wrote:
> Just wondering if anyone in the community has developed their own
> test/library code for utilising the onboard MPU6000 gyro?

You mean reading values off of it? I know writing firmware to run inside
it was a no-go a couple years ago when it first came around.

> Currently developing my own custom rover firmware and am now looking
> at obtaining gyro information from the MPU6000, hoping there may be a
> sketch out there before I go hacking the standard AP_InertialSensor
> library.

I quickly found two libraries on mbed.org which should port to Arduino
fairly easily. The best documented is the first.

http://mbed.org/users/Sissors/code/MPU6050/docs/5c63e20c50f3/classMPU6050.html
http://mbed.org/users/garfieldsg/code/MPU6050/

Michael

[JP B]

Hi,

After much googling and reserach today I found the beginnings of a sketch for getting values from the APM onboard MPU6000 gyro.  I unashamedly took it from this post:

http://forum.arduino.cc/index.php?PHPSESSID=6mia4mslt5dalc1fi4ad8rl2g1&topic=125623.0

It is interesting as it uses SPI - most examples I have seen use the more commong I2C interface for the MPU6000.  I have modified from the original code to set the chip select pin to 53 as per APM standard in AP_InertialSensor_MPU6000.h (and removed the two other chip select pins the original author had declared and used, as I didn't see the need).

This is very very much a work in progress for me - what is below is a very first draft to see if communication is established.

Currently, as far as my understanding goes, the sketch is returning acceleration axis values.  I believe the gyro axis values have yet to be picked up and serial printed for analysis.  

I also do not understand why the original author uses time in calculations in function GyroDeg.  I am also not sure of the definition of:

#define ToD(x) (x/131)

#define ToG(x) (x*9.80665/16384)

#define Aoffset 0.8

If anyone cares to contribute to this sketch to improve that would be very much welcomed.  I shall return tomorrow to understand and progress it - for tonight I shall sleep easy with a working compass I achieved today :)

#include <SPI.h>

#include <math.h>

#define ToD(x) (x/131)

#define ToG(x) (x*9.80665/16384)

#define xAxis 0

#define yAxis 1

#define zAxis 2

#define Aoffset 0.8

int time=0;

int time_old=0;

const int ChipSelPin1 = 53;

float angle=0;

float angleX=0;

float angleY=0;

float angleZ=0;

void setup() {

  Serial.begin(9600);  

  //As per APM standard code, stop the barometer from holding the SPI bus

  pinMode(40, OUTPUT);

  digitalWrite(40, HIGH);

  SPI.begin();  

  SPI.setClockDivider(SPI_CLOCK_DIV16); 

  SPI.setBitOrder(MSBFIRST); 

  SPI.setDataMode(SPI_MODE0);

  delay(100);

  

  pinMode(ChipSelPin1, OUTPUT);

  ConfigureMPU6000();  // configure chip

}

void loop()

{

  Serial.print("Acc X ");

  Serial.print(AcceX(ChipSelPin1));

  Serial.print("   ");

  Serial.print("Acc Y ");

  Serial.print(AcceY(ChipSelPin1));

  Serial.print("   ");

  Serial.print("Acc Z ");  

  Serial.println(AcceZ(ChipSelPin1));  

}

void SPIwrite(byte reg, byte data, int ChipSelPin) {

  uint8_t dump;

  digitalWrite(ChipSelPin,LOW);

  dump=SPI.transfer(reg);

  dump=SPI.transfer(data);

  digitalWrite(ChipSelPin,HIGH);

}

uint8_t SPIread(byte reg,int ChipSelPin) {

  uint8_t dump;

  uint8_t return_value;

  uint8_t addr=reg|0x80;

  digitalWrite(ChipSelPin,LOW);

  dump=SPI.transfer(addr);

  return_value=SPI.transfer(0x00);

  digitalWrite(ChipSelPin,HIGH);

  return(return_value);

}

int AcceX(int ChipSelPin) {

  uint8_t AcceX_H=SPIread(0x3B,ChipSelPin);

  uint8_t AcceX_L=SPIread(0x3C,ChipSelPin);

  int16_t AcceX=AcceX_H<<8|AcceX_L;

  return(AcceX);

}

int AcceY(int ChipSelPin) {

  uint8_t AcceY_H=SPIread(0x3D,ChipSelPin);

  uint8_t AcceY_L=SPIread(0x3E,ChipSelPin);

  int16_t AcceY=AcceY_H<<8|AcceY_L;

  return(AcceY);

}

int AcceZ(int ChipSelPin) {

  uint8_t AcceZ_H=SPIread(0x3F,ChipSelPin);

  uint8_t AcceZ_L=SPIread(0x40,ChipSelPin);

  int16_t AcceZ=AcceZ_H<<8|AcceZ_L;

  return(AcceZ);

}

int GyroX(int ChipSelPin) {

  uint8_t GyroX_H=SPIread(0x43,ChipSelPin);

  uint8_t GyroX_L=SPIread(0x44,ChipSelPin);

  int16_t GyroX=GyroX_H<<8|GyroX_L;

  return(GyroX);

}

int GyroY(int ChipSelPin) {

  uint8_t GyroY_H=SPIread(0x45,ChipSelPin);

  uint8_t GyroY_L=SPIread(0x46,ChipSelPin);

  int16_t GyroY=GyroY_H<<8|GyroY_L;

  return(GyroY);

}

int GyroZ(int ChipSelPin) {

  uint8_t GyroZ_H=SPIread(0x47,ChipSelPin);

  uint8_t GyroZ_L=SPIread(0x48,ChipSelPin);

  int16_t GyroZ=GyroZ_H<<8|GyroZ_L;

  return(GyroZ);

}

//--- Function to obtain angles based on accelerometer readings ---//

float AcceDeg(int ChipSelPin,int AxisSelect) {

  float Ax=ToG(AcceX(ChipSelPin));

  float Ay=ToG(AcceY(ChipSelPin));

  float Az=ToG(AcceZ(ChipSelPin));

  float ADegX=((atan(Ax/(sqrt((Ay*Ay)+(Az*Az)))))/PI)*180;

  float ADegY=((atan(Ay/(sqrt((Ax*Ax)+(Az*Az)))))/PI)*180;

  float ADegZ=((atan((sqrt((Ax*Ax)+(Ay*Ay)))/Az))/PI)*180;

  switch (AxisSelect)

  {

    case 0:

    return ADegX;

    break;

    case 1:

    return ADegY;

    break;

    case 2:

    return ADegZ;

    break;

  }

}

//--- Function to obtain angles based on gyroscope readings ---//

float GyroDeg(int ChipSelPin, int AxisSelect) {

  time_old=time;

  time=millis();

  float dt=time-time_old;

  if (dt>=1000)

  {

    dt=0;

  }

  float Gx=ToD(GyroX(ChipSelPin));

  if (Gx>0 && Gx<1.4)

  {

    Gx=0;

  }

  float Gy=ToD(GyroY(ChipSelPin));

  float Gz=ToD(GyroZ(ChipSelPin));

  angleX+=Gx*(dt/1000);

  angleY+=Gy*(dt/1000);

  angleZ+=Gz*(dt/1000);

  switch (AxisSelect)

  {

    case 0:

    return angleX;

    break;

    case 1:

    return angleY;

    break;

    case 2:

    return angleZ;

    break;

  }

}

void ConfigureMPU6000()

{

  // DEVICE_RESET @ PWR_MGMT_1, reset device

  SPIwrite(0x6B,0x80,ChipSelPin1);

  delay(150);

  // TEMP_DIS @ PWR_MGMT_1, wake device and select GyroZ clock

  SPIwrite(0x6B,0x03,ChipSelPin1);

  delay(150);

  // I2C_IF_DIS @ USER_CTRL, disable I2C interface

  SPIwrite(0x6A,0x10,ChipSelPin1);

  delay(150);

  // SMPRT_DIV @ SMPRT_DIV, sample rate at 1000Hz

  SPIwrite(0x19,0x00,ChipSelPin1);

  delay(150);

  // DLPF_CFG @ CONFIG, digital low pass filter at 42Hz

  SPIwrite(0x1A,0x03,ChipSelPin1);

  delay(150);

  // FS_SEL @ GYRO_CONFIG, gyro scale at 250dps

  SPIwrite(0x1B,0x00,ChipSelPin1);

  delay(150);

  // AFS_SEL @ ACCEL_CONFIG, accel scale at 2g (1g=8192)

  SPIwrite(0x1C,0x00,ChipSelPin1);

  delay(150);

}

[Agniva Sengupta]

This code works brilliant... thanks for saving a life. :)

[JP B]

Hi Agniva,

No problem, you are welcome.  If you come up with any improvements please post back here, could be helpful to all.

Thanks.

[Casper]

Op donderdag 19 december 2013 10:43:10 UTC+1 schreef JP B:

Hi Agniva,

No problem, you are welcome.  If you come up with any improvements please post back here, could be helpful to all.

Thanks.

Dear JP,

I am very sorry to bring up this 3 year old forum again.

But I found your project to write code to the APM board very intresting.

If any, could you give me some more code that you wrote for this board using Arduino software? (without using the standard APM librarys)

Thank you in advance!

Regards,

Casper.

 

[Cao Hung]

Hi!!! I using MPU6050 with stm32f1 // You can halp me about Lib MPU6050!!! Thanks

Vào 04:20:44 UTC+7 Thứ Tư, ngày 06 tháng 11 năm 2013, Michael Shimniok đã viết: