新手arduino 陀螺仪 电子罗盘 气压计 GPS 整合算法出错

adasown

本人新手菜鸟
采集陀螺仪数据做姿态解算时，跑着跑着数据就丢失了，代码问题 求大神指教

#include "Wire.h"
#include "SPI.h"  
#include "I2Cdev.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "MS561101BA.h"
#include "NEMAGPS.h"
#include "SoftwareSerial.h"
#include "Servo.h"

Servo Servo_Roll;
Servo Servo_Pitch;
Servo Servo_Yaw;

int val_Roll;
int val_Roll_history;
int val_Pitch;
int val_Yaw;
int val_High;
int deadzone_Roll=3;
int deadzone_Pitch=3;
int deadzone_Yaw=3;
int deadzone_High=2;

#define D_value 5

/////////////////////////create a softwareserial for the gps
SoftwareSerial gpsSerial(10,11); // RX=10, TX=11
/////////////////////////create a GPS object
GPS myGPS(&gpsSerial);
///////////////////////// Store our compass as a variable.
HMC5883L compass;
float xDegrees;
/////////////////////////for MS5611
#define MOVAVG_SIZE 32
MS561101BA baro = MS561101BA();
float movavg_buff[MOVAVG_SIZE];
int movavg_i=0;

const float sea_press = 1013.25;
float press, temp;

// Store our MPU as a variable.
MPU6050 accelgyro;
MPU6050 initialize;
int16_t ax, ay, az;
int16_t gx, gy, gz;

#define Gry_offset_pitch 0.16
#define Gyr_Gain 16.384
#define Angle_offset_pitch -1
#define pi 3.14159

float omega2, Angle_Roll, Angle_Filtered_Pitch;
float Angle_Filtered_Z, Angle_Z, omega3;
///// PID data
float kp_Roll = 9965 * 0.0001;
float ki_Roll = 5 * 0.00000000005;
float kd_Roll = 120 * 0.1;

float kp_Pitch = (100 * 0.0001)+1;
float ki_Pitch  = 5 * 0.00000000005;
float kd_Pitch  =120 * 0.1;

float kp_Yaw = 10 * 0.1;
float ki_Yaw = 0 * 0.0005;
float kd_Yaw = 1 * 0.01;

float kp_High = 10 * 0.1;
float ki_High = 0 * 0.0005;
float kd_High = 1 * 0.01;

///////

float Angle_Raw, Angle_Filtered_Roll, omega;
float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0;

unsigned long preTime = 0;
float SampleTime = 0;
unsigned long lastTime_Roll;
unsigned long lastTime_Pitch;
unsigned long lastTime_Yaw;
unsigned long lastTime_High;
int timeChange_Roll;
int timeChange_Pitch;
int timeChange_Yaw;
int timeChange_High;

float Input_Roll, Output_Roll;
float Input_Pitch, Output_Pitch;
float Input_Yaw, Output_Yaw;
float Input_High, Output_High;

float dErr_Roll, error_Roll;
float dErr_Pitch, error_Pitch;
float dErr_Yaw, error_Yaw;
float dErr_High, error_High;

float errSum_Roll , errSum_Pitch , errSum_Yaw , errSum_High;
float lastErr_Roll, lastErr_Pitch, lastErr_Yaw, lastErr_High;

void setup()
{
  Servo_Roll.attach(3);
  Servo_Roll.writeMicroseconds(1480);  // set servo to mid-point
  Servo_Pitch.attach(4);
  Servo_Pitch.writeMicroseconds(1531);  // set servo to mid-point
  Servo_Yaw.attach(5);
  Servo_Yaw.writeMicroseconds(1500);  // set servo to mid-point
  Wire.begin();
/////////////////////////initialize the softwareserial
  gpsSerial.begin(9600);
/////////////////////////for communication with PC
  Serial.begin(9600);
/////////////////////////
  compass = HMC5883L(); // Construct a new HMC5883 compass.
  //////////////////////////////////////////////
  //MPU6050
  
  accelgyro.initialize();
  initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
   //MS5611
   // Embedded adventures module CSB is VCC so HIGH
  baro.init(MS561101BA_ADDR_CSB_HIGH);
  
  // populate movavg_buff before starting loop
  for(int i=0; i<MOVAVG_SIZE; i++) {
    movavg_buff[i] = baro.getPressure(MS561101BA_OSR_4096);
  }
  for(int i = 0; i < 200; i++)  // Looping 200 times to get the real gesture.
  {
    Filter();
   
  }
}

