OpenEdv-开源电子网

 找回密码
 立即注册

扫一扫,访问微社区

正点原子新作:阿波罗STM32F767&F429&探索者STM32F4开发板&赶快来下载资料哦。

查看: 298|回复: 9

PWM高电平被压在0.999毫秒

[复制链接]

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
发表于 7 天前 | 显示全部楼层 |阅读模式
10金钱
花了近1个月的时间写完了飞控代码 准备调PID了 结果发现无论怎么给油门PWM高电平持续时间被压制在了0.999ms 真尼玛恶心
现在束手无策 不知道从哪里找问题 求大神指点迷津
代码量有点大 我分楼发吧

回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 7 天前 | 显示全部楼层
先是I2C与MPU6050
#include <struct_all.h>
#include <I2C_MPU6050.h>

#define SCL_Pin         GPIO_Pin_10
#define SDA_Pin         GPIO_Pin_11

#define        SCL_H           {GPIOB->BSRRH = SCL_Pin;I2C_delay(4);}         //SCL&cedil;&szlig;&micro;&ccedil;&AElig;&frac12;
#define        SCL_L           {GPIOB->BSRRL  = SCL_Pin;I2C_delay(4);}         //SCL&micro;&Iacute;&micro;&ccedil;&AElig;&frac12;
#define        SDA_H           {GPIOB->BSRRH = SDA_Pin;I2C_delay(4);}         //SDA&cedil;&szlig;&micro;&ccedil;&AElig;&frac12;
#define        SDA_L           {GPIOB->BSRRL  = SDA_Pin;I2C_delay(4);}         //SDA&micro;&Iacute;&micro;&ccedil;&AElig;&frac12;
#define        SDA_Read        GPIOB->IDR  & SDA_Pin         //SDA&para;&Aacute;&Ecirc;&yacute;&frac34;&Yacute;


uint8_t        GYRO_Offset = 0;//&sup2;&raquo;×&Ocirc;&para;&macr;&ETH;&pound;&Otilde;&yacute;
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); //&acute;ò&iquest;&ordf;&Iacute;&acirc;&Eacute;èB&micro;&Auml;&Ecirc;±&Ouml;&Oacute;

        GPIO_InitStructure.GPIO_Pin = Debug1_Pin | Debug2_Pin | Debug3_Pin; //&Oacute;&Atilde;&Oacute;&Uacute;&sup2;&acirc;&Aacute;&iquest;&sup3;&Igrave;&ETH;ò&Ocirc;&Euml;&ETH;&ETH;&Euml;&Ugrave;&Acirc;&Ecirc;
        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&Iuml;&szlig;&Icirc;&ordf;&micro;&Iacute;&micro;&ccedil;&AElig;&frac12;&Ocirc;ò×&Uuml;&Iuml;&szlig;&Atilde;&brvbar;,&Iacute;&Euml;&sup3;&ouml;
        SDA_L;
        if(SDA_Read)
                return 0;        //SDA&Iuml;&szlig;&Icirc;&ordf;&cedil;&szlig;&micro;&ccedil;&AElig;&frac12;&Ocirc;ò×&Uuml;&Iuml;&szlig;&sup3;&ouml;&acute;í,&Iacute;&Euml;&sup3;&ouml;
       
        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;        //&ETH;&acute;&Oacute;&brvbar;&acute;&eth;&ETH;&Aring;&ordm;&Aring;
        __nop();
    SCL_H;
        __nop();
    SCL_L;      
}


static void I2C_NACK(void)
{
        SCL_L;                
//        __nop();
        SDA_H;        //&sup2;&raquo;&ETH;&acute;&Oacute;&brvbar;&acute;&eth;&ETH;&Aring;&ordm;&Aring;
//        __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&AElig;&eth;&Ecirc;&frac14;&acute;í&Icirc;ó&pound;&not;·&micro;&raquo;&Oslash;
        I2C_WriteByte(MPU6050Address); //&ETH;&acute;Slave&micro;&Oslash;&Ouml;·&pound;&not;&sup2;&cent;&Aring;&auml;&Ouml;&Atilde;&sup3;&Eacute;&ETH;&acute;&Auml;&pound;&Ecirc;&frac12;
        if( !I2C_WaitAck() )
        {
                I2C_Stop();
                return 0;        //&Icirc;&THORN;ACK&pound;&not;·&micro;&raquo;&Oslash;
        }
        I2C_WriteByte(Regs_Addr);    //&ETH;&acute;&frac14;&Auml;&acute;&aelig;&AElig;÷&micro;&Oslash;&Ouml;·
        I2C_WaitAck();
        I2C_WriteByte(Regs_Data);    //&ETH;&acute;&frac14;&Auml;&acute;&aelig;&AElig;÷&Ecirc;&yacute;&frac34;&Yacute;
        I2C_WaitAck();
        I2C_Stop();         
        return 1;
}

