为什么不管怎么给油门PWM高电平持续时间都被压制在了0.999ms？

先是I2C与MPU6050
#include
#include
#define SCL_Pin         GPIO_Pin_10
#define SDA_Pin         GPIO_Pin_11
#define        SCL_H           {GPIOB->BSRRH = SCL_Pin;I2C_delay(4);}         //SCL¸ßµçƽ
#define        SCL_L           {GPIOB->BSRRL  = SCL_Pin;I2C_delay(4);}         //SCLµÍµçƽ
#define        SDA_H           {GPIOB->BSRRH = SDA_Pin;I2C_delay(4);}         //SDA¸ßµçƽ
#define        SDA_L           {GPIOB->BSRRL  = SDA_Pin;I2C_delay(4);}         //SDAµÍµçƽ
#define        SDA_Read        GPIOB->IDR  & SDA_Pin         //SDA¶ÁÊý¾Ý
uint8_t        GYRO_Offset = 0;//²»×Ô¶¯Ð£Õý
uint8_t        ACC_Offset  = 0;
uint32_t I2C_Erro=0;
static uint8_t        MPU6050_Buffer[14];
void I2C2_Init(void)
{
        GPIO_InitTypeDef GPIO_InitStructure;
        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB , ENABLE); //´ò¿ªÍâÉèBµÄʱÖÓ
        GPIO_InitStructure.GPIO_Pin = Debug1_Pin | Debug2_Pin | Debug3_Pin; //ÓÃÓÚ²âÁ¿³ÌÐòÔËÐÐËÙÂÊ
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
        GPIO_InitStructure.GPIO_OType= GPIO_OType_PP;
        GPIO_Init(GPIOB, &GPIO_InitStructure);               
        GPIO_InitStructure.GPIO_Pin = SCL_Pin; //SCL
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
        GPIO_InitStructure.GPIO_OType= GPIO_OType_OD;
        GPIO_Init(GPIOB, &GPIO_InitStructure);       
        GPIO_InitStructure.GPIO_Pin = SDA_Pin; //SDA
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
        GPIO_InitStructure.GPIO_OType= GPIO_OType_OD;
        GPIO_Init(GPIOB, &GPIO_InitStructure);       
       
}
static void I2C_delay(int delay)
{
    while (delay)
        delay--;
}
static uint8_t I2C_Start(void)       
{
        SCL_L;                                       
        SDA_H;
        SCL_H;
       
        if(!SDA_Read)
                return 0;        //SDAÏßΪµÍµçƽÔò×ÜÏßæ,Í˳ö
        SDA_L;
        if(SDA_Read)
                return 0;        //SDAÏßΪ¸ßµçƽÔò×ÜÏß³ö´í,Í˳ö
       
        SDA_L;
        return 1;
}
static void I2C_Stop(void)
{
        SCL_L;                                       
        SDA_L;
        SCL_H;
        SDA_H;
}
static void I2C_ACK(void)
{
        SCL_L;        
        __nop();
        SDA_L;        //дӦ´ðÐźÅ
        __nop();
    SCL_H;
        __nop();
    SCL_L;      
}
static void I2C_NACK(void)
{
        SCL_L;                
//        __nop();
        SDA_H;        //²»Ð´Ó¦´ðÐźÅ
//        __nop();
    SCL_H;
//        __nop();
    SCL_L;  
}
static uint8_t I2C_WaitAck(void)
{
        SCL_L;                               
        SDA_H;
        SCL_H;
               
        if(SDA_Read)
        {
                SCL_L;
                return 0;       
        }
       
        SCL_L;
        return 1;          
}
static void I2C_WriteByte(unsigned char SendByte)
{
        uint8_t i=8;
       
        while(i--)
        {
                SCL_L;
                if( SendByte&0x80 )
                        SDA_H
                else
                        SDA_L
                SendByte <<= 1;
                SCL_H;
  }   
        SCL_L;  
}
static uint8_t I2C_ReadByte(void)
{
        uint8_t i=8;
        uint8_t ReceiveByte =0;
       
        SDA_H;
        while(i--)
        {
                SCL_L;  
                __nop();
                SCL_H;                
                ReceiveByte <<= 1;
                if(SDA_Read)
                        ReceiveByte |= 0x01;
        }
        SCL_L;
        return ReceiveByte;
}
static uint8_t Single_WriteI2C(unsigned char Regs_Addr,unsigned char Regs_Data)
{  
        if( !I2C_Start() )
                return 0;        //I2CÆðʼ´íÎ󣬷µ»Ø
        I2C_WriteByte(MPU6050Address); //дSlaveµØÖ·£¬²¢ÅäÖóÉдģʽ
        if( !I2C_WaitAck() )
        {
                I2C_Stop();
                return 0;        //ÎÞACK£¬·µ»Ø
        }
        I2C_WriteByte(Regs_Addr);    //д¼Ä´æÆ÷µØÖ·
        I2C_WaitAck();
        I2C_WriteByte(Regs_Data);    //д¼Ä´æÆ÷Êý¾Ý
        I2C_WaitAck();
        I2C_Stop();         
        return 1;
}
static uint8_t Single_ReadI2C(unsigned char Regs_Addr)
{
        uint8_t ret;
        if( !I2C_Start() )
                return 0;        //I2CÆðʼ´íÎ󣬷µ»Ø
        I2C_WriteByte(MPU6050Address);         //дSlaveµØÖ·£¬²¢ÅäÖóÉдģʽ
        if( !I2C_WaitAck() )
        {
                I2C_Stop();
                return 0;        //ÎÞACK£¬·µ»Ø
        }
       
        I2C_WriteByte(Regs_Addr);            //д¼Ä´æÆ÷µØÖ·
        I2C_WaitAck();
        I2C_Start();
        I2C_WriteByte(MPU6050Address+1);//дSlaveµØÖ·£¬²¢ÅäÖóɶÁģʽ
        I2C_WaitAck();
       
        ret=I2C_ReadByte();                                //´Ó´«¸ÐÆ÷ÖжÁ³öÊý¾Ý
        I2C_NACK();                                                //ÎÞÓ¦´ð
        I2C_Stop();                          //½áÊø±¾¶ÎIIC½ø³Ì
        return ret;       
}
uint8_t MPU6050_SequenceRead(void)
{
        uint8_t index=14;
       
        if( !I2C_Start() )
                return 0;        //I2CÆðʼ´íÎ󣬷µ»Ø
        I2C_WriteByte(MPU6050Address);         //дSlaveµØÖ·£¬²¢ÅäÖóÉдģʽ
        if( !I2C_WaitAck() )
        {
                I2C_Stop();
                return 0;        //ÎÞACK£¬·µ»Ø
        }
        I2C_WriteByte(ACCEL_XOUT_H);    //д¼Ä´æÆ÷µØÖ·
        I2C_WaitAck();
       
        I2C_Start();
        I2C_WriteByte(MPU6050Address+1);//дSlaveµØÖ·£¬²¢ÅäÖóɶÁģʽ
        I2C_WaitAck();
        while(--index)        //Á¬¶Á13λ¼Ä´æÆ÷
        {
                MPU6050_Buffer[13 - index] = I2C_ReadByte();
                I2C_ACK();
        }
        MPU6050_Buffer[13] = I2C_ReadByte();        //¶ÁµÚ14¼Ä´æÆ÷
        I2C_NACK();       
        I2C_Stop();       
       
        return 1;
}
void MPU6050_SingleRead(void)
{
                MPU6050_Buffer[0] = Single_ReadI2C(ACCEL_XOUT_H);
                MPU6050_Buffer[1] = Single_ReadI2C(ACCEL_XOUT_L);               
                MPU6050_Buffer[2] = Single_ReadI2C(ACCEL_YOUT_H);       
                MPU6050_Buffer[3] = Single_ReadI2C(ACCEL_YOUT_L);
                MPU6050_Buffer[4] = Single_ReadI2C(ACCEL_ZOUT_H);
                MPU6050_Buffer[5] = Single_ReadI2C(ACCEL_ZOUT_L);
               
                MPU6050_Buffer[8]  = Single_ReadI2C(GYRO_XOUT_H);
                MPU6050_Buffer[9]  = Single_ReadI2C(GYRO_XOUT_L);
                MPU6050_Buffer[10] = Single_ReadI2C(GYRO_YOUT_H);
                MPU6050_Buffer[11] = Single_ReadI2C(GYRO_YOUT_L);
                MPU6050_Buffer[12] = Single_ReadI2C(GYRO_ZOUT_H);
                MPU6050_Buffer[13] = Single_ReadI2C(GYRO_ZOUT_L);
}
uint8_t InitMPU6050(void)
{
        if( Single_ReadI2C(WHO_AM_I) != 0x68)//¼ì²éMPU6050ÊÇ·ñÕý³£
        {
                return 0;
        }
       
        Single_WriteI2C(PWR_MGMT_1, 0x00);        //µçÔ´¹ÜÀí£¬µäÐÍÖµ£º0x00£¬Õý³£Ä£Ê½
        I2C_delay(20000); //Ô¼2.5msÑÓʱ
        Single_WriteI2C(SMPLRT_DIV, 0x00);        //ÍÓÂÝÒDzÉÑùÂÊ£¬µäÐÍÖµ£º0x00£¬²»·ÖƵ£¨8KHZ£©
        I2C_delay(20000);            
        Single_WriteI2C(CONFIG2, 0x00);           //µÍͨÂ˲¨ÆµÂÊ£¬µäÐÍÖµ£º0x00£¬²»ÆôÓÃMPU6050×Ô´øÂ˲¨
        I2C_delay(20000);
        Single_WriteI2C(GYRO_CONFIG, 0x18);        //ÍÓÂÝÒÇ×Լ켰²âÁ¿·¶Î§£¬µäÐÍÖµ£º0x18(²»×Լ죬2000deg/s)
        I2C_delay(20000);
        Single_WriteI2C(ACCEL_CONFIG, 0x1F);//¼ÓËÙ¼Æ×Լ졢²âÁ¿·¶Î§¼°¸ßͨÂ˲¨ÆµÂÊ£¬µäÐÍÖµ£º0x1F(²»×Լ죬16G)
        I2C_delay(20000);
       
        return 1;
}
void MPU6050_Compose(void)
{
        acc.x  = ((((int16_t)MPU6050_Buffer[0]) << 8) | MPU6050_Buffer[1]) - offset_acc.x;        //¼õÈ¥ÁãÆ«
        acc.y  = ((((int16_t)MPU6050_Buffer[2]) << 8) | MPU6050_Buffer[3]) - offset_acc.y;
        acc.z  = ((((int16_t)MPU6050_Buffer[4]) << 8) | MPU6050_Buffer[5]) - offset_acc.z;
        gyro.x = ((((int16_t)MPU6050_Buffer[8])  << 8) | MPU6050_Buffer[9])  - offset_gyro.x;
        gyro.y = ((((int16_t)MPU6050_Buffer[10]) << 8) | MPU6050_Buffer[11]) - offset_gyro.y;
        gyro.z = ((((int16_t)MPU6050_Buffer[12]) << 8) | MPU6050_Buffer[13]) - offset_gyro.z;
       
}

