Arduino Asked on November 5, 2021
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() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 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;
}
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.
Answered by David Cary on November 5, 2021
Get help from others!
Recent Questions
Recent Answers
© 2024 TransWikia.com. All rights reserved. Sites we Love: PCI Database, UKBizDB, Menu Kuliner, Sharing RPP