I2C R/W Test. Debug code posted here.

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

Nov-12-2011 UPDATE: Alex's V1.9 released on Nov-11-2011 now has an I2C error counter in the GUI's debug2 window. Please use it to determine if your MultiWii has I2C performance problems. Users with earlier versions (V1.8 and higher) can continue to use the test code discussed here to troubleshoot I2C problems.

The initial release for V1.9 had an I2C bug that caused board lockups on some installations. There were several reports of "dead" boards and I was affected too. Although the latest V1.9 releases have fixed the lockup issue, it turns out that the bug helped identify an I2C performance problem on my model that I did not know I had. Furthermore, it is very likely that any board that experienced the lockup issue on the early V1.9 code has an I2C problem. In other words, the lockup would never have occurred if the I2C transmissions were working perfectly.

While debugging the lockup problem I wrote some test code to see what was going on. The test code chirps the buzzer whenever a basic I2C transmission is not successful. I've since added an error counter to it so that the GUI's debug window can show the number of bad I2C hits.

To demonstrate how the test code helped me: After applying the latest V1.9 code fix my model was working fine (and no hints of any I2C issues!). But after applying the test code the buzzer was very noisy (a lot of beeping) which showed there were I2C problems. Temporarily reducing the I2C clock to 100KHz did not help at this point. My o-scope found an inadequate I2C clock signal (SCL); It had poor rise-time and low amplitude. I reduced the SCL pullup resistor value and then ran the test again. The buzzer chirp test was now very quiet, so I thought all the problems were solved. But I allowed the test to run for more than an hour. Every few minutes I would hear a single short chirp. At the end of the hour long test the GUI debug window showed a half-dozen error hits. Reducing the I2C speed to 100KHz eliminated the final remaining sporadic errors and all I2C issues were solved.

To use the debug code all you have to do is edit the sensors.pde file. This code should work in version 1.8 and higher. So open sensors.pde and completely comment out the waitTransmissionI2C() function. Replace it with this:

Code: Select all

// START OF RC-CAM DEBUG CODE.
// I2C Test Code: Used to help identify I2C xfr problems.
// If TWINT flag is not detected the buzzer will chirp and the
// global error counter will increment (max 1000 hits).
// Hint: The GUI's debug window can be used to show the error count.
#define I2C_WAIT_TIME 2000            // Timeout count is long enough to hear the buzzer chirp.
static int16_t i2c_wait_error = -1;   // I2C error counter. Init to -1 for GUI confirmation.
void waitTransmissionI2C() {
  uint16_t count = I2C_WAIT_TIME;
  while (!(TWCR & (1<<TWINT))) {
    count--;
    if (count==(I2C_WAIT_TIME-200)){ // TWINT flag not detected, I2C problem!
        BUZZERPIN_ON;                // Chirp the user.
        if(i2c_wait_error == -1) {
            i2c_wait_error = 1;
        }
        else {
            if(i2c_wait_error<1000) i2c_wait_error++;  // Limit error count to 1000 max.
        }
    }
    else if (count==0) {            // We are in a blocking state => we don't insist
      TWCR = 0;                     // Force a reset on TWINT register
      neutralizeTime = micros();    // We take a timestamp here to neutralize the value during a short delay after the hard reset
      BUZZERPIN_OFF;                // Chirp is done.
      break;                        // Abort, this xfr is bad and needs to be killed.
    }
  }
}
// END OF DEBUG CODE


Then open the serial.pde and find this code sequence (may be different if you have edited this file before):

Code: Select all

      serialize16(BaroAlt/10); // 4 variables are here for general monitoring purpose
      serialize16(0);  // debug2


Replace it with:

Code: Select all

      serialize16(BaroAlt/10); // 4 variables are here for general monitoring purpose
      serialize16(i2c_wait_error);  // debug2


Now upload the edits to your wiiCopter and verify that the GUI's debug2 window shows -1. If it shows 0 then your serial.pde edits are wrong. If greater than 0 then you have I2C problems. Pressing the Arduino reset button will clear the count back to -1 if you want to start the test again.

Allow the model to run for an hour. Motors off is best, so you can hear the buzzer (but try it with motors on too to test for intermittent solder connection problems). Basically, if you hear any chirps or beeps then you've got problems. As an alternative, the GUI's debug2 window will show the running error count (max 1000 errors will be counted).

- Thomas
Last edited by mr.rc-cam on Sat Nov 12, 2011 7:36 pm, edited 2 times in total.

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: I2C R/W Test. Debug code posted here.

Post by copterrichie »

question: was the internal Pull-up Resistors enabled?

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

question: was the internal Pull-up Resistors enabled?

Yes and no. I tried both ways and neither changed the problem.

My custom IMU has FET based logic level conversion that includes all the necessary pullups (10K ohms). My scope showed that SDA signal was fine, but the SCL was lazy looking (shark fin rising edge profile, reduced amplitude). To satisfy my curiosity I enable the ATMEGA's internal I2C pullups to see if that would help improve the SCL since it would be similar to adding a resistor in parallel with the external pullup (effectively reducing its ohm value which is something that was needed in my particular installation). The scope showed a minor improvement but I still had some I2C errors.

So I reduced the external SCL pullup resistor's value on the CPU/5V logic side and that cleaned up the signal to perfection. Now the SCL has very square edges and robust logic levels.

And it rewarded me by fixing an annoyance that haunted my Quad; I used to get a gyro data glitch about 3-5 times a minute with V1.8 and V1.8p2. After fixing the SCL signal I reloaded V1.8p2 and the glitch was gone. So a 2 cent resistor was all that I needed to eliminate an old mystery.

Long story short, I don't think I would have been able to find the problem and confirm the hardware fix without the test code. So I figured someone else may find a good use for it. ;)

mickt
Posts: 3
Joined: Fri Nov 11, 2011 12:48 pm

Re: I2C R/W Test. Debug code posted here.

Post by mickt »


mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

mickt wrote:I found this may be of interest http://dsscircuits.com/articles/effects ... stors.html

That article nicely demonstrates the issue that affected my installation, which has six I2C slaves devices. With the 400KHz I2C clock my waveform looked like the one shown with 10K pullup/400KHz. After I reduced the pullup to 2.2K and changed the clock to 100KHz it looked like the 2.2K/100KHz waveform posted in the article.

Keep in mind that the debug code is a general purpose I2C test. If your model fails it then that does not mean you have the wrong pullup resistors. Finding the cause of the failures is up to your troubleshooting skills, but knowing you have I2C problems is half the battle.

Noctaro
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: I2C R/W Test. Debug code posted here.

Post by Noctaro »

Ok,
it looks like i found my problem too!
As far as i know the Paris V3 board got 2,2k resistors integrated (activated on PARIS by closing the solder pads). I bought the preasembled version with BMA180 + LLC and added HMC5883l + BMP085.

As right now i don´t want to disassemble my copter, give me a hint please.

For this Paris board, the solder bridge should be closed already. Or do i get something wrong?
If the bridge is closed, do i need to activate the internal pullups in the software?

Right now i got them enabled and i think this may cause the glitches. I read anywhere that internal and external PullUps, if both are activated will create bad data.
I did not dare to deactivate the internal pullups right now, as i am afraid of destroying my sensors.

thx & greetz
Noc

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

As far as i know the Paris V3 board got 2,2k resistors integrated (activated on PARIS by closing the solder pads).

If it has the Sparkfun style LLC then the default pullups on the LLC BOB are 10K ohms. You will have to visually inspect your Paris board to determine its pullup values and to see if they are enabled. But unless you know that they are sourced from 5V and are on the "high voltage" CPU side, then I think it is best to disable them. If you need more help with this then post a detailed schematic to the V3 board.

