MPU6050 Pitch and Roll filters help

[deepfreeze]

I am trying to make TVC Rocket and trying to apply the mpu6050 by myself since the DMP not working good on rockets. I am getting the gyro and Acc values then i i do a data fusion. Neither complementery, nor kalman filter not working properly. I have two questions.
1- Should i do offset calculation or something in here?
2- How do i apply kalman filter or complementary filter and not having glitchness in my demo.

Can you please help me how to apply mpu6050 on a tvc rocket ? I am about to burnout not sleeping since 22 hours.

#include "I2Cdev.h"
#include "Wire.h"
#include <ArduinoJson.h>
#include <Time.h>
#define TIME_MSG_LEN 11 // time sync to PC is HEADER followed by Unix time_t as ten ASCII digits
#define TIME_HEADER 'T' // Header tag for serial time sync message
#define TIME_REQUEST 7 // ASCII bell character requests a time sync message



//MPU INCLUDES
#include "MPU6050_6Axis_MotionApps20.h"
#include <KalmanFilter.h>
#include <ArduinoJson.h>

KalmanFilter kalmanX(0.001, 0.003, 0.03);
KalmanFilter kalmanY(0.001, 0.003, 0.03);

int MPU_ADDRESS = 0x68;
int MPU_POWER_ADDR = 0X6B;

float GyroX, GyroY, GyroZ = 0;
float gyro_x_angle, gyro_y_angle, gyro_z_angle = 0;
float Temp = 0;
float AccX, AccY, AccZ = 0;
float accel_x_angle, accel_y_angle = 0;

float mpu_yaw, mpu_pitch, mpu_roll = 0;

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;


float mpu_last_time, mpu_real_time, mpu_Ts = 0;




//#############################################################
//DEVICE FUNCTIONS
//#############################################################

//GYROSCOPE
void set_gyro_sensitivity(int sens){

  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x1B); // start device
  Wire.write(sens);

  Wire.endTransmission();

  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 set_accel_sensitivity(int sens){

    Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x1C); // start device
  Wire.write(sens);

  Wire.endTransmission();

  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;

   }

  }

bool mpu_setup() {
    digitalWrite(ledBLU_pin, HIGH);


  Wire.beginTransmission(MPU_ADDRESS);   //initialize comm with MPU @ 0x68
  Wire.write(MPU_POWER_ADDR);                   //write to register 0x6B (POWER MGMT)
  Wire.write(0x00);                   //reset all internal registers to default values
  Wire.endTransmission();             //end comm
  delay(100);

  set_accel_sensitivity(AFS_SEL_2);
  delay(5000);
  set_gyro_sensitivity(FS_SEL_2);
  delay(5000);

    return true;

}

void read_gyro_data(){
  mpu_last_time = mpu_real_time;
  mpu_real_time = millis();
  mpu_Ts = (mpu_real_time - mpu_last_time) / 1000;

  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x3B);
  Wire.endTransmission();

  Wire.requestFrom(MPU_ADDRESS,6);

  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
  //convert data to units of degree
  GyroX = GyroX / GYRO_LSB_SENS; //degree/
  GyroY = GyroY / GYRO_LSB_SENS;
  GyroZ = GyroZ / GYRO_LSB_SENS;

  gyro_z_angle += GyroZ * mpu_Ts;


}

void read_accel_data(){
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x3B);
  Wire.endTransmission();

  Wire.requestFrom(MPU_ADDRESS,6);

  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


  //convert data to G unite 1g = 9.8 m/s^2
  AccX = AccX / LSB_SENS;
  AccY = AccY / LSB_SENS;
  AccZ = AccZ / LSB_SENS;

  accel_x_angle = (atan2(AccY, AccZ)) * 180 / PI;                                   //calculate roll
  accel_y_angle = (atan2(-AccX, sqrt(pow(AccY, 2) + pow(AccZ, 2)))) * 180 / PI;   //calculate pitch


}

void data_fusion(){
//complementery filter
  //mpu_roll = 0.96 * (mpu_roll + gyro_x_angle * mpu_Ts) + 0.04 * accel_x_angle;
  //mpu_pitch = 0.96 * (mpu_pitch + gyro_z_angle * mpu_Ts) + 0.04 * accel_y_angle;
  // Kalman filter
  mpu_pitch = kalmanY.update(accel_y_angle, GyroY);
  mpu_roll = kalmanX.update(accel_x_angle, GyroX);

}
void read_and_send_mpu_data(){

  read_accel_data();
  read_gyro_data();
  data_fusion();


  DynamicJsonDocument doc(1024);
  doc["CODE"] = "MPU_DATA";
  doc["yaw"] = 0;
  doc["pitch"] = mpu_pitch;
  doc["roll"] = mpu_roll;
  doc["x_accel"] = 0;
  doc["y_accel"] = mpu_pitch;
  doc["z_accel"] = mpu_roll;


 serializeJson(doc, Serial);
 Serial.println();




}






void setup() {

    pinMode(ledGRN_pin, OUTPUT);
    pinMode(ledBLU_pin, OUTPUT);
    pinMode(ledRED_pin, OUTPUT);
    pinMode(PARACHUTE_pin, OUTPUT);
    pinMode(LEGS_pin, OUTPUT);
  pinMode(MOTOR_pin, OUTPUT);
    Wire.begin();
    Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

    // initialize serial communication
    // (115200 chosen for teapot output but we dont use it. You can change it later. See if it effects python code)
    Serial.begin(115200);

    is_bmp_ready = bmp280_setup();
    is_mpu_ready = mpu_setup();

    set_rocket_state(GROUND_IDLE);



}



//#############################################################
//MAIN PROGRAM LOOP
//#############################################################

void loop() {


  read_and_send_mpu_data();




}

 

[waltr]

1- I do not see the function

set_rocket_state(GROUND_IDLE);

defined.
I assume this is the obtain Offset Errors for calibration.

But then I do not see where you substrate the Offset Errors from the data read.

2- The "Loop()" has nothing to set a time interval. Data fusion requires data input at the KNOWN Time Step.
Look up examples that use a timer to set the interval of data sampling. It may be a function named milli() or something like that. Check the compiler/code library documents for the name and usage.

3- Can you post a link to the Kalman function source? There are many ways to implement a Kalman filter.
Where did you get the Kalman coefficients?
You should be able to get reasonably smooth results without either of those filers.

4- What is a "TVC Rocket"?
You have started two other threads on your MPU6050 project. It would be best to keep all of this in one thread. Then everyone can easily see what you have tried and your progression.

 

[waltr]

I would back up a bit and test that the RAW Sensor data looks good.

In the loop read the sensors (acc & Gyro) then output them on the serial port to your PC. Move the sensor as you did in the video in the other thread.
A good format is send all 6 values separated with a comma.
Capture the data to a file then open the file in Excel or other spreadsheet program.
Now graph the data.

The plots should show smoothly changing lines and staying within the range of expected values.
If not then there is a problem with reading the sensors.

You can also use this code to obtain offsets.
With NO MOTION the Gryos should be Zero but will have some error.
The Accelerometer should Read 9.81m/s/s (1G) on each axis that is aligned with the Earth's Gravity (up). So with the sensor flat one axis should read either 9.81 or -9.81 and when flipped over read the opposite.

When I wire new parts and write new code I always start with basics to ensure it is working properly. Only then add code for more complex calculations.