void loop()
{
  Roll_Pitch();
  
  
  //HMC5883L_Values();
// delay(50);
// Yaw_Control();
  //delay(50);
// PID_Yaw();
  //delay(50);
//MS5611_Values();
//Print_MS5611();
// PID_Roll();
// GPS_Values();
//PID_Roll();
}
void Roll_Pitch()
{
  Filter();  
  Print_mpu6050();
  delay(10);
  Roll_Control();
  PID_Roll();
  delay(10);
  Pitch_Control();
  PID_Pitch();
  delay(10);
}
void Filter()
{
  // Raw datas
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  //Angle_Z = (atan2(ay, ax) * 180 / pi);
  Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset_pitch);
  Angle_Roll = (atan2(ax, az) * 180 / pi);
  
  omega = gx / Gyr_Gain + Gry_offset_pitch;
  omega2 = -(gy / Gyr_Gain - 0.9);
  omega3 = -(gz / Gyr_Gain + 1.68);
  //omega3 = -gz / Gyr_Gain;
  
  Angle_Filtered_Z = Angle_Z + omega3 / 100.00;
  Angle_Z = Angle_Filtered_Z;
  // Filtering datas to get the real gesture.
  unsigned long now = millis();
  float dt = (now - preTime) / 1000.00;
  preTime = now;
  float K = 0.8;
  float A = K / (K + dt);
  Angle_Filtered_Roll = A * (Angle_Filtered_Roll + omega * dt) + (1 - A) * Angle_Raw;
  Angle_Filtered_Pitch = A * (Angle_Filtered_Pitch + omega2 * dt) + (1 - A) * Angle_Roll;
  //Angle_Filtered_Z = A * (Angle_Filtered_Z + omega3 * dt) + (1 - A) * Angle_Z;
  
// Serial.print("YAW:");
// Serial.print(Angle_Filtered_Z);
// Serial.print("\t");
}