I bought the preasembled version with BMA180 + LLC and added HMC5883l + BMP085.

I suggest you remove the two devices you added (unsolder their SCL and SDA wires), disable them in the sketch, and try the test again. And reduce the I2C speed to 100KHz. If you have an o-scope then use it to see the quality of your I2C signals.

If the bridge is closed, do i need to activate the internal pullups in the software?

With the typical LLC (like Sparkfun's) there will be no harm if the CPU's internal pullups are enabled. But normally they would be disabled when external pullups are used.

Note: There are many reasons for the I2C test to fail. So keep in mind that your chosen pullup values may be fine and the problem might be elsewhere.

KeesvR
Posts: 194
Joined: Fri May 27, 2011 6:51 pm
Location: The Netherlands

Re: I2C R/W Test. Debug code posted here.

Post by KeesvR »

mr.rc-cam wrote:
To use the debug code all you have to do is edit the sensors.pde file. This code should work in version 1.8 and higher. So open sensors.pde and completely comment out the waitTransmissionI2C() function.


- Thomas

Can you exactly describe what i need to comment out, I looked in the sensor.pde and I'm not sure what i need to do.
I'm a noob at coding. :?

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

KeesvR wrote:Can you exactly describe what i need to comment out, I looked in the sensor.pde and I'm not sure what i need to do.
I'm a noob at coding. :?


(1) Make a backup of sensors.pde and serial.pde to another directory (copy them to the desktop). The backups will allow you to revert back to the stock code after you've performed the test.

(2) Open sensors.pde and find the waitTransmissionI2C() function. It will look something like this (may vary a tiny bit depending on the sketch version):

Code: Select all

void waitTransmissionI2C() {
  uint16_t count = 255;
  while (!(TWCR & (1<<TWINT))) {
    count--;
    if (count==0) { //we are in a blocking state => we don't insist
      TWCR = 0;  //and we force a reset on TWINT register
      neutralizeTime = micros(); //we take a timestamp here to neutralize the value during a short delay after the hard reset
      break;    // Abort.
    }
  }
}


(3) Delete that code and replace all of it with the revised waitTransmissionI2C() function code. That is to say, cut and past the entire code segment published in post #1. This code begins with "// START OF RC-CAM DEBUG CODE." and ends with "// END OF DEBUG CODE."

(4) Open serial.pde and perform the small edit there, as explained in post #1.


- Thomas

KeesvR
Posts: 194
Joined: Fri May 27, 2011 6:51 pm
Location: The Netherlands

Re: I2C R/W Test. Debug code posted here.

Post by KeesvR »

All clear now, the first step i had done already, the second was confusing for me because i found a lot of "waitTransmissionI2C" lines.

Thanks Thomas, I'm gonna try this.

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: I2C R/W Test. Debug code posted here.

Post by Alexinparis »

Hi Thomas,

I think I found something very interesting tonight.
I thought we could send a START + STOP condition at the same time to free some bits on the I2C line.

Code: Select all

  TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWSTO); // send REPEAT START condition

It works OK with genuine WMP and other I2C sensors.
Some WMP clones don't like it and it was one of the reason why I implemented the POWERPIN, with the ability to do a hard reset on the bus when a blocking state occurs.
I suspect it doesn't work so well in fact, with some I2C error on the bus that are hidden due to low occurrences. This might explain differences between 1.8 and 1.9 (due to a different and shorter loop time)

So I tried to implement something closer to the norm with a proper STOP order after each command.
It doesn't work so well with WMP clone (still the freeze problem).
But if we don't wait for the STOP command acknowledgement , it works very well.
All my bad clones are now usable without any risk to freeze !

And the best: it works also very well with every other I2C sensors. It's maybe the end of bad spikes.
Could you try with this new Sensors.pde

Code: Select all

// ************************************************************************************************************
// board orientation and setup
// ************************************************************************************************************
//default board orientation
#if !defined(ACC_ORIENTATION)
  #define ACC_ORIENTATION(X, Y, Z)  {accADC[ROLL]  = X; accADC[PITCH]  = Y; accADC[YAW]  = Z;}
#endif
#if !defined(GYRO_ORIENTATION)
  #define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;}
#endif
#if !defined(MAG_ORIENTATION)
  #define MAG_ORIENTATION(X, Y, Z)  {magADC[ROLL]  = X; magADC[PITCH]  = Y; magADC[YAW]  = Z;}
#endif

/*** I2C address ***/
#if !defined(ADXL345_ADDRESS)
  #define ADXL345_ADDRESS 0x3A
  //#define ADXL345_ADDRESS 0xA6   //WARNING: Conflicts with a Wii Motion plus!
#endif

#if !defined(BMA180_ADDRESS)
  #define BMA180_ADDRESS 0x80
  //#define BMA180_ADDRESS 0x82
#endif

#if !defined(ITG3200_ADDRESS)
  #define ITG3200_ADDRESS 0XD0
  //#define ITG3200_ADDRESS 0XD2
#endif

#if !defined(MS561101BA_ADDRESS)
  #define MS561101BA_ADDRESS 0xEE //CBR=0 0xEE I2C address when pin CSB is connected to LOW (GND)
  //#define MS561101BA_ADDRESS 0xEF //CBR=1 0xEF I2C address when pin CSB is connected to HIGH (VCC)
#endif

//ITG3200 and ITG3205 Gyro LPF setting
#if defined(ITG3200_LPF_256HZ) || defined(ITG3200_LPF_188HZ) || defined(ITG3200_LPF_98HZ) || defined(ITG3200_LPF_42HZ) || defined(ITG3200_LPF_20HZ) || defined(ITG3200_LPF_10HZ)
  #if defined(ITG3200_LPF_256HZ)
    #define ITG3200_SMPLRT_DIV 0  //8000Hz
    #define ITG3200_DLPF_CFG   0
  #endif
  #if defined(ITG3200_LPF_188HZ)
    #define ITG3200_SMPLRT_DIV 0  //1000Hz
    #define ITG3200_DLPF_CFG   1
  #endif
  #if defined(ITG3200_LPF_98HZ)
    #define ITG3200_SMPLRT_DIV 0
    #define ITG3200_DLPF_CFG   2
  #endif
  #if defined(ITG3200_LPF_42HZ)
    #define ITG3200_SMPLRT_DIV 0
    #define ITG3200_DLPF_CFG   3
  #endif
  #if defined(ITG3200_LPF_20HZ)
    #define ITG3200_SMPLRT_DIV 0
    #define ITG3200_DLPF_CFG   4
  #endif
  #if defined(ITG3200_LPF_10HZ)
    #define ITG3200_SMPLRT_DIV 0
    #define ITG3200_DLPF_CFG   5
  #endif
#else
    //Default settings LPF 256Hz/8000Hz sample
    #define ITG3200_SMPLRT_DIV 0  //8000Hz
    #define ITG3200_DLPF_CFG   0
#endif

uint8_t rawADC[6];
static uint32_t neutralizeTime = 0;
 
// ************************************************************************************************************
// I2C general functions
// ************************************************************************************************************

// Mask prescaler bits : only 5 bits of TWSR defines the status of each I2C request
#define TW_STATUS_MASK   (1<<TWS7) | (1<<TWS6) | (1<<TWS5) | (1<<TWS4) | (1<<TWS3)
#define TW_STATUS       (TWSR & TW_STATUS_MASK)

void i2c_init(void) {
  #if defined(INTERNAL_I2C_PULLUPS)
    I2C_PULLUPS_ENABLE
  #else
    I2C_PULLUPS_DISABLE
  #endif
  TWSR = 0;                                    // no prescaler => prescaler = 1
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2;   // change the I2C clock rate
  TWCR = 1<<TWEN;                              // enable twi module, no interrupt
}

