File content as of revision 6:6e0f02edb404:

#include "mbed.h"
#include "nRF24L01P.h"
#include "SoftPWM.h"
#include"JY901.h"

#define TRANSFER_SIZE   12
#define fail_max   10 //最大重连次数

#define maxroll 10
#define maxpitch 10
#define maxyaw 10//遥控器调节的最大值

#define maxp_z 0.5
#define maxpi0 0.3
#define maxpi1 0.25
#define maxpi2 0.2//参数设定需满足 0.33*maxp_z+maxpi1+maxpi2<0.5 ;maxp_z+0.5*maxpi2+maxpi0<0.5
#define p_z_max 0.3f
#define p_z_min -0.2
#define I0_INIT 0.0f
#define I1_INIT 0.0f

SoftPWM PWM1(PA_2);
PwmOut PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);//六路PWM输出接口 
//PS:PA_2 mbed不支持PWM输出 与USART3的串口时钟初始化冲突 只能采用软件pwm，其原有库有一定的问题,应用此方法后不应该采用中断处理时间较长的中断）
JY901 _JY901(PA_9,PA_10);//JY901 接口 （波特率为115200，在bufferserial库中更改！）
nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);//nrf模块，nrf可能会由于电压波动断电，需要软件重启

int state=0;//状态标志
int pid_flag=0;//pid标志位
int fail_num=0;//nrf通讯失败计数
float target_roll[3]={0,0,-30},p_z=p_z_min,target_roll_attach[3]={0,0,0},p_z_attach=0;//目标
float err_old[3]={0,0,0};//记录前一个的误差值
float I[3]={I0_INIT,I1_INIT,0};//积分值
float kp1=0.004,ki1=8e-5,kd1=0.08,
      kp2=-0.002,ki2=0,kd2=-0.12,
      kp0=0.005,ki0=1e-4,kd0=0.10;//pid参数设置
char rxData[TRANSFER_SIZE];

void _pid_flag(){pid_flag=1;}//定时控制pid

void pid_control(){
    _JY901.receiveData();
    float roll[3],P1,P2,P3,P4,P5,P6;
    _JY901.getAttitude(roll[0], roll[1], roll[2]);
    if(roll[0]>180) roll[0]-=360;
    if(roll[1]>180) roll[1]-=360;
    if(roll[2]>180) roll[2]-=360;//更新数据
    
    
    target_roll_attach[2] = (float)(((int)rxData[1]*256+(int)rxData[2])-32768);
    if(state!=3) p_z_attach =65536.0f-(float)((int)rxData[3]*256+(int)rxData[4]);
    target_roll_attach[1] = (float)(((int)rxData[5]*256+(int)rxData[6])-32768);
    target_roll_attach[0] = (float)(((int)rxData[7]*256+(int)rxData[8])-32768);
    
    target_roll[0]=maxroll*target_roll_attach[0]/32767.0f;
    target_roll[1]=maxpitch*target_roll_attach[1]/32767.0f;
    target_roll[2]=maxyaw*target_roll_attach[2]/32767.0f;
    if(state!=3) p_z=p_z_min+(p_z_attach/65532.0f)*(p_z_max-p_z_min);
    else  p_z+=p_z_attach*1.5e-8;
    
  /*  if (target_roll[0]>maxroll)target_roll[0]=maxroll;
    if (target_roll[0]<-maxroll)target_roll[0]=-maxroll;
    if (target_roll[1]>maxpitch)target_roll[1]=maxpitch;
    if (target_roll[1]<-maxpitch)target_roll[1]=-maxpitch;
    if (target_roll[2]>maxyaw)target_roll[2]=maxyaw;
    if (target_roll[2]<-maxyaw)target_roll[2]=-maxyaw;*/
    if (p_z>maxp_z) p_z=maxp_z;
    if (p_z<-maxp_z) p_z=-maxp_z;//动力限幅
    
    P1=0.5;P2=0.5;P3=0.5;P4=0.5;P5=0.5;P6=0.5;
    float err[3];
    float pi1,pi2,pi0;
    err[0]=target_roll[0]-roll[0];  
    err[1]=target_roll[1]-roll[1];
    err[2]=target_roll[2]-roll[2];
    
    I[0]+=err[0];
    I[1]+=err[1]; 
    I[2]+=err[2];
    
    if(I[0]>0.2/ki0) I[0]=0.2/ki0;
    if(I[1]>0.2/ki1) I[1]=0.2/ki1;
    if(I[2]>0.2/ki2) I[2]=0.2/ki2;
    if(I[0]<-0.2/ki0) I[0]=-0.2/ki0;
    if(I[1]<-0.2/ki1) I[1]=-0.2/ki1;
    if(I[2]<-0.2/ki2) I[2]=-0.2/ki2;//积分项限幅防止长时间积分无法回调
    
    pi0=err[0]*kp0+ki0*I[0]+kd0*(err[0]-err_old[0]);
    pi1=err[1]*kp1+ki1*I[1]+kd1*(err[1]-err_old[1]);
    pi2=err[2]*kp2+ki2*I[2]+kd2*(err[2]-err_old[2]);//计算pid
    
    err_old[1]=err[1];
    err_old[2]=err[2];
    err_old[0]=err[0];
    
    if(pi1>maxpi1) pi1=maxpi1;//pid限幅
    if(pi1<-maxpi1) pi1=-maxpi1;
    if(pi0>maxpi0) pi0=maxpi0;
    if(pi0<-maxpi0) pi0=-maxpi0;
    if(pi2>maxpi2) pi2=maxpi2;
    if(pi2<-maxpi2) pi2=-maxpi2;
     
    P3+=(p_z/3-pi1-pi2);
    P6+=(p_z/3+pi1+pi2);
    P1+=(p_z+pi0-0.5*pi2);
    P2+=(p_z+pi0+0.5*pi2);
    P4+=(p_z+-pi0+0.5*pi2);
    P5+=(p_z+-pi0-0.5*pi2);//计算电机pwm占空比
    
    PWM3=P3;PWM6=P6;
    if(P1<=0.01) PWM1=0.01;else PWM1=P1;
    PWM2=P2;
    PWM4=P4;PWM5=P5;//更新pwm占空比
}