void Print_mpu6050()
{
  Serial.print("Roll:");
  Serial.print(Angle_Filtered_Roll,5);
  Serial.print("\t");
  Serial.print("Pitch:");
  Serial.println(Angle_Filtered_Pitch,5);
}
void PID_Roll()
{
  // Calculating the output values using the gesture values and the PID values.
  unsigned long now = millis();
  timeChange_Roll = (now - lastTime_Roll);
  if(timeChange_Roll >= SampleTime)
  {
    Input_Roll = val_Roll;
    error_Roll = Input_Roll;
    errSum_Roll += error_Roll * timeChange_Roll;
    dErr_Roll = (error_Roll - lastErr_Roll) / timeChange_Roll;
    Output_Roll = kp_Roll * error_Roll + ki_Roll * errSum_Roll + kd_Roll * dErr_Roll;
    //Serial.print("Output_Roll*******=");
    //Serial.println(Output_Roll);
    if(Output_Roll>1576)
    Output_Roll=1576;
    if(Output_Roll<1400)
    Output_Roll=1400;
    Servo_Roll.writeMicroseconds(Output_Roll);   

  }
}
void PID_Pitch()
{
  // Calculating the output values using the gesture values and the PID values.
  unsigned long now = millis();
  timeChange_Pitch = (now - lastTime_Pitch);
  if(timeChange_Pitch >= SampleTime)
  {
    Input_Pitch = val_Pitch;
    error_Pitch = Input_Pitch;
    errSum_Pitch += error_Pitch * timeChange_Pitch;
    dErr_Pitch = (error_Pitch - lastErr_Pitch) / timeChange_Pitch;
    Output_Pitch = kp_Pitch * error_Pitch + ki_Pitch * errSum_Pitch + kd_Pitch * dErr_Pitch;
    Serial.print("Output_Pitch*****************=");
    Serial.println(Output_Pitch);
    if(Output_Roll>1616)
    Output_Roll=1616;
    if(Output_Roll<1448)
    Output_Roll=1448;
    Servo_Pitch.writeMicroseconds(Output_Pitch);   

  }
}
  void PID_Yaw()
{
  // Calculating the output values using the gesture values and the PID values.
  unsigned long now = millis();
  timeChange_Yaw = (now - lastTime_Yaw);
  if(timeChange_Yaw >= SampleTime)
  {
    Input_Yaw = val_Yaw;
    error_Yaw = Input_Yaw;
    errSum_Yaw += error_Yaw * timeChange_Yaw;
    dErr_Yaw = (error_Yaw - lastErr_Yaw) / timeChange_Yaw;
    Output_Yaw = kp_Yaw * error_Yaw + ki_Yaw * errSum_Yaw + kd_Yaw * dErr_Yaw;
    Serial.print("\n");
    Serial.print("Output_Yaw=");
    Serial.println(Output_Yaw);
    Serial.print("\n");
    Servo_Yaw.writeMicroseconds(Output_Yaw);   

  }
}
void Roll_Control()
{  
  if(Angle_Filtered_Roll > deadzone_Roll)
  {
     val_Roll=Angle_Filtered_Roll;
     val_Roll=map(abs(val_Roll),deadzone_Roll,150,1464,1400);
     //Serial.print("val_Roll=+");
     //Serial.println(val_Roll);
  }
if(Angle_Filtered_Roll < -deadzone_Roll)
  {
     val_Roll=Angle_Filtered_Roll;
     val_Roll=map(abs(val_Roll),deadzone_Roll,150,1496,1576);
     //Serial.print("val_Roll-=");
     //Serial.println(val_Roll);
  }
}
void Pitch_Control()
{
     
  if(Angle_Filtered_Pitch > deadzone_Pitch)
  {
    val_Pitch=Angle_Filtered_Pitch;
    val_Pitch=map(abs(val_Pitch),deadzone_Pitch,150,1554,1616);
    //Serial.print("val_Pitch+=");
    //Serial.println(val_Pitch);
  }
if(Angle_Filtered_Pitch < -deadzone_Pitch)
  {
    val_Pitch=Angle_Filtered_Pitch;
    val_Pitch=map(abs(val_Pitch),deadzone_Pitch,150,1510,1448);
    //Serial.print("val_Pitch-=");
    //Serial.println(val_Pitch);
  }
  }
void Yaw_Control()
{
  if(xDegrees > 180)
  {
     val_Yaw=xDegrees;
     val_Yaw=map(abs(val_Yaw),180,360,90,180);
     Serial.print("val_Yaw=");
     Serial.println(val_Yaw);
  }
if(xDegrees < 180)
  {
     val_Yaw=xDegrees;
     val_Yaw=map(abs(val_Yaw),180,0,90,0);
     Serial.print("val_Yaw=");
     Serial.println(val_Yaw);
  }
}
void HMC5883L_Values()
{
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  
float xHeading = atan2(scaled.YAxis, scaled.XAxis);
  if(xHeading < 0)    xHeading += 2*PI;
  if(xHeading > 2*PI)    xHeading -= 2*PI;
  xDegrees = xHeading * 180/M_PI;
  Serial.print("Yaw: ");
  Serial.println(xDegrees,5);
}
void Print_HMC5833L()
{
   Serial.print("Yaw: ");
  Serial.println(xDegrees,5);
  /*  rxtrs data
     Serial.print("Raw:\t");
   Serial.print(raw.XAxis);
   Serial.print("   ");   
   Serial.print(raw.YAxis);
   Serial.print("   ");   
   Serial.print(raw.ZAxis);
   Serial.print("   \tScaled:\t");
   
   Serial.print(scaled.XAxis);
   Serial.print("   ");   
   Serial.print(scaled.YAxis);
   Serial.print("   ");   
   Serial.print(scaled.ZAxis);

   Serial.print("   \tHeading:\t");
   Serial.print(heading);
   Serial.print(" Radians   \t");
   Serial.print(headingDegrees);
   Serial.println(" Degrees   \t");*/
  
}

