File content as of revision 8:8f3dfde6fa88:

#include "mbed.h"
#include "FXOS8700Q.h"
#include "FXAS21000.h"
#include "kalman.c"
#include "arm_math.h"

#define PI             3.14159265358979f
#define Rad2Dree       57.295779513082320876798154814105f

#define PID_ROLL_KP        0.0245f              /* Proporcional */ //0.015f
#define PID_ROLL_KI        0.000175f              /* Integral */
#define PID_ROLL_KD        0.0f              /* Derivative */

#define PID_PITCH_KP       0.0245f              /* Proporcional */ //0.015f
#define PID_PITCH_KI       0.000175f              /* Integral */
#define PID_PITCH_KD       0.0f              /* Derivative */

#define PID_YAW_KP       0.0245f              /* Proporcional */ //0.015f
#define PID_YAW_KI       0.000175f              /* Integral */
#define PID_YAW_KD       0.0f              /* Derivative */

#define ROLL_SP            PI/2
#define PITCH_SP           PI/2
#define YAW_SP             PI/2
FXOS8700Q_acc combo_acc(D14,D15,FXOS8700CQ_SLAVE_ADDR0);
FXOS8700Q_mag combo_mag(D14,D15,FXOS8700CQ_SLAVE_ADDR0);
FXAS21000 gyro(D14,D15);

AnalogIn   ultra(A0);
Timer GlobalTime;
Timer ProgramTimer;

PwmOut esc1(D10);
PwmOut esc2(D11);
PwmOut esc3(D12);
PwmOut esc4(D13);
DigitalOut reset1(D7);
    

kalman filter_pitch; 
kalman filter_roll;
kalman filter_yaw;

Serial xbe(D1,D0); 
 
Serial pc(USBTX, USBRX); 
   // tx, rx

float R;
double angle[3];
unsigned long timer;
long loopStartTime;
char i;
char command = ' ';
char sflag = 0;
float high; 

float roll_error;
float pitch_error;
float yaw_error;

float roll;
float pitch;
float yaw;
float ESC1 = 0.0006f;   //pitch up
float ESC2 = 0.0006f;   //roll  up
float ESC3 = 0.0006f;   //roll  down
float ESC4 = 0.0006f;   //pitch down

int main() {
    
    char c;
     float gyro_data[3];
    MotionSensorDataUnits adata;
    MotionSensorDataUnits mdata;
    
     arm_pid_instance_f32 RPID;
    arm_pid_instance_f32 PPID;
    arm_pid_instance_f32 YPID;
  
      //Pitch
    PPID.Kp = PID_PITCH_KP/1000.0f;        /* Proporcional */
    PPID.Ki = PID_PITCH_KI/1000.0f;        /* Integral */
    PPID.Kd = PID_PITCH_KD/1000.0f;        /* Derivative */
    
    //Roll
    RPID.Kp = PID_ROLL_KP/1000.0f;        /* Proporcional */
    RPID.Ki = PID_ROLL_KI/1000.0f;        /* Integral */
    RPID.Kd = PID_ROLL_KD/1000.0f;        /* Derivative */
    
    //yaw
     YPID.Kp = PID_YAW_KP/1000.0f;        /* Proporcional */
    YPID.Ki = PID_YAW_KI/1000.0f;        /* Integral */
    YPID.Kd = PID_YAW_KD/1000.0f;        /* Derivative */
    
    
    arm_pid_init_f32(&RPID, 1);
    arm_pid_init_f32(&PPID, 1);
    arm_pid_init_f32(&YPID, 1);
    
    
    
 reset1=0;
 xbe.baud(9600);
 reset1=1;
 
 GlobalTime.start();
    
    esc1.period(0.02f); 
    esc2.period(0.02f);
    esc3.period(0.02f);
    esc4.period(0.02f);              //Comparten el mismo timer
    esc1.pulsewidth(ESC1);
    esc2.pulsewidth(ESC2);
    esc3.pulsewidth(ESC3);
    esc4.pulsewidth(ESC4);
    
    combo_acc.enable();
    combo_mag.enable();
    printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
    printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
    
    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
     kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);

    

        