void nrf_restart()
{
    my_nrf24l01p.powerUp();  
    my_nrf24l01p.setTransferSize(TRANSFER_SIZE);
    my_nrf24l01p.setReceiveMode();
    my_nrf24l01p.enable();      
}//nrf重启动

int get_command()
{
   switch (rxData[0]){
       case 0x50:
        return 1;
   
       case 0x52:
        return 2;
   
       case 0x53:
        return 3;  
    }     
    return 0;
}

void state_config()
{
    switch(state)
    {
        case 0://待机状态等待启动命令
        if (get_command()==1) {p_z=p_z_min;p_z_attach=0;state=1;}
        break;
        
        case 1://启动状态，等待缓降(0x53)或急停命令(0x52)
        int u_flag=get_command();
        if (u_flag==3) {state=3;break;}
        
        if (u_flag==2) {state=2;break;}
        
        break;
        
        case 2://急停状态，急停后进入待机状态
            PWM3=0;PWM6=0;PWM4=0;PWM5=0;PWM1=0;PWM2=0;
            state=0;
            target_roll[0]=0;
            target_roll[1]=0;
            target_roll[2]=0;
            target_roll_attach[0]=0;
            target_roll_attach[1]=0;
            target_roll_attach[2]=0;
            I[0]=I0_INIT;
            I[1]=I1_INIT;I[2]=0;
     //       p_z_attach=-2e5;
        break;
        
        case 3://缓降状态，等待急停命令，缓降更改目标为0，匀减速降落。
            if (get_command()==2) state=2;     
            target_roll[0]=0;
            target_roll[1]=0;
            target_roll[2]=0;
            target_roll_attach[0]=0;
            target_roll_attach[1]=0;
            target_roll_attach[2]=0;
            p_z_attach=-2e5;
        break;
    }
}
void pwm_init(){
    //PWM1.period_us(100);
    PWM2.period_ms(1);
    PWM3.period_ms(1);
    PWM4.period_ms(1);    
    PWM5.period_ms(1);
    PWM6.period_ms(1);
    PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;
}

int main() {
    pwm_init(); 
    nrf_restart();
    Ticker time;
    time.attach(&_pid_flag,0.01);
    while(1){
        if(my_nrf24l01p.readable()) {
            my_nrf24l01p.read(NRF24L01P_PIPE_P0,rxData,sizeof(rxData));
            state_config();
            }
            else {
                if (fail_num==fail_max) {nrf_restart();fail_num=0;} 
                else fail_num++;
                }//接收遥控器指令,一定次数没收到指令自动重新启动nrf模块
        switch(state)
        {
            case 0:
            break;
            case 1:
                if (pid_flag) {pid_control();pid_flag=0;}
            break;
            case 3:
                if (pid_flag) {pid_control();pid_flag=0;}
            break;
        }
     }  
}