static uint8_t Single_ReadI2C(unsigned char Regs_Addr)
{
        uint8_t ret;

        if( !I2C_Start() )
                return 0;        //I2C&AElig;&eth;&Ecirc;&frac14;&acute;í&Icirc;ó&pound;&not;·&micro;&raquo;&Oslash;
        I2C_WriteByte(MPU6050Address);         //&ETH;&acute;Slave&micro;&Oslash;&Ouml;·&pound;&not;&sup2;&cent;&Aring;&auml;&Ouml;&Atilde;&sup3;&Eacute;&ETH;&acute;&Auml;&pound;&Ecirc;&frac12;
        if( !I2C_WaitAck() )
        {
                I2C_Stop();
                return 0;        //&Icirc;&THORN;ACK&pound;&not;·&micro;&raquo;&Oslash;
        }
       
        I2C_WriteByte(Regs_Addr);            //&ETH;&acute;&frac14;&Auml;&acute;&aelig;&AElig;÷&micro;&Oslash;&Ouml;·
        I2C_WaitAck();
        I2C_Start();
        I2C_WriteByte(MPU6050Address+1);//&ETH;&acute;Slave&micro;&Oslash;&Ouml;·&pound;&not;&sup2;&cent;&Aring;&auml;&Ouml;&Atilde;&sup3;&Eacute;&para;&Aacute;&Auml;&pound;&Ecirc;&frac12;
        I2C_WaitAck();
       
        ret=I2C_ReadByte();                                //&acute;&Oacute;&acute;&laquo;&cedil;&ETH;&AElig;÷&Ouml;&ETH;&para;&Aacute;&sup3;&ouml;&Ecirc;&yacute;&frac34;&Yacute;

        I2C_NACK();                                                //&Icirc;&THORN;&Oacute;&brvbar;&acute;&eth;
        I2C_Stop();                          //&frac12;á&Ecirc;&oslash;±&frac34;&para;&Icirc;IIC&frac12;&oslash;&sup3;&Igrave;
        return ret;       
}


uint8_t MPU6050_SequenceRead(void)
{
        uint8_t index=14;
       
        if( !I2C_Start() )
                return 0;        //I2C&AElig;&eth;&Ecirc;&frac14;&acute;í&Icirc;ó&pound;&not;·&micro;&raquo;&Oslash;
        I2C_WriteByte(MPU6050Address);         //&ETH;&acute;Slave&micro;&Oslash;&Ouml;·&pound;&not;&sup2;&cent;&Aring;&auml;&Ouml;&Atilde;&sup3;&Eacute;&ETH;&acute;&Auml;&pound;&Ecirc;&frac12;
        if( !I2C_WaitAck() )
        {
                I2C_Stop();
                return 0;        //&Icirc;&THORN;ACK&pound;&not;·&micro;&raquo;&Oslash;
        }
        I2C_WriteByte(ACCEL_XOUT_H);    //&ETH;&acute;&frac14;&Auml;&acute;&aelig;&AElig;÷&micro;&Oslash;&Ouml;·
        I2C_WaitAck();
       
        I2C_Start();
        I2C_WriteByte(MPU6050Address+1);//&ETH;&acute;Slave&micro;&Oslash;&Ouml;·&pound;&not;&sup2;&cent;&Aring;&auml;&Ouml;&Atilde;&sup3;&Eacute;&para;&Aacute;&Auml;&pound;&Ecirc;&frac12;
        I2C_WaitAck();

        while(--index)        //&Aacute;&not;&para;&Aacute;13&Icirc;&raquo;&frac14;&Auml;&acute;&aelig;&AElig;÷
        {
                MPU6050_Buffer[13 - index] = I2C_ReadByte();
                I2C_ACK();
        }

        MPU6050_Buffer[13] = I2C_ReadByte();        //&para;&Aacute;&micro;&Uacute;14&frac14;&Auml;&acute;&aelig;&AElig;÷
        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)//&frac14;ì&sup2;éMPU6050&Ecirc;&Ccedil;·&ntilde;&Otilde;&yacute;&sup3;&pound;
        {
                return 0;
        }
       
        Single_WriteI2C(PWR_MGMT_1, 0x00);        //&micro;&ccedil;&Ocirc;&acute;&sup1;&Uuml;&Agrave;í&pound;&not;&micro;&auml;&ETH;&Iacute;&Ouml;&micro;&pound;&ordm;0x00&pound;&not;&Otilde;&yacute;&sup3;&pound;&Auml;&pound;&Ecirc;&frac12;
        I2C_delay(20000); //&Ocirc;&frac14;2.5ms&Ntilde;&Oacute;&Ecirc;±
        Single_WriteI2C(SMPLRT_DIV, 0x00);        //&Iacute;&Oacute;&Acirc;&Yacute;&Ograve;&Ccedil;&sup2;&Eacute;&Ntilde;ù&Acirc;&Ecirc;&pound;&not;&micro;&auml;&ETH;&Iacute;&Ouml;&micro;&pound;&ordm;0x00&pound;&not;&sup2;&raquo;·&Ouml;&AElig;&micro;&pound;¨8KHZ&pound;&copy;
        I2C_delay(20000);            
        Single_WriteI2C(CONFIG2, 0x00);           //&micro;&Iacute;&Iacute;¨&Acirc;&Euml;&sup2;¨&AElig;&micro;&Acirc;&Ecirc;&pound;&not;&micro;&auml;&ETH;&Iacute;&Ouml;&micro;&pound;&ordm;0x00&pound;&not;&sup2;&raquo;&AElig;&ocirc;&Oacute;&Atilde;MPU6050×&Ocirc;&acute;&oslash;&Acirc;&Euml;&sup2;¨
        I2C_delay(20000);
        Single_WriteI2C(GYRO_CONFIG, 0x18);        //&Iacute;&Oacute;&Acirc;&Yacute;&Ograve;&Ccedil;×&Ocirc;&frac14;ì&frac14;°&sup2;&acirc;&Aacute;&iquest;·&para;&Icirc;§&pound;&not;&micro;&auml;&ETH;&Iacute;&Ouml;&micro;&pound;&ordm;0x18(&sup2;&raquo;×&Ocirc;&frac14;ì&pound;&not;2000deg/s)
        I2C_delay(20000);
        Single_WriteI2C(ACCEL_CONFIG, 0x1F);//&frac14;&Oacute;&Euml;&Ugrave;&frac14;&AElig;×&Ocirc;&frac14;ì&iexcl;&cent;&sup2;&acirc;&Aacute;&iquest;·&para;&Icirc;§&frac14;°&cedil;&szlig;&Iacute;¨&Acirc;&Euml;&sup2;¨&AElig;&micro;&Acirc;&Ecirc;&pound;&not;&micro;&auml;&ETH;&Iacute;&Ouml;&micro;&pound;&ordm;0x1F(&sup2;&raquo;×&Ocirc;&frac14;ì&pound;&not;16G)
        I2C_delay(20000);
       
        return 1;
}

