main.cpp

00001 #include "mbed.h"
00002 #include "FXOS8700.h"
00003 #include "FXAS21002.h"
00004 #include "kalman.c"
00005 
00006 #define ROLL_SERVO_CENTER 1570
00007 #define ROLL_SERVO_MAX 2000
00008 #define ROLL_SERVO_MIN 1000
00009 
00010 #define PITCH_SERVO_CENTER 1570
00011 #define PITCH_SERVO_MAX 2000
00012 #define PITCH_SERVO_MIN 1000
00013 
00014 #define PI             3.14159265
00015 #define Rad2Degree       57.2957795
00016 
00017 
00018 Serial pc(USBTX, USBRX); // Serial Port 115200
00019 
00020 // Sensor GPIO Definitions, I2C
00021 FXOS8700 accel(D14,D15);
00022 FXOS8700 mag(D14,D15);
00023 FXAS21002 gyro(D14,D15);
00024 
00025 //Timer used in Kalman filter
00026 Timer ProgramTimer;
00027 
00028 // Servo GPIO Definitions
00029 PwmOut Pitch_M(D11);
00030 PwmOut Roll_M(D10);
00031 
00032 //Define Kalman filter Parameters
00033 kalman filter_pitch; 
00034 kalman filter_roll;
00035 
00036 float R;
00037 float Roll_P_C = 1.0f, Roll_I_C = 0.01f, Roll_D_C = 0.01f; //Roll PID coefficient Values
00038 float Roll_P = 0, Roll_I = 0, Roll_D =0, Roll_Correct = 0, Roll_Error = 0;  //Roll PID Control Values
00039 float Pitch_P_C = 1.0f, Pitch_I_C = 0.01f, Pitch_D_C = 0.01f; //Pitch PID coefficient Values
00040 float Pitch_P = 0, Pitch_I = 0, Pitch_D = 0, Pitch_Correct = 0, Pitch_Error = 0; //Pitch PID control Values
00041 int Roll_Servo_Val, Pitch_Servo_Val;  // Servo pulse widths
00042 double angle[3];      // Kalman Pitch and Roll angles
00043 unsigned long timer;  // absolute time stamp
00044 int val, Loop_Count;  // Loop_Count used to settel kalman filter
00045 
00046 int main() {
00047 
00048     pc.baud(115200); // Serial Port 115200
00049     
00050     // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 & Gyroscope FXAS21002
00051     accel.accel_config();
00052     mag.mag_config();
00053     gyro.gyro_config();
00054     
00055     // Initialize Kalman Filter
00056     kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
00057     kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
00058 
00059     // Accelerometer, Magnomometer and Gyro Variables
00060     float adata[3];       //Accelerometer x, y, x values
00061     float mag_data[3];    //Magnomometer x, y, x values
00062     float gyro_data[3];   //Gyro pitch, roll, yaw values
00063     
00064     // Roll Servo Setup
00065     int Roll_pulse = ROLL_SERVO_CENTER;
00066     Roll_M.period_us(20000);          // servo requires a 20ms period
00067     Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms
00068     // Pitch Servo Setup  
00069     int Pitch_pulse = PITCH_SERVO_CENTER;
00070     Pitch_M.period_us(20000);          // servo requires a 20ms period
00071     Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms
00072     wait(1);  // Allow servos to settel 
00073     
00074     //Absolute Time Needed to Calculate Delta Time
00075     ProgramTimer.start();
00076     timer = ProgramTimer.read_us();
00077     
00078     Loop_Count = 15;   // Allows Kalman filter to sync.
00079 
00080     while (true) {
00081 
00082         //scanf("%d", &val);                       // Used for debug
00083         //printf("\r\n Value is %d \r\n", val);    // Used for debug
00084 
00085         // Acquire Accelerometer values
00086         accel.acquire_accel_data_g(adata);  // Acquire Acceleration Vectors
00087         printf("\r\n Accelerometer Values\r\n");
00088         printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", adata[0],adata[1],adata[2]);
00089                 
00090         // Acquire Magnomometer Vectors
00091         mag.acquire_mag_data_uT(mag_data);        // Acquire Magnomometer Vectors
00092         printf("\r\n Magnomometer Values\r\n");
00093         printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", mag_data[0],mag_data[1],mag_data[2]);    
00094                 
00095         // Acquire Gyro values
00096         gyro.acquire_gyro_data_dps(gyro_data);    // Acquire Gyro values
00097         printf("\r\n Gyro Values\r\n");
00098         printf(" Pitch: %4.2f,\t Roll: %4.2f,\t Yaw: %4.2f\r\n", gyro_data[0],gyro_data[1],gyro_data[2]);
00099         
00100         // RMS Value of Accelerometer 
00101         R = sqrt(std::pow(adata[0], 2) + std::pow(adata[1], 2) + std::pow(adata[2], 2));
00102         
00103         // Kalman Filter
00104         kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); // Predict uses Pitch portion of gyro and delta time
00105         kalman_update(&filter_pitch, acos(adata[0]/R));   //Update uses normalized Pitch portion of accelerometer
00106         kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); // Predict uses Roll portion of gyro and delta time
00107         kalman_update(&filter_roll, acos(adata[1]/R));    //Update uses normalized Roll portion of accelerometer
00108         
00109         angle[0] = kalman_get_angle(&filter_pitch);   // Kalman Pitch
00110         angle[1] = kalman_get_angle(&filter_roll);    // Kalman Roll
00111         
00112         timer = ProgramTimer.read_us();     // Update Timer
00113     
00114         printf("\r\n Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n", Rad2Degree * angle[1], Rad2Degree * angle[0]);
00115 
00116         if (Loop_Count > 0) 
00117         {
00118             // Loop Iterations to Initialize Kalman Filter to sync.
00119             Loop_Count--;
00120             printf("\n\rLoop_Count = %d\n\r", Loop_Count);
00121         }
00122         else
00123         {
00124             // Loop Iterations Complete, Apply Results of Kalman Filter
00125             // PID Controller (Mostly Proportional)
00126             Roll_Error = (Rad2Degree * angle[1]) - 90;  // Calculate Roll Error
00127             Roll_D = Roll_Error - Roll_P;               // Differentiation of Roll Error
00128             Roll_I = Roll_I + Roll_Error;               // Intigration of Roll Error
00129             Roll_P = Roll_Error;                        // Proportional Roll Error
00130             Roll_Correct = Roll_P * Roll_P_C + Roll_I * Roll_I_C + Roll_D * Roll_D_C;  // Sum product of PID coefficients and values
00131             printf("\n\rRoll_Correct = %f\n\r",Roll_Correct);       
00132             
00133             Roll_pulse =  Roll_pulse + Roll_Correct;    // Update Roll Servo PWM Value
00134             if (Roll_pulse <= ROLL_SERVO_MIN) Roll_pulse = ROLL_SERVO_MIN;
00135             if (Roll_pulse >= ROLL_SERVO_MAX) Roll_pulse = ROLL_SERVO_MAX;
00136             Roll_M.pulsewidth_us(Roll_pulse);           // servo position determined by a pulsewidth between 1-2ms  
00137             printf("\n\rRoll_pulse = %d\n\r",Roll_pulse);
00138             
00139             Pitch_Error = (Rad2Degree * angle[0]) - 90; // Calculate Pitch Error
00140             Pitch_D = Pitch_Error - Pitch_P;            // Differentiation of Pitch Error
00141             Pitch_I = Pitch_I + Pitch_Error;            // Intigration of Pitch Error
00142             Pitch_P = Pitch_Error;                      // Proportional Pitch Error
00143             Pitch_Correct = Pitch_P * Pitch_P_C + Pitch_I * Pitch_I_C + Pitch_D * Pitch_D_C; // Sum product of PID coefficients and values
00144             printf("\n\rPitch_Correct = %f\n\r",Pitch_Correct);       
00145 
00146             Pitch_pulse =  Pitch_pulse + Pitch_Correct; // Update Roll Servo PWM Value
00147             if (Pitch_pulse <= PITCH_SERVO_MIN) Pitch_pulse = PITCH_SERVO_MIN;
00148             if (Pitch_pulse >= PITCH_SERVO_MAX) Pitch_pulse = PITCH_SERVO_MAX;
00149             Pitch_M.pulsewidth_us(Pitch_pulse);         // servo position determined by a pulsewidth between 1-2ms  
00150             printf("\n\rPitch_pulse = %d\n\r",Pitch_pulse);               
00151         }         
00152         
00153         wait(0.01);  // Loop timer
00154         }
00155 
00156 }
00157 
00158