void i2c_rep_start(uint8_t address) {
  TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) ; // send REPEAT START condition
  waitTransmissionI2C();                       // wait until transmission completed
  TWDR = address;                              // send device address
  TWCR = (1<<TWINT) | (1<<TWEN);
  waitTransmissionI2C();                       // wail until transmission completed
}

void i2c_stop(void) {
  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
  //  while(TWCR & (1<<TWSTO));                <- can produce a blocking state with some WMP clones
}

void i2c_write(uint8_t data ) {   
  TWDR = data;                                 // send data to the previously addressed device
  TWCR = (1<<TWINT) | (1<<TWEN);
  waitTransmissionI2C();
}

uint8_t i2c_readAck() {
  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
  waitTransmissionI2C();
  return TWDR;
}

uint8_t i2c_readNak(void) {
  TWCR = (1<<TWINT) | (1<<TWEN);
  waitTransmissionI2C();
  i2c_stop();
  return TWDR;
}

void waitTransmissionI2C() {
  uint16_t count = 255;
  while (!(TWCR & (1<<TWINT))) {
    count--;
    if (count==0) {              //we are in a blocking state => we don't insist
      TWCR = 0;                  //and we force a reset on TWINT register
      neutralizeTime = micros(); //we take a timestamp here to neutralize the value during a short delay
      #ifdef LOG_VALUES
        i2c_errors_count++;
      #endif
      break;
    }
  }
}

void i2c_getSixRawADC(uint8_t add, uint8_t reg) {
  i2c_rep_start(add);
  i2c_write(reg);         // Start multiple read at the reg register
  i2c_rep_start(add +1);  // I2C read direction => I2C address + 1
  for(uint8_t i = 0; i < 5; i++)
    rawADC[i]=i2c_readAck();
  rawADC[5]= i2c_readNak();
}

void i2c_writeReg(uint8_t add, uint8_t reg, uint8_t val) {
  i2c_rep_start(add+0);  // I2C write direction
  i2c_write(reg);        // register selection
  i2c_write(val);        // value to write in register
  i2c_stop();
}

uint8_t i2c_readReg(uint8_t add, uint8_t reg) {
  i2c_rep_start(add+0);  // I2C write direction
  i2c_write(reg);        // register selection
  i2c_rep_start(add+1);  // I2C read direction
  return i2c_readNak();  // Read single register and return value
}

// ****************
// GYRO common part
// ****************
void GYRO_Common() {
  static int16_t previousGyroADC[3] = {0,0,0};
  static int32_t g[3];
  uint8_t axis;
 
  if (calibratingG>0) {
    for (axis = 0; axis < 3; axis++) {
      // Reset g[axis] at start of calibration
      if (calibratingG == 400) g[axis]=0;
      // Sum up 400 readings
      g[axis] +=gyroADC[axis];
      // Clear global variables for next reading
      gyroADC[axis]=0;
      gyroZero[axis]=0;
      if (calibratingG == 1) {
        gyroZero[axis]=g[axis]/400;
        blinkLED(10,15,1+3*nunchuk);
      }
    }
    calibratingG--;
  }
  for (axis = 0; axis < 3; axis++) {
    gyroADC[axis]  -= gyroZero[axis];
    //anti gyro glitch, limit the variation between two consecutive readings
    gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
    previousGyroADC[axis] = gyroADC[axis];
  }
}

// ****************
// ACC common part
// ****************
void ACC_Common() {
  static int32_t a[3];
 
  if (calibratingA>0) {
    for (uint8_t axis = 0; axis < 3; axis++) {
      // Reset a[axis] at start of calibration
      if (calibratingA == 400) a[axis]=0;
      // Sum up 400 readings
      a[axis] +=accADC[axis];
      // Clear global variables for next reading
      accADC[axis]=0;
      accZero[axis]=0;
    }
    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
    if (calibratingA == 1) {
      accZero[ROLL]  = a[ROLL]/400;
      accZero[PITCH] = a[PITCH]/400;
      accZero[YAW]   = a[YAW]/400-acc_1G; // for nunchuk 200=1G
      accTrim[ROLL]   = 0;
      accTrim[PITCH]  = 0;
      writeParams(); // write accZero in EEPROM
    }
    calibratingA--;
  }
  accADC[ROLL]  -=  accZero[ROLL] ;
  accADC[PITCH] -=  accZero[PITCH];
  accADC[YAW]   -=  accZero[YAW] ;
}


// ************************************************************************************************************
// I2C Barometer BOSCH BMP085
// ************************************************************************************************************
// I2C adress: 0xEE (8bit)   0x77 (7bit)
// principle:
//  1) read the calibration register (only once at the initialization)
//  2) read uncompensated temperature (not mandatory at every cycle)
//  3) read uncompensated pressure
//  4) raw temp + raw pressure => calculation of the adjusted pressure
//  the following code uses the maximum precision setting (oversampling setting 3)
// ************************************************************************************************************

#if defined(BMP085)
#define BMP085_ADDRESS 0xEE
static struct {
  // sensor registers from the BOSCH BMP085 datasheet
  int16_t  ac1, ac2, ac3, b1, b2, mb, mc, md;
  uint16_t ac4, ac5, ac6;
  union {uint16_t val; uint8_t raw[2]; } ut; //uncompensated T
  union {uint32_t val; uint8_t raw[4]; } up; //uncompensated P
  uint8_t  state;
  uint32_t deadline;
} bmp085_ctx; 
#define OSS 3

void i2c_BMP085_readCalibration(){
  delay(10);
  bmp085_ctx.ac1 = i2c_BMP085_readIntRegister(0xAA);
  bmp085_ctx.ac2 = i2c_BMP085_readIntRegister(0xAC);
  bmp085_ctx.ac3 = i2c_BMP085_readIntRegister(0xAE);
  bmp085_ctx.ac4 = i2c_BMP085_readIntRegister(0xB0);
  bmp085_ctx.ac5 = i2c_BMP085_readIntRegister(0xB2);
  bmp085_ctx.ac6 = i2c_BMP085_readIntRegister(0xB4);
  bmp085_ctx.b1  = i2c_BMP085_readIntRegister(0xB6);
  bmp085_ctx.b2  = i2c_BMP085_readIntRegister(0xB8);
  bmp085_ctx.mb  = i2c_BMP085_readIntRegister(0xBA);
  bmp085_ctx.mc  = i2c_BMP085_readIntRegister(0xBC);
  bmp085_ctx.md  = i2c_BMP085_readIntRegister(0xBE);
}

void  Baro_init() {
  delay(10);
  i2c_BMP085_readCalibration();
  i2c_BMP085_UT_Start();
  delay(5);
  i2c_BMP085_UT_Read();
}

// read a 16 bit register
int16_t i2c_BMP085_readIntRegister(uint8_t r) {
  union {int16_t val; uint8_t raw[2]; } data;
  i2c_rep_start(BMP085_ADDRESS + 0);
  i2c_write(r);
  i2c_rep_start(BMP085_ADDRESS + 1);//I2C read direction => 1
  data.raw[1] = i2c_readAck();
  data.raw[0] = i2c_readNak();
  return data.val;
}

// read uncompensated temperature value: send command first
void i2c_BMP085_UT_Start() {
  i2c_writeReg(BMP085_ADDRESS,0xf4,0x2e);
  i2c_rep_start(BMP085_ADDRESS + 0);
  i2c_write(0xF6);
  i2c_stop();
}

// read uncompensated pressure value: send command first
void i2c_BMP085_UP_Start () {
  i2c_writeReg(BMP085_ADDRESS,0xf4,0x34+(OSS<<6)); // control register value for oversampling setting 3
  i2c_rep_start(BMP085_ADDRESS + 0); //I2C write direction => 0
  i2c_write(0xF6);
  i2c_stop();
}