void MPU6050_Compose(void)
{
        acc.x  = ((((int16_t)MPU6050_Buffer[0]) << 8) | MPU6050_Buffer[1]) - offset_acc.x;        //&frac14;&otilde;&Egrave;&yen;&Aacute;&atilde;&AElig;&laquo;
        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;
       
}
回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 7 天前 | 显示全部楼层
然后是滤波
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_Num;i++)
        {
                Filter_sum_x += Filter_x[i];
                Filter_sum_y += Filter_y[i];
                Filter_sum_z += Filter_z[i];
        }       
       
        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;
}


回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 7 天前 | 显示全部楼层
角速度转弧度制
#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;
}
回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 7 天前 | 显示全部楼层
正点原子公众号
四元数转姿态角
#include <IMU.h>
#include <math.h>
#include <Maths.h>
#include <I2C_MPU6050.h>

#define Kp         1.0f    // ±&Egrave;&Agrave;&yacute;&sup3;&pound;&Ecirc;&yacute;
#define Ki         0.001f  // &raquo;&yacute;·&Ouml;&sup3;&pound;&Ecirc;&yacute;
#define halfT 0.0005f//°&euml;&Ouml;&Uuml;&AElig;&Uacute;
#define T        0.001f  // &Ouml;&Uuml;&AElig;&Uacute;&Icirc;&ordf;1ms

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;             // &Euml;&Auml;&Ocirc;&ordf;&Ecirc;&yacute;
float exInt = 0, eyInt = 0, ezInt = 0;            // &Icirc;ó&sup2;&icirc;&raquo;&yacute;·&Ouml;&Agrave;&Ucirc;&frac14;&AElig;&Ouml;&micro;