void MS5611_Values()
{
  temp = baro.getTemperature(MS561101BA_OSR_4096);
  pushAvg(baro.getPressure(MS561101BA_OSR_4096));
  press = getAvg(movavg_buff, MOVAVG_SIZE);
}
void Print_MS5611()
{
  Serial.print("temp: ");
  Serial.print(temp,5);
  Serial.print("degC  pres: ");
  Serial.print(press,5);
  Serial.print("mbar  altitude: ");
  Serial.print(getAltitude(press, temp),5);
  Serial.println(" m");
}

float getAltitude(float press, float temp) {
  return ((pow((sea_press / press), 1/5.257) - 1.0) * (temp + 273.15)) / 0.0065;
}

void pushAvg(float val) {
  movavg_buff[movavg_i] = val;
  movavg_i = (movavg_i + 1) % MOVAVG_SIZE;
}

float getAvg(float * buff, int size) {
  float sum = 0.0;
  for(int i=0; i<size; i++) {
    sum += buff[i];
  }
  return sum / size;
}
void GPS_Values()
{
  if(myGPS.read()==0){
        if (myGPS.isFixed()==0)
        {
                Serial.println(F("GPS not fixed"));
                Serial.println(myGPS.getSateNumber());
                Serial.println("***********    *****");
              }
              else
              {
          Print_GPS();
        }  
  }
}
void Print_GPS()
{
    Serial.println(F("GPS fixed"));
            Serial.print(F("Position:"));
            Serial.print("Lat ");
            Serial.print(myGPS.getLat(),5);
            Serial.print('\b');
            Serial.print(myGPS.getNS());
            Serial.print('\t');
            Serial.print("Lon ");
            Serial.print(myGPS.getLon(),5);
            Serial.print('\b');
            Serial.println(myGPS.getEW());
            
            Serial.print("Time:");
            Serial.print("20");
            Serial.print(myGPS.getYear());
            Serial.print("-");
            Serial.print(myGPS.getMonth());
            Serial.print("-");
            Serial.print(myGPS.getDay());
            Serial.print("\t");
            Serial.print(myGPS.getHour()+8);
            Serial.print("-");
            Serial.print(myGPS.getMinute());
            Serial.print("-");
            Serial.print(myGPS.getSecond());
            Serial.print('\n');
            
            Serial.print("Speed:");
            Serial.print(myGPS.getSpdInMs());
            Serial.print("M/S");
            Serial.print('\t');
            Serial.print(myGPS.getSpdInKMh());
            Serial.print("Km/h");
            Serial.print('\n');
            
            Serial.print("Altitude:");
            Serial.print(myGPS.getAlt());
            Serial.print('\n');
            Serial.print("Number of Sate:");
            Serial.print(myGPS.getSateNumber());
            Serial.print('\n');
}
int servo_filter()
{
  int new_val_Roll;
  new_val_Roll=val_Roll;
  
  if((new_val_Roll-val_Roll_history>D_value)||(val_Roll_history-new_val_Roll>D_value))
  new_val_Roll=val_Roll_history;
  return new_val_Roll;
}

adasown

代码仅供参考  能取出各个传感器数据，但是后期处理应该不是很好，本人C语言功底不好 希望大家帮忙检查