// read uncompensated pressure value: read result bytes
// the datasheet suggests a delay of 25.5 ms (oversampling settings 3) after the send command
void i2c_BMP085_UP_Read () {
  i2c_rep_start(BMP085_ADDRESS + 1);//I2C read direction => 1
  bmp085_ctx.up.raw[2] = i2c_readAck();
  bmp085_ctx.up.raw[1] = i2c_readAck();
  bmp085_ctx.up.raw[0] = i2c_readNak();
}

// read uncompensated temperature value: read result bytes
// the datasheet suggests a delay of 4.5 ms after the send command
void i2c_BMP085_UT_Read() {
  i2c_rep_start(BMP085_ADDRESS + 1);//I2C read direction => 1
  bmp085_ctx.ut.raw[1] = i2c_readAck();
  bmp085_ctx.ut.raw[0] = i2c_readNak();
}

void i2c_BMP085_Calculate() {
  int32_t  x1, x2, x3, b3, b5, b6, p, tmp;
  uint32_t b4, b7;
  // Temperature calculations
  x1 = ((int32_t)bmp085_ctx.ut.val - bmp085_ctx.ac6) * bmp085_ctx.ac5 >> 15;
  x2 = ((int32_t)bmp085_ctx.mc << 11) / (x1 + bmp085_ctx.md);
  b5 = x1 + x2;
  // Pressure calculations
  b6 = b5 - 4000;
  x1 = (bmp085_ctx.b2 * (b6 * b6 >> 12)) >> 11;
  x2 = bmp085_ctx.ac2 * b6 >> 11;
  x3 = x1 + x2;
  tmp = bmp085_ctx.ac1;
  tmp = (tmp*4 + x3) << OSS;
  b3 = (tmp+2)/4;
  x1 = bmp085_ctx.ac3 * b6 >> 13;
  x2 = (bmp085_ctx.b1 * (b6 * b6 >> 12)) >> 16;
  x3 = ((x1 + x2) + 2) >> 2;
  b4 = (bmp085_ctx.ac4 * (uint32_t)(x3 + 32768)) >> 15;
  b7 = ((uint32_t) (bmp085_ctx.up.val >> (8-OSS)) - b3) * (50000 >> OSS);
  p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
  x1 = (p >> 8) * (p >> 8);
  x1 = (x1 * 3038) >> 16;
  x2 = (-7357 * p) >> 16;
  pressure = p + ((x1 + x2 + 3791) >> 4);
}

void Baro_update() {
  if (currentTime < bmp085_ctx.deadline) return;
  bmp085_ctx.deadline = currentTime;
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, BMP085 is ok with this speed
  switch (bmp085_ctx.state) {
    case 0:
      i2c_BMP085_UT_Start();
      bmp085_ctx.state++; bmp085_ctx.deadline += 4600;
      break;
    case 1:
      i2c_BMP085_UT_Read();
      bmp085_ctx.state++;
      break;
    case 2:
      i2c_BMP085_UP_Start();
      bmp085_ctx.state++; bmp085_ctx.deadline += 26000;
      break;
    case 3:
      i2c_BMP085_UP_Read();
      i2c_BMP085_Calculate();
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f;
      bmp085_ctx.state = 0; bmp085_ctx.deadline += 20000;
      break;
  }
}
#endif

// ************************************************************************************************************
// I2C Barometer MS561101BA
// ************************************************************************************************************
// first contribution from Fabio
// modification from Alex (September 2011)
//
// specs are here: http://www.meas-spec.com/downloads/MS5611-01BA03.pdf
// useful info on pages 7 -> 12
#if defined(MS561101BA)

// registers of the device
#define MS561101BA_PRESSURE    0x40
#define MS561101BA_TEMPERATURE 0x50
#define MS561101BA_RESET       0x1E

// OSR (Over Sampling Ratio) constants
#define MS561101BA_OSR_256  0x00
#define MS561101BA_OSR_512  0x02
#define MS561101BA_OSR_1024 0x04
#define MS561101BA_OSR_2048 0x06
#define MS561101BA_OSR_4096 0x08

#define OSR MS561101BA_OSR_4096

static struct {
  // sensor registers from the MS561101BA datasheet
  uint16_t c[7];
  union {uint32_t val; uint8_t raw[4]; } ut; //uncompensated T
  union {uint32_t val; uint8_t raw[4]; } up; //uncompensated P
  uint8_t  state;
  uint32_t deadline;
} ms561101ba_ctx;

void i2c_MS561101BA_reset(){
  i2c_writeReg(MS561101BA_ADDRESS, MS561101BA_RESET, 0);
}

void i2c_MS561101BA_readCalibration(){
  union {uint16_t val; uint8_t raw[2]; } data;
  delay(10);
  for(uint8_t i=0;i<6;i++) {
    i2c_rep_start(MS561101BA_ADDRESS + 0);
    i2c_write(0xA2+2*i);
    i2c_rep_start(MS561101BA_ADDRESS + 1);//I2C read direction => 1
    data.raw[1] = i2c_readAck();  // read a 16 bit register
    data.raw[0] = i2c_readNak();
    ms561101ba_ctx.c[i+1] = data.val;
  }
}

void  Baro_init() {
  delay(10);
  i2c_MS561101BA_reset();
  delay(10);
  i2c_MS561101BA_readCalibration();
}

// read uncompensated temperature value: send command first
void i2c_MS561101BA_UT_Start() {
  i2c_rep_start(MS561101BA_ADDRESS+0);      // I2C write direction
  i2c_write(MS561101BA_TEMPERATURE + OSR);  // register selection
  i2c_stop();
}

// read uncompensated pressure value: send command first
void i2c_MS561101BA_UP_Start () {
  i2c_rep_start(MS561101BA_ADDRESS+0);      // I2C write direction
  i2c_write(MS561101BA_PRESSURE + OSR);     // register selection
  i2c_stop();
}

// read uncompensated pressure value: read result bytes
void i2c_MS561101BA_UP_Read () {
  i2c_rep_start(MS561101BA_ADDRESS + 0);
  i2c_write(0);
  i2c_rep_start(MS561101BA_ADDRESS + 1);
  ms561101ba_ctx.up.raw[2] = i2c_readAck();
  ms561101ba_ctx.up.raw[1] = i2c_readAck();
  ms561101ba_ctx.up.raw[0] = i2c_readNak();
}

// read uncompensated temperature value: read result bytes
void i2c_MS561101BA_UT_Read() {
  i2c_rep_start(MS561101BA_ADDRESS + 0);
  i2c_write(0);
  i2c_rep_start(MS561101BA_ADDRESS + 1);
  ms561101ba_ctx.ut.raw[2] = i2c_readAck();
  ms561101ba_ctx.ut.raw[1] = i2c_readAck();
  ms561101ba_ctx.ut.raw[0] = i2c_readNak();
}

void i2c_MS561101BA_Calculate() {
  int64_t dT   = ms561101ba_ctx.ut.val - ((uint32_t)ms561101ba_ctx.c[5] << 8);  //int32_t according to the spec, but int64_t here to avoid cast after
  int64_t off  = ((uint32_t)ms561101ba_ctx.c[2] <<16) + ((dT * ms561101ba_ctx.c[4]) >> 7);
  int64_t sens = ((uint32_t)ms561101ba_ctx.c[1] <<15) + ((dT * ms561101ba_ctx.c[3]) >> 8);
  pressure     = (( (ms561101ba_ctx.up.val * sens ) >> 21) - off) >> 15;
}