void IMUpdate(float gx, float gy, float gz, float ax, float ay, float az)
{
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;
       
  //&Euml;&Auml;&Ocirc;&ordf;&Ecirc;&yacute;&raquo;&yacute;·&Ouml;&pound;&not;&Ccedil;ó&micro;&Atilde;&micro;±&Ccedil;°&micro;&Auml;×&Euml;&Igrave;&not;
        float q0_last = q0;       
        float q1_last = q1;       
        float q2_last = q2;       
        float q3_last = q3;       

        //°&Ntilde;&frac14;&Oacute;&Euml;&Ugrave;&para;&Egrave;&frac14;&AElig;&micro;&Auml;&Egrave;&yacute;&Icirc;&not;&Iuml;ò&Aacute;&iquest;×&ordf;&sup3;&Eacute;&micro;&yen;&Icirc;&raquo;&Iuml;ò&Aacute;&iquest;
        norm = invSqrt(ax*ax + ay*ay + az*az);
        ax = ax * norm;
        ay = ay * norm;
        az = az * norm;

        //&sup1;&Agrave;&frac14;&AElig;&Ouml;&Oslash;&Aacute;&brvbar;&frac14;&Oacute;&Euml;&Ugrave;&para;&Egrave;·&frac12;&Iuml;ò&Ocirc;&Uacute;·&Eacute;&ETH;&ETH;&AElig;÷×&oslash;±ê&Iuml;&micro;&Ouml;&ETH;&micro;&Auml;±í&Ecirc;&frac34;&pound;&not;&Icirc;&ordf;&Euml;&Auml;&Ocirc;&ordf;&Ecirc;&yacute;±í&Ecirc;&frac34;&micro;&Auml;&ETH;&yacute;×&ordf;&frac34;&Oslash;&Otilde;ó&micro;&Auml;&micro;&Uacute;&Egrave;&yacute;&ETH;&ETH;
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

        //&frac14;&Oacute;&Euml;&Ugrave;&para;&Egrave;&frac14;&AElig;&para;&Aacute;&Egrave;&iexcl;&micro;&Auml;·&frac12;&Iuml;ò&Oacute;&euml;&Ouml;&Oslash;&Aacute;&brvbar;&frac14;&Oacute;&Euml;&Ugrave;&para;&Egrave;·&frac12;&Iuml;ò&micro;&Auml;&sup2;&icirc;&Ouml;&micro;&pound;&not;&Oacute;&Atilde;&Iuml;ò&Aacute;&iquest;&sup2;&aelig;&sup3;&Euml;&frac14;&AElig;&Euml;&atilde;
        ex = ay*vz - az*vy;
        ey = az*vx - ax*vz;
        ez = ax*vy - ay*vx;

        //&Icirc;ó&sup2;&icirc;&Agrave;&Ucirc;&raquo;&yacute;&pound;&not;&Ograve;&Ntilde;&Oacute;&euml;&raquo;&yacute;·&Ouml;&sup3;&pound;&Ecirc;&yacute;&Iuml;à&sup3;&Euml;
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;

        //&Oacute;&Atilde;&sup2;&aelig;&raquo;&yacute;&Icirc;ó&sup2;&icirc;&Agrave;&acute;×&oumlI&ETH;&THORN;&Otilde;&yacute;&Iacute;&Oacute;&Acirc;&Yacute;&Aacute;&atilde;&AElig;&laquo;&pound;&not;&frac14;&acute;&micro;&Ouml;&Iuml;&ucirc;&Iacute;&Oacute;&Acirc;&Yacute;&para;&Aacute;&Ecirc;&yacute;&Ouml;&ETH;&micro;&Auml;&AElig;&laquo;&Ograve;&AElig;&Aacute;&iquest;       
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;


        //&Ograve;&raquo;&frac12;×&frac12;ü&Euml;&AElig;&Euml;&atilde;·¨
        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;
                       
        //&Euml;&Auml;&Ocirc;&ordf;&Ecirc;&yacute;&sup1;&aelig;·&para;&raquo;&macr;
        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;
}

回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 7 天前 | 显示全部楼层
串级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);                             
}

回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 7 天前 | 显示全部楼层
主函数
#include <stm32f4xx.h>
#include <Struct_All.h>
#include <drivers_init.h>
#include <Filter.h>
#include <Maths.h>
#include <IMU.h>
#include <Control.h>
#include <Timer3.h>
#include <delay.h>
#include <NVIC.h>

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);
                }
        }
  
}
回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 7 天前 | 显示全部楼层
PWM部分和NVIC分组绝对正确 这里就不贴了
这是完全自动飞行的代码 不含遥控器的
回复

使用道具 举报

  在线 

492

主题

9万

帖子

30

精华

管理员

Rank: 12Rank: 12Rank: 12

积分
126514
金钱
126514
注册时间
2010-12-1
在线时间
1092 小时
发表于 6 天前 | 显示全部楼层
帮顶
回复

使用道具 举报

  离线 

7

主题

35

帖子

0

精华

初级会员

Rank: 2

积分
66
金钱
66
注册时间
2018-4-21
在线时间
11 小时
 楼主| 发表于 5 天前 | 显示全部楼层
来个大神帮帮忙啊
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则




QQ|联系我们|手机版|官方淘宝店|新浪微博|微信公众平台|OpenEdv-开源电子网 ( 粤ICP备12000418号-1 )

GMT+8, 2018-5-24 01:03

Powered by OpenEdv-开源电子网

© 2001-2030 OpenEdv-开源电子网

快速回复 返回顶部 返回列表
/* */