新手arduino 陀螺仪 电子罗盘 气压计 GPS 整合算法出错
本人新手菜鸟采集陀螺仪数据做姿态解算时，跑着跑着数据就丢失了，代码问题 求大神指教
#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;
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 = 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("degCpres: ");
Serial.print(press,5);
Serial.print("mbaraltitude: ");
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 = 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;
}
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;
}
代码仅供参考能取出各个传感器数据，但是后期处理应该不是很好，本人C语言功底不好 希望大家帮忙检查
页:
[1]