void Baro_update() {
  if (currentTime < ms561101ba_ctx.deadline) return;
  ms561101ba_ctx.deadline = currentTime;
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, MS5611 is ok with this speed
  switch (ms561101ba_ctx.state) {
    case 0:
      i2c_MS561101BA_UT_Start();
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 15000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 1:
      i2c_MS561101BA_UT_Read();
      ms561101ba_ctx.state++;
      break;
    case 2:
      i2c_MS561101BA_UP_Start();
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 15000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 3:
      i2c_MS561101BA_UP_Read();
      i2c_MS561101BA_Calculate();
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f;
      ms561101ba_ctx.state = 0; ms561101ba_ctx.deadline += 30000;
      break;
  }
}
#endif


// ************************************************************************************************************
// I2C Accelerometer ADXL345
// ************************************************************************************************************
// I2C adress: 0x3A (8bit)    0x1D (7bit)
// Resolution: 10bit (Full range - 14bit, but this is autoscaling 10bit ADC to the range +- 16g)
// principle:
//  1) CS PIN must be linked to VCC to select the I2C mode
//  2) SD0 PIN must be linked to VCC to select the right I2C adress
//  3) bit  b00000100 must be set on register 0x2D to read data (only once at the initialization)
//  4) bits b00001011 must be set on register 0x31 to select the data format (only once at the initialization)
// ************************************************************************************************************
#if defined(ADXL345)
void ACC_init () {
  delay(10);
  i2c_writeReg(ADXL345_ADDRESS,0x2D,1<<3); //  register: Power CTRL  -- value: Set measure bit 3 on
  i2c_writeReg(ADXL345_ADDRESS,0x31,0x0B); //  register: DATA_FORMAT -- value: Set bits 3(full range) and 1 0 on (+/- 16g-range)
  i2c_writeReg(ADXL345_ADDRESS,0x2C,8+2+1); // register: BW_RATE     -- value: 200Hz sampling (see table 5 of the spec)
  acc_1G = 256;
}

void ACC_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, ADXL435 is ok with this speed
  i2c_getSixRawADC(ADXL345_ADDRESS,0x32);

  ACC_ORIENTATION( - ((rawADC[3]<<8) | rawADC[2]) ,
                     ((rawADC[1]<<8) | rawADC[0]) ,
                     ((rawADC[5]<<8) | rawADC[4]) );
  ACC_Common();
}
#endif

// ************************************************************************************************************
// contribution initially from opie11 (rc-groups)
// adaptation from C2po (may 2011)
// contribution from ziss_dm (June 2011)
// contribution from ToLuSe (Jully 2011)
// I2C Accelerometer BMA180
// ************************************************************************************************************
// I2C adress: 0x80 (8bit)    0x40 (7bit) (SDO connection to VCC)
// I2C adress: 0x82 (8bit)    0x41 (7bit) (SDO connection to VDDIO)
// Resolution: 14bit
//
// Control registers:
//
// 0x20    bw_tcs:   |                                           bw<3:0> |                        tcs<3:0> |
//                   |                                             150Hz |                 !!Calibration!! |
// ************************************************************************************************************
#if defined(BMA180)
void ACC_init () {
  delay(10);
  //default range 2G: 1G = 4096 unit.
  i2c_writeReg(BMA180_ADDRESS,0x0D,1<<4); // register: ctrl_reg0  -- value: set bit ee_w to 1 to enable writing
  delay(5);
  uint8_t control = i2c_readReg(BMA180_ADDRESS, 0x20);
  control = control & 0x0F; // register: bw_tcs reg: bits 4-7 to set bw -- value: set low pass filter to 10Hz (bits value = 0000xxxx)
  control = control | 0x00;
  i2c_writeReg(BMA180_ADDRESS, 0x20, control);
  delay(5);
  control = i2c_readReg(BMA180_ADDRESS, 0x30);
  control = control & 0xFC;
  control = control | 0x02;
  i2c_writeReg(BMA180_ADDRESS, 0x30, control);
  delay(5);
  acc_1G = 512;
}

void ACC_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2;  // Optional line.  Sensor is good for it in the spec.
  i2c_getSixRawADC(BMA180_ADDRESS,0x02);
  //usefull info is on the 14 bits  [2-15] bits  /4 => [0-13] bits  /8 => 11 bit resolution
  ACC_ORIENTATION(  - ((rawADC[1]<<8) | rawADC[0])/32 ,
                    - ((rawADC[3]<<8) | rawADC[2])/32 ,
                      ((rawADC[5]<<8) | rawADC[4])/32 );
  ACC_Common();
}
#endif

// ************************************************************************************************************
// contribution from Point65 and mgros (rc-groups)
// contribution from ziss_dm (June 2011)
// contribution from ToLuSe (Jully 2011)
// I2C Accelerometer BMA020
// ************************************************************************************************************
// I2C adress: 0x70 (8bit)
// Resolution: 10bit
// Control registers:
//
// Datasheet: After power on reset or soft reset it is recommended to set the SPI4-bit to the correct value.
//            0x80 = SPI four-wire = Default setting
// | 0x15: | SPI4 | enable_adv_INT | new_data_INT | latch_INT | shadow_dis | wake_up_pause<1:0> | wake_up |
// |       |    1 |              0 |            0 |         0 |          0 |                 00 |       0 |
//
// | 0x14: |                       reserved <2:0> |            range <1:0> |               bandwith <2:0> |
// |       |                      !!Calibration!! |                     2g |                         25Hz |
//
// ************************************************************************************************************
#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70,0x15,0x80);
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
  control = control | (0x00 << 3); //Range 2G 00
  control = control | 0x00;        //Bandwidth 25 Hz 000
  i2c_writeReg(0x70,0x14,control);
  acc_1G = 255;
}

void ACC_getADC(){
  TWBR = ((16000000L / 400000L) - 16) / 2;
  i2c_getSixRawADC(0x70,0x02);
  ACC_ORIENTATION(    ((rawADC[1]<<8) | rawADC[0])/64 ,
                      ((rawADC[3]<<8) | rawADC[2])/64 ,
                      ((rawADC[5]<<8) | rawADC[4])/64 );
  ACC_Common();
}
#endif

// ************************************************************************************************************
// standalone I2C Nunchuk
// ************************************************************************************************************
#if defined(NUNCHACK)
void ACC_init() {
  i2c_writeReg(0xA4 ,0xF0 ,0x55 );
  i2c_writeReg(0xA4 ,0xFB ,0x00 );
  delay(250);
  acc_1G = 200;
}

void ACC_getADC() {
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate. !! you must check if the nunchuk is ok with this freq
  i2c_getSixRawADC(0xA4,0x00);

  ACC_ORIENTATION(  ( (rawADC[3]<<2)        + ((rawADC[5]>>4)&0x2) ) ,
                  - ( (rawADC[2]<<2)        + ((rawADC[5]>>3)&0x2) ) ,
                    ( ((rawADC[4]&0xFE)<<2) + ((rawADC[5]>>5)&0x6) ));
  ACC_Common();
}
#endif

// ************************************************************************
// LIS3LV02 I2C Accelerometer
//contribution from adver (http://multiwii.com/forum/viewtopic.php?f=8&t=451)
// ************************************************************************
#if defined(LIS3LV02)
#define LIS3A  0x3A // I2C adress: 0x3A (8bit)

void i2c_ACC_init(){
  i2c_writeReg(LIS3A ,0x20 ,0xD7 ); // CTRL_REG1   1101 0111 Pwr on, 160Hz
  i2c_writeReg(LIS3A ,0x21 ,0x50 ); // CTRL_REG2   0100 0000 Littl endian, 12 Bit, Boot
  acc_1G = 256;
}