while(1)
{
                
                while(xbe.readable())
       
         {
                     c = xbe.getc();
                     
                     pc.printf("Value %c",c);
                      pc.printf("receving");
            
                     
                    if (c == 'u') 
          {
            if (ESC1<0.001f) 
            {
        ESC1 += 0.0001f;
        ESC2 += 0.0001f;
        ESC3 += 0.0001f;
        ESC4 += 0.0001f;
        esc1.pulsewidth(ESC1);
        esc2.pulsewidth(ESC2);
        esc3.pulsewidth(ESC3);
        esc4.pulsewidth(ESC4);
        wait_ms(1000);
         pc.printf("Up");
            }
        }    
            
           
               
     else if (c == 'd') 
     {
            if ((ESC1 > 0.0006f)||(ESC2 > 0.0006f)||(ESC3 > 0.0006f)||(ESC4 > 0.0006f))
             {
                 ESC1 -= 0.0001f;
        ESC2 -= 0.0001f;
        ESC3 -= 0.0001f;
        ESC4 -= 0.0001f;
        esc1.pulsewidth(ESC1);
        esc2.pulsewidth(ESC2);
        esc3.pulsewidth(ESC3);
        esc4.pulsewidth(ESC4);
            
                pc.printf("down");
            }
            
                printf("%f\r\n", ESC1);
           
       
        }
        
        else if(c=='r')
        
        
        {
            
            ESC1 = 0.0006f;
         ESC2 = 0.0006f;
        ESC3 = 0.0006f;
        ESC4 = 0.0006f;
            pc.printf("still");
           
                printf("%f\r\n", ESC1);
            
            
       esc1.pulsewidth(ESC1);
        esc2.pulsewidth(ESC2);
        esc3.pulsewidth(ESC3);
        esc4.pulsewidth(ESC4);
  
        
        }
        
        
        else if (c == 's') {
        
                pc.printf("flyingatSameSpeed");
            
            
                printf("%f\r\n", ESC1);
            
           esc1.pulsewidth(ESC1);
        esc2.pulsewidth(ESC2);
        esc3.pulsewidth(ESC3);
        esc4.pulsewidth(ESC4);
        }
        
        
        
        
        else if (c =='g') {
        
                pc.printf("Moving left");
                
             if(ESC3<0.01f)
             {   
               ESC3 += 0.0001f;
        ESC4 += 0.0001f;
        
            }
          
                printf("%f\r\n", ESC3);
            
              
           esc1.pulsewidth(ESC1);
        esc2.pulsewidth(ESC2);
        esc3.pulsewidth(ESC3);
        esc4.pulsewidth(ESC4);
        }
        
        
        
         else if (c =='h') {
        
                pc.printf("Moving roght");
                
                    if(ESC2<0.01f)
             {   
               ESC1 += 0.0001f;
        ESC2 += 0.0001f;
        
            }
          
                printf("%f\r\n", ESC3);
            
              
           esc1.pulsewidth(ESC1);
        esc2.pulsewidth(ESC2);
        esc3.pulsewidth(ESC3);
        esc4.pulsewidth(ESC4);
        }  
    ProgramTimer.start();
    loopStartTime = ProgramTimer.read_us();
    timer = loopStartTime;
     high= (float)(ultra.read_u16()*2.75f/512.0f)*2.54f;
        
        combo_acc.getAxis(adata);
        combo_mag.getAxis(mdata);;
        gyro.ReadXYZ(gyro_data);
        
        R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
        
        kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch, acos(adata.x/R));
        kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll, acos(adata.y/R));
        kalman_predict(&filter_yaw, gyro_data[2],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_yaw, acos(adata.z/R));
     
     angle[0] = kalman_get_angle(&filter_pitch);
        angle[1] = kalman_get_angle(&filter_roll);
        angle[2]=kalman_get_angle(&filter_yaw);
        
        if (angle[0]>PI) angle[0] = PI;
        else if (angle[0]<0) angle[0] = 0.0f;
        else angle[0] += 0.0f;
        
        if (angle[1]>PI) angle[1] = PI;
        else if (angle[1]<0) angle[1] = 0.0f;
        else angle[1] += 0.0f;
          
        if (angle[2]>PI) angle[2] = PI;
        else if (angle[2]<0) angle[2] = 0.0f;
        else angle[2] += 0.0f;
        
        pitch_error = angle[0] - PITCH_SP;
        roll_error = angle[1] - ROLL_SP;
        yaw_error = angle[2] - YAW_SP;
        
        pitch = arm_pid_f32(&PPID, pitch_error);
        roll = arm_pid_f32(&RPID, roll_error);
        yaw = arm_pid_f32(&YPID, yaw_error);
        timer = ProgramTimer.read_us();
        
        ESC1=ESC1-pitch+yaw;
        ESC2=ESC2-roll +yaw;
        ESC3=ESC3+roll-yaw;
        ESC4=ESC3+pitch-yaw;
        
        if(ESC1>0.0014f)
        {
            ESC1=.00014f;
            }
     else if(ESC1<0.001f)
     {
            ESC1=0.001f;
            }
            
    else{
            pc.printf("no change");
            }
            
        
        
        
           if(ESC2>0.0014f)
        {
            ESC2=.00014f;
            }
     else if(ESC2<0.001f)
     {
            ESC2=0.001f;
            }
            
    else{
            pc.printf("no change");
            }
            
            
               if(ESC3>0.0014f)
        {
            ESC3=.00014f;
            }
     else if(ESC3<0.001f)
     {
            ESC3=0.001f;
            }
            
    else{
            pc.printf("no change");
            }
            
        
           if(ESC4>0.0014f)
        {
            ESC4=.00014f;
            }
     else if(ESC4<0.001f)
     {
            ESC4=0.001f;
            }
            
    else{
            pc.printf("no change");
            }
            
        
         pc.printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
        pc.printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
        printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
        pc.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n YAW Angle Z: %.6f \r\n  ", Rad2Dree * angle[1], Rad2Dree * angle[0], Rad2Dree * angle[2]);
        pc.printf("roll = %.6f     pitch = %.6f \r\n   yaw = %.6f \r\n",roll,pitch,yaw);
        pc.printf("ESC1 = %.6f     ESC4 = %.6f \r\n", ESC1,ESC4);
        pc.printf("ESC2 = %.6f     ESC3 = %.6f \r\n", ESC2,ESC3);
        //printf("dist = %.2f \r\n",dist);
        
        wait(0.02f);
        
            
       }
     
         // 20ms is the default period of the ESC pwm
    }
}