然后是滤波
static float ACC_IIR_FACTOR;
void Calculate_FilteringCoefficient(float Time, float Cut_Off)
{
        ACC_IIR_FACTOR = Time /( Time + 1/(2.0f*Pi*Cut_Off) );
}
void ACC_IIR_Filter(struct _acc *Acc_in,struct _acc *Acc_out)
{
        Acc_out->x = Acc_out->x + ACC_IIR_FACTOR*(Acc_in->x - Acc_out->x);
        Acc_out->y = Acc_out->y + ACC_IIR_FACTOR*(Acc_in->y - Acc_out->y);
        Acc_out->z = Acc_out->z + ACC_IIR_FACTOR*(Acc_in->z - Acc_out->z);
}
#define Filter_Num 2
void Gyro_Filter(struct _gyro *Gyro_in,struct _gyro *Gyro_out)
{
        static int16_t Filter_x[Filter_Num],Filter_y[Filter_Num],Filter_z[Filter_Num];
        static uint8_t Filter_count;
        int32_t Filter_sum_x=0,Filter_sum_y=0,Filter_sum_z=0;
        uint8_t i=0;
       
        Filter_x[Filter_count] = Gyro_in->x;
        Filter_y[Filter_count] = Gyro_in->y;
        Filter_z[Filter_count] = Gyro_in->z;
        for(i=0;i         {
                Filter_sum_x += Filter_x;
                Filter_sum_y += Filter_y;
                Filter_sum_z += Filter_z;
        }       
       
        Gyro_out->x = Filter_sum_x / Filter_Num;
        Gyro_out->y = Filter_sum_y / Filter_Num;
        Gyro_out->z = Filter_sum_z / Filter_Num;
       
        Filter_count++;
        if(Filter_count == Filter_Num)
                Filter_count=0;
}

角速度转弧度制
#include "Maths.h"
#include "struct_all.h"
void Get_Radian(struct _gyro *Gyro_in,struct _SI_float *Gyro_out)
{
        Gyro_out->x = (float)(Gyro_in->x * RawData_to_Radian);
        Gyro_out->y = (float)(Gyro_in->y * RawData_to_Radian);
        Gyro_out->z = (float)(Gyro_in->z * RawData_to_Radian);
}
float invSqrt(float x)
{
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i>>1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  return y;
}

四元数转姿态角
#include
#include
#include
#include
#define Kp         1.0f    // ±ÈÀý³£Êý
#define Ki         0.001f  // »ý·Ö³£Êý
#define halfT 0.0005f//°ëÖÜÆÚ
#define T        0.001f  // ÖÜÆÚΪ1ms
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;             // ËÄÔªÊý
float exInt = 0, eyInt = 0, ezInt = 0;            // Îó²î»ý·ÖÀÛ¼ÆÖµ
void IMUpdate(float gx, float gy, float gz, float ax, float ay, float az)
{
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;
       
  //ËÄÔªÊý»ý·Ö£¬ÇóµÃµ±Ç°µÄ×Ë̬
        float q0_last = q0;       
        float q1_last = q1;       
        float q2_last = q2;       
        float q3_last = q3;       
        //°Ñ¼ÓËٶȼƵÄÈýάÏòÁ¿×ª³Éµ¥Î»ÏòÁ¿
        norm = invSqrt(ax*ax + ay*ay + az*az);
        ax = ax * norm;
        ay = ay * norm;
        az = az * norm;
        //¹À¼ÆÖØÁ¦¼ÓËٶȷ½ÏòÔÚ·ÉÐÐÆ÷×ø±êϵÖеıíʾ£¬ÎªËÄÔªÊý±íʾµÄÐýת¾ØÕóµÄµÚÈýÐÐ
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
        //¼ÓËٶȼƶÁÈ¡µÄ·½ÏòÓëÖØÁ¦¼ÓËٶȷ½ÏòµÄ²îÖµ£¬ÓÃÏòÁ¿²æ³Ë¼ÆËã
        ex = ay*vz - az*vy;
        ey = az*vx - ax*vz;
        ez = ax*vy - ay*vx;
        //Îó²îÀÛ»ý£¬ÒÑÓë»ý·Ö³£ÊýÏà³Ë
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;
        //Óòæ»ýÎó²îÀ´×öIÐÞÕýÍÓÂÝÁãÆ«£¬¼´µÖÏûÍÓÂݶÁÊýÖеÄÆ«ÒÆÁ¿       
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;
        //Ò»½×½üËÆËã·¨
        q0 = q0_last + (-q1_last*gx - q2_last*gy - q3_last*gz)*halfT;
        q1 = q1_last + ( q0_last*gx + q2_last*gz - q3_last*gy)*halfT;
        q2 = q2_last + ( q0_last*gy - q1_last*gz + q3_last*gx)*halfT;
        q3 = q3_last + ( q0_last*gz + q1_last*gy - q2_last*gx)*halfT;
                       
        //ËÄÔªÊý¹æ·¶»¯
        norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 * norm;
        q1 = q1 * norm;
        q2 = q2 * norm;
        q3 = q3 * norm;
       
        out_angle.yaw  +=  filter_gyro.z * RawData_to_Angle * 0.001f;
}
void Get_Eulerian_Angle(struct _out_angle *angle)
{       
        angle->pitch = -atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3)*Radian_to_Angle;
        angle->roll  =  asin (2.0f*(q0*q2 - q1*q3))*Radian_to_Angle;
}