void i2c_ACC_getADC(){
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
  i2c_getSixRawADC(LIS3A,0x28+0x80);
  ACC_ORIENTATION(  (rawADC[3]<<8 | rawADC[2])/4 ,
                   -(rawADC[1]<<8 | rawADC[0])/4 ,
                   -(rawADC[5]<<8 | rawADC[4])/4);
  ACC_Common();
}
#endif

// ************************************************************************************************************
// I2C Accelerometer LSM303DLx
// contribution from wektorx (http://www.multiwii.com/forum/viewtopic.php?f=8&t=863)
// ************************************************************************************************************
#if defined(LSM303DLx_ACC)
void ACC_init () {
  delay(10);
  i2c_writeReg(0x30,0x20,0x27);
  i2c_writeReg(0x30,0x23,0x30);
  i2c_writeReg(0x30,0x21,0x00);

  acc_1G = 256;
}

  void ACC_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2;
  i2c_getSixRawADC(0x30,0xA8);

  ACC_ORIENTATION( - ((rawADC[3]<<8) | rawADC[2])/16 ,
                     ((rawADC[1]<<8) | rawADC[0])/16 ,
                     ((rawADC[5]<<8) | rawADC[4])/16 );
  ACC_Common();
}
#endif

// ************************************************************************************************************
// ADC ACC
// ************************************************************************************************************
#if defined(ADCACC)
void ACC_init(){
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  acc_1G = 75;
}

void ACC_getADC() {
  ACC_ORIENTATION( -analogRead(A1) ,
                   -analogRead(A2) ,
                    analogRead(A3) );
  ACC_Common();
}
#endif

// ************************************************************************************************************
// contribution from Ciskje
// I2C Gyroscope L3G4200D
// ************************************************************************************************************
#if defined(L3G4200D)
void Gyro_init() {
  delay(100);
  i2c_writeReg(0XD2+0 ,0x20 ,0x8F ); // CTRL_REG1   400Hz ODR, 20hz filter, run!
  delay(5);
  i2c_writeReg(0XD2+0 ,0x24 ,0x02 ); // CTRL_REG5   low pass filter enable
}

void Gyro_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
  i2c_getSixRawADC(0XD2,0x80|0x28);

  GYRO_ORIENTATION(  ((rawADC[1]<<8) | rawADC[0])/20  ,
                     ((rawADC[3]<<8) | rawADC[2])/20  ,
                    -((rawADC[5]<<8) | rawADC[4])/20  );
  GYRO_Common();
}
#endif

// ************************************************************************************************************
// I2C Gyroscope ITG3200
// ************************************************************************************************************
// I2C adress: 0xD2 (8bit)   0x69 (7bit)
// I2C adress: 0xD0 (8bit)   0x68 (7bit)
// principle:
// 1) VIO is connected to VDD
// 2) I2C adress is set to 0x69 (AD0 PIN connected to VDD)
// or 2) I2C adress is set to 0x68 (AD0 PIN connected to GND)
// 3) sample rate = 1000Hz ( 1kHz/(div+1) )
// ************************************************************************************************************
#if defined(ITG3200)
void Gyro_init() {
  delay(100);
  i2c_writeReg(ITG3200_ADDRESS, 0x3E, 0x80); //register: Power Management  --  value: reset device
//  delay(5);
//  i2c_writeReg(ITG3200_ADDRESS, 0x15, ITG3200_SMPLRT_DIV); //register: Sample Rate Divider  -- default value = 0: OK
  delay(5);
  i2c_writeReg(ITG3200_ADDRESS, 0x16, 0x18 + ITG3200_DLPF_CFG); //register: DLPF_CFG - low pass filter configuration
  delay(5);
  i2c_writeReg(ITG3200_ADDRESS, 0x3E, 0x03); //register: Power Management  --  value: PLL with Z Gyro reference
  delay(100);
}

void Gyro_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
  i2c_getSixRawADC(ITG3200_ADDRESS,0X1D);
  GYRO_ORIENTATION(  + ( ((rawADC[2]<<8) | rawADC[3])/4) , // range: +/- 8192; +/- 2000 deg/sec
                     - ( ((rawADC[0]<<8) | rawADC[1])/4 ) ,
                     - ( ((rawADC[4]<<8) | rawADC[5])/4 ) );
  GYRO_Common();
}
#endif



// ************************************************************************************************************
// I2C Compass common function
// ************************************************************************************************************
#if MAG
void Mag_getADC() {
  static uint32_t t,tCal = 0;
  static int16_t magZeroTempMin[3];
  static int16_t magZeroTempMax[3];
  uint8_t axis;
  if ( currentTime < t ) return; //each read is spaced by 100ms
  t = currentTime + 100000;
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
  Device_Mag_getADC();
  if (calibratingM == 1) {
    tCal = t;
    for(axis=0;axis<3;axis++) {magZero[axis] = 0;magZeroTempMin[axis] = 0; magZeroTempMax[axis] = 0;}
    calibratingM = 0;
  }
  magADC[ROLL]  -= magZero[ROLL];
  magADC[PITCH] -= magZero[PITCH];
  magADC[YAW]   -= magZero[YAW];
  if (tCal != 0) {
    if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
      LEDPIN_TOGGLE;
      for(axis=0;axis<3;axis++) {
        if (magADC[axis] < magZeroTempMin[axis]) magZeroTempMin[axis] = magADC[axis];
        if (magADC[axis] > magZeroTempMax[axis]) magZeroTempMax[axis] = magADC[axis];
      }
    } else {
      tCal = 0;
      for(axis=0;axis<3;axis++)
        magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis])/2;
      writeParams();
    }
  }
}
#endif

// ************************************************************************************************************
// I2C Compass HMC5843 & HMC5883
// ************************************************************************************************************
// I2C adress: 0x3C (8bit)   0x1E (7bit)
// ************************************************************************************************************
#if defined(HMC5843) || defined(HMC5883)
void Mag_init() {
  delay(100);
  i2c_writeReg(0X3C ,0x02 ,0x00 ); //register: Mode register  --  value: Continuous-Conversion Mode
}

void Device_Mag_getADC() {
  i2c_getSixRawADC(0X3C,0X03);
  #if defined(HMC5843)
    MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) ,
                     ((rawADC[2]<<8) | rawADC[3]) ,
                    -((rawADC[4]<<8) | rawADC[5]) );
  #endif
  #if defined (HMC5883)
    MAG_ORIENTATION( ((rawADC[4]<<8) | rawADC[5]) ,
                    -((rawADC[0]<<8) | rawADC[1]) ,
                    -((rawADC[2]<<8) | rawADC[3]) );
  #endif
}
#endif

// ************************************************************************************************************
// I2C Compass AK8975 (Contribution by EOSBandi)
// ************************************************************************************************************
// I2C adress: 0x18 (8bit)   0x0C (7bit)
// ************************************************************************************************************
#if defined(AK8975)
void Mag_init() {
  delay(100);
  i2c_writeReg(0x18,0x0a,0x01);  //Start the first conversion
  delay(100);
}

void Device_Mag_getADC() {
  i2c_getSixRawADC(0x18,0x03);
  MAG_ORIENTATION( ((rawADC[3]<<8) | rawADC[2]) ,         
                   ((rawADC[1]<<8) | rawADC[0]) ,     
                  -((rawADC[5]<<8) | rawADC[4]) );
  //Start another meassurement
  i2c_writeReg(0x18,0x0a,0x01);
}
#endif

