您当前的位置:五五电子网电子知识电子学习基础知识电脑-单片机-自动控制四轴飞行器姿态控制算法注释分析 正文
四轴飞行器姿态控制算法注释分析

四轴飞行器姿态控制算法注释分析

点击数:7899 次   录入时间:03-04 11:54:00   整理:http://www.55dianzi.com   电脑-单片机-自动控制

    tmp_long /= 3;

    tmp_long2 /= 3;

    }

    /*当偏航轴的操纵杆输入较大时候,则说明这时候偏航轴有一个角速度,为了消除有害加速度的影响,必须对这

    两个数值再做一次衰减*/

    if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]]) > 25)

    {

    tmp_long /= 3;

    tmp_long2 /= 3;

    }

    /*做一个限制,补偿值必须在一定的范围内。将补偿的范围限制在+-32*/

    #define AUSGLEICH 32

    if(tmp_long > AUSGLEICH)

    tmp_long = AUSGLEICH;

    if(tmp_long < -AUSGLEICH)

    tmp_long =-AUSGLEICH;

    if(tmp_long2 > AUSGLEICH)

    tmp_long2 = AUSGLEICH;

    if(tmp_long2 <-AUSGLEICH)

    tmp_long2 =-AUSGLEICH;

    /*将补偿值考虑进去,这时候Mess_IntegralNick补偿了,Mess_IntegralNick2没有补偿,因为在后面还要用到

    */

    Mess_IntegralNick -= tmp_long;

    Mess_IntegralRoll -= tmp_long2;

    } // end if of if(!Looping_Nick && !Looping_Roll)

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    /*当 >ABGLEICH_ANZAHL(256)时候 说明测量了256次航向*/

    /*变量ZaehlMessungen是在AD检测的函数中改变的,也就是说,下面这个if语句是每256个检测周期计算一次,

    而不是控制周期,检测周期要高于控制周期*/

    if(ZaehlMessungen >= ABGLEICH_ANZAHL)//关于时间积累的处理过程

    {

    static int cnt = 0;

    static char last_n_p,last_n_n,last_r_p,last_r_n;

    static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;

    if(!Looping_Nick && !Looping_Roll)

    {

    MittelIntegralNick /= ABGLEICH_ANZAHL;

    MittelIntegralRoll /= ABGLEICH_ANZAHL;

    /*计算加速度计积分的作用,在不运动时候,xy加速度计的积分应该是0,所以xy积

    分而z不积分*/

    IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) /

    ABGLEICH_ANZAHL;

    IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) /

    ABGLEICH_ANZAHL;

    IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;

    #define MAX_I 0//(Poti2/10)

    // Nick ++++++++++++++++++++++++++++++++++++++++++++++++

    /*不考虑补偿的陀螺仪积分的积分-加速度计积分/平衡项*/

    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);

    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;

    // Roll ++++++++++++++++++++++++++++++++++++++++++++++++

    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);

    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;

    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;

    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;

    if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in

    [EE_Parameter.Kanalbele×g[K_GIER]]) > 25))

    {

    /*这个参数在后面的程序中还要进行修正,修正后的值加入到陀螺仪的积分

    中,可以认为这个参数是系统对于陀螺仪漂移的估计*/

    LageKorrekturNick /= 2;

    LageKorrekturRoll /= 2;

    }

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // Gyro-Drift ermitteln//陀螺漂移的确定

    /*对陀螺仪漂移的估计过程*/

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    /*前面MittelIntegralNick已经用过了,因此这里使用MittelIntegralNick2*/

    MittelIntegralNick2 /= ABGLEICH_ANZAHL;

    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;

    /*有校正和没有校正的陀螺仪积分做差,即陀螺仪的漂移*/

    /*IntegralNick2是没有校正的陀螺仪积分 IntegralNick是有校正的陀螺仪积分 这里的校正指的是使用加速度

    计积分进行的校正*/

    tmp_long = IntegralNick2 - IntegralNick;

    tmp_long2 = IntegralRoll2 - IntegralRoll;

    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;

    /*将差值加入到Mess_IntegralNick2和Mess_IntegralRoll2中 这时Mess_IntegralNick2和Mess_IntegralRoll2

    被使用*/

    IntegralFehlerNick = tmp_long;

    IntegralFehlerRoll = tmp_long2;

    /*下面两个公式的作用就是让Mess_IntegralNick2=Mess_IntegralNick,

    Mess_IntegralRoll2=Mess_IntegralRoll为下一个计算周期做准备*/

    Mess_IntegralNick2 -= IntegralFehlerNick;

    Mess_IntegralRoll2 -= IntegralFehlerRoll;

    // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;

    // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;

    DebugOut.Analog[17] = IntegralAccNick / 26;

    DebugOut.Analog[18] = IntegralAccRoll / 26;

    DebugOut.Analog[19] = IntegralFehlerNick;// / 26;

    DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;

    DebugOut.Analog[21] = MittelIntegralNick / 26;

    DebugOut.Analog[22] = MittelIntegralRoll / 26;

    //DebugOut.Analog[28] = ausgleichNick;

    DebugOut.Analog[29] = ausgleichRoll;

    DebugOut.Analog[30] = LageKorrekturRoll * 10;

    #define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)

    #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)

    #define BEWE×GS_LIMIT 20000

    // Nick +++++++++++++++++++++++++++++++++++++++++++++++++

    /*以下部分就是对LageKorrekturNick的修正和对陀螺仪常值误差的修正*/

    cnt = 1;// + labs(IntegralFehlerNick) / 4096;

    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWE×GS_LIMIT)

    {

    if(IntegralFehlerNick > FEHLER_LIMIT2)

    {

    /*必须连续两次的误差都很大,才能进入下面的if语句*/

    if(last_n_p)

    {

    /*连续两次误差较大时,对陀螺仪漂移进行补偿*/

    /*最后改变了LageKorrekturNick的值*/

    cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;

    ausgleichNick = IntegralFehlerNick / 8;

    if(ausgleichNick > 5000)

    ausgleichNick = 5000;

    LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;

    }

    else

    last_n_p = 1;

    }

    else

    last_n_p = 0;

    if(IntegralFehlerNick < -FEHLER_LIMIT2)

    {

    if(last_n_n)

    {

    cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;

    ausgleichNick = IntegralFehlerNick / 8;

    if(ausgleichNick < -5000) ausgleichNick = -5000;

    LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;

    }

    else

    last_n_n = 1;

    }

    else

    last_n_n = 0;

上一页  [1] [2] [3] [4] [5] [6] [7] [8] [9] [10]  下一页


本文关键字:飞行器  电脑-单片机-自动控制电子学习 - 基础知识 - 电脑-单片机-自动控制