串级PID
#include "Maths.h"
#include "Control.h"
#include "struct_all.h"
float thr=0;
float Pitch_i,Roll_i,Yaw_i;              
float Pitch_old,Roll_old,Yaw_old;        
float Pitch_d,Roll_d,Yaw_d;              
float RC_Pitch=0,RC_Roll=0,RC_Yaw=0;           
float Pitch_shell_out,Roll_shell_out,Yaw_shell_out;
  
float Pitch_shell_kp=140;
float Pitch_shell_ki=0.2;
float Pitch_shell_kd=0.8;
float Roll_shell_kp=150;              
float Roll_shell_ki=0.2;
float Roll_shell_kd=0.8;
      
float Yaw_shell_kp=15;//0.87              
float Yaw_shell_ki=0.002;
float Yaw_shell_kd=0.02;   
float Gyro_radian_old_x,Gyro_radian_old_y,Gyro_radian_old_z;
float pitch_core_kp_out,pitch_core_ki_out,pitch_core_kd_out,Roll_core_kp_out,
                        Roll_core_ki_out,Roll_core_kd_out,Roll_core_ki_out,Yaw_core_kp_out,
                        Yaw_core_ki_out,Yaw_core_kd_out;
float Pitch_core_out,Roll_core_out,Yaw_core_out;      
float Pitch_core_kp=0.02;
float Pitch_core_ki=0;
float Pitch_core_kd=0.001;//0.001
float Roll_core_kp=0.02;
float Roll_core_ki=0;
float Roll_core_kd=0.001;
float Yaw_core_kp=0.001;
float Yaw_core_ki=0;
float Yaw_core_kd=0.001;
int16_t moto1=1613,moto2=1613,moto3=1613,moto4=1613;
void CONTROL_PID(struct _out_angle *angle)
{
  RC_Pitch=(300000 -1499.0f )/50;
  RC_Pitch-=3.3f;
  Pitch_i+=(angle->pitch-RC_Pitch);
  if(Pitch_i>300) Pitch_i=300;
  else if(Pitch_i<-300) Pitch_i=-300;
  Pitch_d=angle->pitch-Pitch_old;
  Pitch_shell_out = Pitch_shell_kp*(angle->pitch-RC_Pitch) + Pitch_shell_ki*Pitch_i + Pitch_shell_kd*Pitch_d;
  Pitch_old=angle->pitch;
     
  RC_Roll=(300000-1502)/50;
  Roll_i+=(angle->roll-RC_Roll);
  if(Roll_i>300) Roll_i=300;
  else if(Roll_i<-300) Roll_i=-300;
  Roll_d=angle->roll-Roll_old;
  Roll_shell_out  = Roll_shell_kp*(angle->roll-RC_Roll) + Roll_shell_ki*Roll_i + Roll_shell_kd*Roll_d;
  Roll_old=angle->roll;
  Yaw_d=SI_gyro.z-Yaw_old;
  Yaw_shell_out  = Yaw_shell_kp*(SI_gyro.z-RC_Yaw) + Yaw_shell_ki*Yaw_i + Yaw_shell_kd*Yaw_d;
  Yaw_old=SI_gyro.z;
  pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + SI_gyro.y );
  pitch_core_kd_out = Pitch_core_kd * (SI_gyro.y   - Gyro_radian_old_y);
  Roll_core_kp_out  = Roll_core_kp  * (Roll_shell_out  + SI_gyro.x );
  Roll_core_kd_out  = Roll_core_kd  * (SI_gyro.x   - Gyro_radian_old_x);
  Yaw_core_kp_out  = Yaw_core_kp  * (Yaw_shell_out  + SI_gyro.z);
  Yaw_core_kd_out  = Yaw_core_kd  * (SI_gyro.z   - Gyro_radian_old_z);
  Pitch_core_out = pitch_core_kp_out + pitch_core_kd_out;
  Roll_core_out  = Roll_core_kp_out  + Roll_core_kd_out;
  Yaw_core_out   = Yaw_core_kp_out   + Yaw_core_kd_out;
  Gyro_radian_old_y = SI_gyro.y;
  Gyro_radian_old_x = SI_gyro.x;
  Gyro_radian_old_z = SI_gyro.z;   
  thr=1300;               
  moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out- Yaw_core_out);
  moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out+ Yaw_core_out);      
  moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out- Yaw_core_out);
  moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out+ Yaw_core_out);  
                                                
  Motor_Out(moto1,moto2,moto3,moto4);                             
}

主函数
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
int main()
{
        delay_init(72);
        Nvic_Init();
        drivers_init();
        while(1)
        {
                if(Count_1ms>=1)
                {
                        Count_1ms=0;
                        MPU6050_SingleRead();
                        MPU6050_Compose();
                        ACC_IIR_Filter(&acc,&filter_acc);
                        Gyro_Filter(&gyro,&filter_gyro);
                        Get_Radian(&filter_gyro,&SI_gyro);
                        IMUpdate(SI_gyro.x,SI_gyro.y,SI_gyro.z,filter_acc.x,filter_acc.y,filter_acc.z);
                }               
                if(Count_4ms>=4)
                {
                    Count_4ms=0;
                    Get_Eulerian_Angle(&out_angle);
        CONTROL_PID(&out_angle);
                }
        }
  
}

PWM部分和NVIC分组绝对正确 这里就不贴了
这是完全自动飞行的代码 不含遥控器的

帮顶

来个大神帮帮忙啊

RC_Pitch=(300000 -1499.0f )/50;
if(Roll_i>300) Roll_i=300;感觉这两句把控制量就限制在300了。