#if !GYRO
// ************************************************************************************************************
// I2C Wii Motion Plus + optional Nunchuk
// ************************************************************************************************************
// I2C adress 1: 0xA6 (8bit)    0x53 (7bit)
// I2C adress 2: 0xA4 (8bit)    0x52 (7bit)
// ************************************************************************************************************
void WMP_init() {
  delay(250);
  i2c_writeReg(0xA6, 0xF0, 0x55); // Initialize Extension
  delay(250);
  i2c_writeReg(0xA6, 0xFE, 0x05); // Activate Nunchuck pass-through mode
  delay(250);

  // We need to set acc_1G for the Nunchuk beforehand; It's used in WMP_getRawADC() and ACC_Common()
  // If a different accelerometer is used, it will be overwritten by its ACC_init() later.
  acc_1G = 200;
  acc_25deg = acc_1G * 0.423;
  uint8_t numberAccRead = 0;
  // Read from WMP 100 times, this should return alternating WMP and Nunchuk data
  for(uint8_t i=0;i<100;i++) {
    delay(4);
    if (WMP_getRawADC() == 0) numberAccRead++; // Count number of times we read from the Nunchuk extension
  }
  // If we got at least 25 Nunchuck reads, we assume the Nunchuk is present
  if (numberAccRead>25)
    nunchuk = 1;
  delay(10);
}

uint8_t WMP_getRawADC() {
  uint8_t axis;
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
  i2c_getSixRawADC(0xA4,0x00);

  if (micros() < (neutralizeTime + NEUTRALIZE_DELAY)) {//we neutralize data in case of blocking+hard reset state
    for (axis = 0; axis < 3; axis++) {gyroADC[axis]=0;accADC[axis]=0;}
    accADC[YAW] = acc_1G;
    return 1;
  }

  // Wii Motion Plus Data
  if ( (rawADC[5]&0x03) == 0x02 ) {
    // Assemble 14bit data
    gyroADC[ROLL]  = - ( ((rawADC[5]>>2)<<8) | rawADC[2] ); //range: +/- 8192
    gyroADC[PITCH] = - ( ((rawADC[4]>>2)<<8) | rawADC[1] );
    gyroADC[YAW]  =  - ( ((rawADC[3]>>2)<<8) | rawADC[0] );
    GYRO_Common();
    // Check if slow bit is set and normalize to fast mode range
    gyroADC[ROLL]  = (rawADC[3]&0x01)     ? gyroADC[ROLL]/5  : gyroADC[ROLL];  //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification
    gyroADC[PITCH] = (rawADC[4]&0x02)>>1  ? gyroADC[PITCH]/5 : gyroADC[PITCH]; //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details)
    gyroADC[YAW]   = (rawADC[3]&0x02)>>1  ? gyroADC[YAW]/5   : gyroADC[YAW];   // this step must be done after zero compensation   
    return 1;
  } else if ( (rawADC[5]&0x03) == 0x00 ) { // Nunchuk Data
    ACC_ORIENTATION(  ( (rawADC[3]<<2)      | ((rawADC[5]>>4)&0x02) ) ,
                    - ( (rawADC[2]<<2)      | ((rawADC[5]>>3)&0x02) ) ,
                      ( ((rawADC[4]>>1)<<3) | ((rawADC[5]>>5)&0x06) ) );
    ACC_Common();
    return 0;
  } else
    return 2;
}
#endif

void initSensors() {
  delay(200);
  POWERPIN_ON
  delay(100);
  i2c_init();
  delay(100);
  if (GYRO) Gyro_init();
  else WMP_init();
  if (BARO) Baro_init();
  if (ACC) {ACC_init();acc_25deg = acc_1G * 0.423;}
  if (MAG) Mag_init();
}

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

Alexinparis wrote:I think I found something very interesting tonight.
I thought we could send a START + STOP condition at the same time to free some bits on the I2C line.
{snip..snip}
And the best: it works also very well with every other I2C sensors. It's maybe the end of bad spikes.
Could you try with this new Sensors.pde


Thanks for looking at the I2C functions. I tried out your new sensor.pde. Using the error log you've added I see several dozen I2C errors each minute (was zero with the old code). The errors come in groups of 8, perhaps every 5-15 seconds.

But this does not mean that your new I2C code is bad. I'll get a scope back on my I2C buss and see if the SDA needs a different pullup (still using 10K on it). As I have learned, even good I2C code can't work well if the signals are crap or other hardware issues are present.

I think that this I2C corruption issue has been around for along time, even on good working models. But we just didn't know it was there (ignorance is bliss). For that reason I think it would be great if the GUI had a permanent I2C error count feature. It seems it would be as useful as the cycle time counter. Although easy for me to say, I don't think it would be much work to add it since all the basics are already done. In the meantime the GUI debug window method works fine. :)

User avatar
mgros
Posts: 90
Joined: Thu Jan 20, 2011 12:32 am

Re: I2C R/W Test. Debug code posted here.

Post by mgros »

Alexinparis wrote:And the best: it works also very well with every other I2C sensors. It's maybe the end of bad spikes.
Could you try with this new Sensors.pde




FANTASTIC ALEX!!!!!!!!

Zero I2C error now.

You are a genius!!!

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: I2C R/W Test. Debug code posted here.

Post by Alexinparis »

mgros wrote:
Alexinparis wrote:And the best: it works also very well with every other I2C sensors. It's maybe the end of bad spikes.
Could you try with this new Sensors.pde




FANTASTIC ALEX!!!!!!!!

Zero I2C error now.

You are a genius!!!


And does it solve your Quad twitches ?

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

mgros wrote:Zero I2C error now.

Very cool!
But just to confirm, are you using the debug window to monitor I2C errors? If so, did you enable Alex's logging feature (#define LOG_VALUES) AND add his renamed I2C error counter variable to the GUI's debug?

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

Thanks for looking at the I2C functions. I tried out your new sensor.pde. Using the error log you've added I see several dozen I2C errors each minute (was zero with the old code). The errors come in groups of 8, perhaps every 5-15 seconds.

My apologies, but in my rush to test it for you I neglected to connect the battery! So my the board was only powered by USB voltage. With a proper battery the new code does not have any errors (but keep in mind that the old I2C code was working fine for me too). I tried it with 400KHz and captured a several dozen errors in a 1-hour period, so I will continue to use 100KHz. Long story short, your new code does not cause any problems on my hardware, so hopefully that helps validate the new I2C stop method.

I suspect that some of the data glitch problems out there are due to the old I2C code, some are caused by bad I2C hardware (including SCL pullup issues like I had), some a combination of the two, and other issues. MultiWii builder frustrations from such things will persist, so please don't get upset if I continue to ask for a permanent I2C error count feature in the GUI. :)

BTW, the i2c_errors_count increment in your patched file does not have a overflow boundary check. Because it is an uint8_t it quickly wraps around zero when I2C errors are detected. So the code should be changed to this:

Code: Select all

          #ifdef LOG_VALUES
            if(i2c_errors_count<255) i2c_errors_count++; // Max 255 errors will be counted.
          #endif


But I recommend that you change it to an uint16_t and limit it to 1000 errors max (for aesthetics). For those that will be using it in a GUI debug window value, it is better to make it an int16_t and initialize it to -1 for GUI user confirmation. That is how my error count code was setup in the I2C chirp test.

Update: I've added a comment to the first post that mentions the new error counter in V1.9
Last edited by mr.rc-cam on Sat Nov 12, 2011 11:14 pm, edited 2 times in total.

User avatar
mgros
Posts: 90
Joined: Thu Jan 20, 2011 12:32 am

Re: I2C R/W Test. Debug code posted here.

Post by mgros »

Alexinparis wrote:
mgros wrote:
Alexinparis wrote:And the best: it works also very well with every other I2C sensors. It's maybe the end of bad spikes.
Could you try with this new Sensors.pde




FANTASTIC ALEX!!!!!!!!

Zero I2C error now.

You are a genius!!!


And does it solve your Quad twitches ?



Yes, yes, yes. magnifique

User avatar
mgros
Posts: 90
Joined: Thu Jan 20, 2011 12:32 am

Re: I2C R/W Test. Debug code posted here.

