1

I am using MPU6050 as a part of IMU of my drone, and I wrote this program for Arduino Nano to fetch the orientation of the MPU6050 module in 3D space. And that works perfectly fine, but when I uploaded this exact code to a NodeMCU, the output started getting increased only, i.e., whenever there's any change of orientation of the module, the output, instead of showing the specific orientation, gets increased on and on. Can you please help me out with interfacing the MPU6050 with my NodeMCU. I am providing the program underneath.

#include <Wire.h>
const int MPU = 0x68; // MPU6050 I2C address

char status;                // Declaring all the necessary variables
double T, P;
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll_sensor, pitch_sensor, yaw_sensor;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;


void setup() {
  Serial.begin(115200);
  Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        // end the transmission
  calculate_IMU_error();  // Call this function if you need to get the IMU error values for your module
  delay(20);
}
void loop() {
  // === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); // Read 6 registers total, each axis value is stored in 2 registers


//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value


// Calculating roll_sensor and pitch_sensor from the accelerometer data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * (180 / PI)) - AccErrorX;
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * (180 / PI)) - AccErrorY;


// === Read gyroscope data === //
  previousTime = currentTime;        // Previous time is stored before the actual time read
  currentTime = millis();            // Current time actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); // Read 4 registers total, each axis value is stored in 2 registers


GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;


// Correct the outputs with the calculated error values
  GyroX = GyroX - GyroErrorX;
  GyroY = GyroY - GyroErrorY;
  GyroZ = GyroZ - GyroErrorZ;


// Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  gyroAngleX = gyroAngleX + (GyroX * elapsedTime); // deg/s * s = deg
  gyroAngleY = gyroAngleY + (GyroY * elapsedTime);
  yaw_sensor =  yaw_sensor + (GyroZ * elapsedTime);


// Complementary filter - combine acceleromter and gyro angle values
  roll_sensor = (0.94 * gyroAngleX) + (0.06 * accAngleX);
  pitch_sensor = (0.94 * gyroAngleY) + (0.06 * accAngleY);


// Print the values on the serial monitor
  Serial.print("                   roll_GY: "); Serial.print(roll_sensor);
  Serial.print("         pitch_GY: "); Serial.print(pitch_sensor);
  Serial.print("         yaw_GY: "); Serial.print(yaw_sensor);
  Serial.println();


}
void calculate_IMU_error() {
  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
  // Read accelerometer values 300 times
  while (c < 300) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * (180 / PI)));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * (180 / PI)));
    c++ ;
  }
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 300;
  AccErrorY = AccErrorY / 300;
  c = 0;
  // Read gyro values 300 times
  while (c < 300) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);


GyroX = Wire.read() &lt;&lt; 8 | Wire.read();
GyroY = Wire.read() &lt;&lt; 8 | Wire.read();
GyroZ = Wire.read() &lt;&lt; 8 | Wire.read();

// Sum all readings
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;



}


//Divide the sum by 300 to get the error value
  GyroErrorX = GyroErrorX / 300;
  GyroErrorY = GyroErrorY / 300;
  GyroErrorZ = GyroErrorZ / 300;
}
prikarsartam
  • 85
  • 7

1 Answers1

0

I see that you are reading 16-bit integers from the IMU like this:

// doesn't work on 32-bit systems
void loop() {
    ...
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
    ...
    GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // ...
    GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
    ....
}

While all too often I am forced to use "#ifdef" to set up platform-specific code for each platform, in this case I'm pretty sure it's possibly to write cross-platform compatible code that will work on both platforms; perhaps something more like this:

// warning: untested, but I think it should work
int16_t wire_read_16(){
    int16_t value = (Wire.read() << 8 | Wire.read());
    return value;
}

void loop() {
    ...
    AccX = (wire_read_16()) * (1.0f / 16384.0f); // X-axis value
    AccY = (wire_read_16()) * (1.0f / 16384.0f); // Y-axis value
    AccZ = (wire_read_16()) * (1.0f / 16384.0f); // Z-axis value
    ...
    GyroX = (wire_read_16()) * (1.0f / 131.0f); // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
    GyroY = (wire_read_16()) * (1.0f / 131.0f);
    GyroZ = (wire_read_16()) * (1.0f / 131.0f);
    ....
}

I see that onehorse is apparently having exactly the same problem at "integer type definitions on Teensy 3.1"

It sounds like maybe you've stumbled over a quirk of the C programming language where "int" varies from system to system. (See Ido Gendel, "The problem with ints"). The "int" in some systems (such as the ATmega based Arduinos) is a 16-bit integer. The "int" in some other systems (such as the ARM based Arduinos, perhaps including NodeMCU) is a 32-bit integer. ( inttypes vs Arduino defined integral types ; "Arduino reference: int" ) In yet other systems, "int" is a variety of other sizes.

For example, when your IMU is trying to send you a "-2", it sends 0xFFFE. When the nano puts that into its "int" which is a 16-bit integer, it then later properly interprets that as meaning "-2". I suspect what's happening is that when the other system puts that value into its "int" which is a 32-bit integer, that somehow ends up containing 0x0000FFFE which means +65534, but what should have happened is "sign extension" so that 32-bit integer ends up containing 0xFFFFFFFE which means "-2". By sticking that value into an intermediate "int16_t" variable, we're explicitly telling the compiler that it's a signed 16-bit integer, so later when that compiler sticks it into a 32-bit integer it sign-extends properly.

The C compiler (correctly) handles an int divided by a float by converting both values to floats (and sign extending properly), and then dividing. Likewise with an int divided by a double.

David Cary
  • 1,122
  • 8
  • 23