Hello my fellow friends,
I am so happy that i found a forum to throw my problems and talk to a human. Also everyone is so nice. I love you guys.
Alright after love explosion,
I want to build a model rocket for an F-15 motor and 98mm tube. I am using MPU6050 for that. But as you know DMP is not working good for rocketry.
So i want to get the values manually. But i couldn't find a way to do it. I wrote a code but it gives me the angles wrong. I mean not wrong but considering that i will place my gyro into the rocket vertically, i couldn't figure out how i would do it.
I am sharing you a code and my simulation to show you what i mean. It seems like getting the values. But then it correct itself. It is glitchy. It was good when i was using DMP. I understand it wont be as accurate as DMP but i didnt expect this much. Could you please help me ?
I want to improve the usage of this device.
I am so happy that i found a forum to throw my problems and talk to a human. Also everyone is so nice. I love you guys.
Alright after love explosion,
I want to build a model rocket for an F-15 motor and 98mm tube. I am using MPU6050 for that. But as you know DMP is not working good for rocketry.
So i want to get the values manually. But i couldn't find a way to do it. I wrote a code but it gives me the angles wrong. I mean not wrong but considering that i will place my gyro into the rocket vertically, i couldn't figure out how i would do it.
I am sharing you a code and my simulation to show you what i mean. It seems like getting the values. But then it correct itself. It is glitchy. It was good when i was using DMP. I understand it wont be as accurate as DMP but i didnt expect this much. Could you please help me ?
I want to improve the usage of this device.
C:
#include<Wire.h>
#include <ArduinoJson.h>
//CONSTANTS
int MPU_addr = 0x68;// 0x69 when AD0 is HIGH
int MPU_POWER_ADDR = 0X6B;
int16_t AccX,AccY,AccZ,Temp,GyroX,GyroY,GyroZ;
int AFS_SEL_2 = 0x00;
int AFS_SEL_4 = 0x08;
int AFS_SEL_8 = 0x10;
int AFS_SEL_16 = 0x18;
int FS_SEL_2 = 0x00;
int FS_SEL_4 = 0x08;
int FS_SEL_8 = 0x10;
int FS_SEL_16 = 0x18;
int LSB_SENS = 0;
int GYRO_LSB_SENS = 0;
int minVal=265;
int maxVal=402;
double x;
double y;
double z;
double accel_x_cal,accel_y_cal,accel_z_cal;
double accelX, accelY, accelZ;
float rollangle,pitchangle;
float roll,pitch,yaw;
double gyroX, gyroY, gyroZ;
double gyro_x_cal,gyro_y_cal,gyro_z_cal;
#define A 0.962
#define dt 0.020
void setAccelSensitivity(int sens){
Wire.beginTransmission(MPU_addr);
Wire.write(0x1C); // start device
Wire.write(sens);
Wire.endTransmission(true);
if(sens = AFS_SEL_2){
LSB_SENS = 16384;
}
if(sens = AFS_SEL_4){
LSB_SENS = 8192;
}
if(sens = AFS_SEL_8){
LSB_SENS = 4096;
}
if(sens = AFS_SEL_16){
LSB_SENS = 2048;
}
}
void setGyroSensitivity(int sens){
Wire.beginTransmission(MPU_addr);
Wire.write(0x1B); // start device
Wire.write(sens);
Wire.endTransmission(true);
if(sens = AFS_SEL_2){
GYRO_LSB_SENS = 131;
}
if(sens = AFS_SEL_4){
GYRO_LSB_SENS = 65.5;
}
if(sens = AFS_SEL_8){
GYRO_LSB_SENS = 32.8;
}
if(sens = AFS_SEL_16){
GYRO_LSB_SENS = 16.4;
}
}
void setup(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(MPU_POWER_ADDR); // start device
Wire.write(0);
Wire.endTransmission(true);
setAccelSensitivity(AFS_SEL_2);
setGyroSensitivity(FS_SEL_16);
Serial.begin(9600);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AccX = Wire.read() << 8 | Wire.read(); //ACCEL_XOUT_H & ACCEL_XOUT_L
AccY = Wire.read() << 8 | Wire.read(); //ACCEL_YOUT_H & ACCEL_YOUT_L
AccZ = Wire.read() << 8 | Wire.read(); //ACCEL_ZOUT_H & ACCEL_ZOUT_L
Temp = Wire.read() << 8 | Wire.read(); //TEMP_OUT_H & TEMP_OUT_L
GyroX = Wire.read() << 8 | Wire.read(); //ACCEL_XOUT_H & ACCEL_XOUT_L
GyroY = Wire.read() << 8 | Wire.read(); //ACCEL_YOUT_H & ACCEL_YOUT_L
GyroZ = Wire.read() << 8 | Wire.read(); //ACCEL_ZOUT_H & ACCEL_ZOUT_L
accelX = AccX / LSB_SENS;
accelY = AccY / LSB_SENS;
accelZ = AccZ / LSB_SENS;
gyroX = GyroX / GYRO_LSB_SENS;
gyroY = GyroY / GYRO_LSB_SENS;
gyroZ = GyroZ / GYRO_LSB_SENS;
rollangle=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
pitchangle=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET
//Using Complemetary Filter
roll=A*(roll+gyroX*dt)+(1-A)*rollangle;
pitch=A*(pitch+gyroY*dt)+(1-A)*pitchangle;
yaw=gyroZ;
DynamicJsonDocument doc(1024);
doc["CODE"] = "MPU_DATA";
doc["yaw"] = yaw;
doc["pitch"] = pitch;
doc["roll"] = roll;
doc["x_accel"] = 0;
doc["y_accel"] = 0;
doc["z_accel"] = 0;
serializeJson(doc, Serial);
Serial.println();
}