Post by mgros »

mr.rc-cam wrote:
mgros wrote:Zero I2C error now.

Very cool!
But just to confirm, are you using the debug window to monitor I2C errors? If so, did you enable Alex's logging feature (#define LOG_VALUES) AND add his renamed I2C error counter variable to the GUI's debug?


I have used both, zero in both cases.

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: I2C R/W Test. Debug code posted here.

Post by Alexinparis »

Hi,

Very good news :)

So I know it's not very clean for keeping clean versions (a patch would have been nicer, more thing to modify also),
but I've just re uploaded a new 1.9.

@Thomas,
i2c_errors_count is now int16_t and is used in debug2 variable by default.
again, it's a quick mod, not optimal ;)

Noctaro
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: I2C R/W Test. Debug code posted here.

Post by Noctaro »

Hey,

I tested your new release right now! Zero I2C errors :)

Inflight?
What should i say?
AHRG! It works perfect, i ll do another test with trusted ACC now to see if i can use small angle correction! No ACC drift anymore! No GLITCHES! Acro AND stable mode, both work perfect now for me! Thank you all! I am really impressed, can´t wait to do some fpv captures!

@mr.rc-cam
thank you for your advice! I just disassembled everything to be sure if the LLC is used and could disable the internal pullup. Will visit a friend of mine who got an Oszi just to proof my signal.


greetz
Noc

flyman777
Posts: 55
Joined: Mon Sep 19, 2011 1:44 pm

Re: I2C R/W Test. Debug code posted here.

Post by flyman777 »

Hi ALEX , Hi ALL,

Thank you for the last release of V1.9.
With the first release 1.9, I had problems with the yaw servo twitching and some randomly flips in roll. So I went back to DEV20011029 which works very well.
Apparently they were problems wuth ic2 transmissions.
I uploaded today the last release and all is working perfectly and my tri flyes as never.

However I made some changes in IMU.pde, I feel the tri is more better flying and the stable mode is so much precise yet. The acro mode is also more precise and agile with my settings.
I fly a Atmega328P UNO on self made board + WM+ + BMA180
Trusted-accz remains commented.

My changes:

in IMU.pde :

#define ACC_LPF_FACTOR 6 //3->8, 4->16, 5->32, 6->64

#define GYR_CMPF_FACTOR 330.0f

in void getEstimatedAttitude(){ :
...
static int32_t accTemp[3];
...
#if defined(ACC_LPF_FACTOR)
accTemp[axis] = (accTemp[axis] - (accTemp[axis] >>ACC_LPF_FACTOR)) + accADC[axis];
accSmooth[axis] = accTemp[axis]>>ACC_LPF_FACTOR;
#define ACC_VALUE accSmooth[axis]
...

Thanks a lot
Regards
Claude
flyman777

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

I've just re uploaded a new 1.9 ... {snip .. snip}
i2c_errors_count is now int16_t and is used in debug2 variable by default.
again, it's a quick mod, not optimal

Thanks, I'll try out the new version later today. Now if anyone using the latest code still complains of spikes or glitches, the debug value will help ID bad I2C hardware. There will probably be some models that still have intermittent data spike problems, so be sure to spread the news about your new I2C error count feature to help them troubleshoot it.

- Thomas

flyman777
Posts: 55
Joined: Mon Sep 19, 2011 1:44 pm

Re: I2C R/W Test. Debug code posted here.

Post by flyman777 »

Hi,
I made today 8 flights with the last release and NO i2c errors either on LCD or on GUI.
The tri became so precise that I can fly quasi with a precision of 1 cm !

Best regards

flyman777

ronco
Posts: 317
Joined: Thu Aug 18, 2011 2:58 pm

Re: I2C R/W Test. Debug code posted here.

Post by ronco »

Hi,

and great work alex !

i have tested it on three arduino + wmp configs for houres and have zero i2c errors..

so we dont need the pin 12 wmp power reset function anymore?



regards

Felix

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: I2C R/W Test. Debug code posted here.

Post by Alexinparis »

ronco wrote:Hi,

and great work alex !

i have tested it on three arduino + wmp configs for houres and have zero i2c errors..

so we dont need the pin 12 wmp power reset function anymore?



regards

Felix


Hi,
Yes exactly, this portion of code is now removed as this should not be useful anymore.

spagoziak
Posts: 171
Joined: Thu Jan 20, 2011 1:18 am

Re: I2C R/W Test. Debug code posted here.

Post by spagoziak »

[quote="Alexinparis"]Hi Thomas,

I think I found something very interesting tonight.
[snip]
But if we don't wait for the STOP command acknowledgement , it works very well.
All my bad clones are now usable without any risk to freeze !

And the best: it works also very well with every other I2C sensors. It's maybe the end of bad spikes.
Could you try with this new Sensors.pde
[snip]

Though I haven't had a chance to fly this code yet, I'm very excited--0 errors after 10 minutes! Thank you very much Alex & Thomas :)

spag

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

Though I haven't had a chance to fly this code yet, I'm very excited--0 errors after 10 minutes!

Please note that Alex's code patch you used does NOT report the I2C errors (if used as-is). So download the latest V1.9 and use it instead.

ashta
Posts: 14
Joined: Mon Mar 14, 2011 9:37 am

Re: I2C R/W Test. Debug code posted here.

Post by ashta »

hello all,

wanted a clarification.
Will the latest code 1.9 support the I2C R/W test and show in debug2 window of the GUI? Or do i have to write the patch as discribed in the begining of this thread?
Some where i read that the debug2 value should be -1 and not zero if it is working.
kindly clarify

best regards
ashta

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

Will the latest code 1.9 support the I2C R/W test and show in debug2 window of the GUI? Or do i have to write the patch as discribed in the begining of this thread?

Alex's latest V1.9 code (dated Nov-12) has the I2C error counter in the Debug2 window. The earlier V1.9 releases did not have it.

Because there are three releases that were labeled V1.9, some confusion about the I2C test feature has occurred. That is to say, it is possible to have I2C errors with V1.9 that are not reported in Debug2 if you have installed an earlier version of V1.9! So if your V1.9 Debug2 says 0, it may mean you have no errors or it may mean that you have the wrong V1.9 installed.

Some where i read that the debug2 value should be -1 and not zero if it is working.

With the patch implementation discussed here, the Debug2 value shows -1 if the I2C test code is active (and no I2C errors are occurring). However, Alex did not use the "-1" trick so there is no way to know if you have the latest V1.9 that has the I2C error counter feature. If in doubt, download V1.9 from the software storage site again and start over.

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: I2C R/W Test. Debug code posted here.

Post by Hamburger »

Hi,
I have a hanging system with v1.9 when the magneto is activated. Copter works with v1.8p1.
Pullups to sensor board FreeIMUv1 are 2.7k. Concise description here viewtopic.php?f=8&t=941.
Would this error be i2c related? What could I try to diagnose the error further?

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: I2C R/W Test. Debug code posted here.

Post by mr.rc-cam »

Hamburger wrote:Hi,
I have a hanging system with v1.9 when the magneto is activated. Copter works with v1.8p1.
Pullups to sensor board FreeIMUv1 are 2.7k. Concise description here viewtopic.php?f=8&t=941.
Would this error be i2c related? What could I try to diagnose the error further?


Your GUI lockup is a sign that you do not have the latest V1.9 installed; The earlier version would freeze the GUI if it encountered any I2C issues, so there is a chance you do not have the correct V1.9 installed. Beyond that, please see my comments in your other discussion.

ashta
Posts: 14
Joined: Mon Mar 14, 2011 9:37 am

Re: I2C R/W Test. Debug code posted here.

Post by ashta »

thanks mr.rc-cam for such clear and crisp reply.

Post Reply