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

    }

    else cnt = 0;

    if(cnt > EE_Parameter.Driftkomp)

    cnt = EE_Parameter.Driftkomp;

    /*在飞行器飞行的过程中，如果发现陀螺仪的中立点发生变化，则仍然进行修正*/

    /*误差过大时候，改变陀螺仪的常值误差，每次最多改变EE_Parameter.Driftkomp*/

    if(IntegralFehlerNick > FEHLER_LIMIT)

    AdNeutralNick += cnt;

    if(IntegralFehlerNick < -FEHLER_LIMIT)

    AdNeutralNick -= cnt;

    DebugOut.Analog[27] = ausgleichRoll;

    DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);

    DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);

    } // 整个的融合过程结束

    else

    {

    LageKorrekturRoll = 0;

    LageKorrekturNick = 0;

    }

    /*如果IntegralFaktor为零，也就是没有使用陀螺仪积分对电机输出进行修正，则不使用LageKorrekturRoll，

    也就是不进行陀螺仪漂移的补偿*/

    /*在Heading_hold标志位被置位的情况下*/

    if(!IntegralFaktor)

    {

    LageKorrekturRoll = 0;

    LageKorrekturNick = 0;

    } // z.B. bei HH

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

    /*将上一次的值储存下来*/

    MittelIntegralNick_Alt = MittelIntegralNick;

    MittelIntegralRoll_Alt = MittelIntegralRoll;

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

    /*数据清零 加速度计积分每一次都进行清零*/

    IntegralAccNick = 0;

    IntegralAccRoll = 0;

    IntegralAccZ = 0;

    MittelIntegralNick = 0;

    MittelIntegralRoll = 0;

    MittelIntegralNick2 = 0;

    MittelIntegralRoll2 = 0;

    ZaehlMessungen = 0;

    } //end if of if(ZaehlMessungen >= ABGLEICH_ANZAHL)

    //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);

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

    // Gieren//偏航

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

    if(abs(StickGier) > 20) // war 35

    {

    if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))

    NeueKompassRichtungMerken = 1;

    }

    tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y

    = ax + bx?

    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;

    sollGier = tmp_int;

    /*如果没有这句话 那么偏航轴的期望角度将一直等于0度 那么如果需要调整偏航轴的角度 就必须一直

    不断的进行偏航的修正 加上这句话后 就不用一直修正了*/

    Mess_Integral_Gier -= tmp_int;

    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen约束和限制

    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;

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

    // Kompass

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

    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))

    {

    int w,v;

    static int SignalSchlecht = 0;

    w = abs(IntegralNick /512);

    v = abs(IntegralRoll /512);

    if(v > w) w = v;

    if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)

    {

    KompassStartwert = KompassValue;

    NeueKompassRichtungMerken = 0;

    }

    w = (w * Parameter_KompassWirkung) / 64;

    w = Parameter_KompassWirkung - w;

    if(w > 0)

    {

    if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;

    if(SignalSchlecht) SignalSchlecht--;

    }

    else SignalSchlecht = 500;

    }

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

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

    // Debugwerte zuordnen

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

    if(!TimerWerteausgabe--)

    {

    TimerWerteausgabe = 24;

    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;

    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;

    DebugOut.Analog[2] = Mittelwert_AccNick;

    DebugOut.Analog[3] = Mittelwert_AccRoll;

    DebugOut.Analog[4] = MesswertGier;

    DebugOut.Analog[5] = HoehenWert;

    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);

    DebugOut.Analog[8] = KompassValue;

    DebugOut.Analog[9] = UBat;

    DebugOut.Analog[10] = SenderOkay;

    DebugOut.Analog[16] = Mittelwert_AccHoch;

    /* DebugOut.Analog[16] = motor_rx[0];

    DebugOut.Analog[17] = motor_rx[1];

    DebugOut.Analog[18] = motor_rx[2];

    DebugOut.Analog[19] = motor_rx[3];

    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];

    DebugOut.Analog[20] /= 14;

    DebugOut.Analog[21] = motor_rx[4];

    DebugOut.Analog[22] = motor_rx[5];

    DebugOut.Analog[23] = motor_rx[6];

    DebugOut.Analog[24] = motor_rx[7];

    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];

    */

    // DebugOut.Analog[9] = MesswertNick;

    // DebugOut.Analog[9] = SollHoehe;

    // DebugOut.Analog[10] = Mess_Integral_Gier / 128;

    // DebugOut.Analog[11] = KompassStartwert;

    // DebugOut.Analog[10] = Parameter_Gyro_I;

    // DebugOut.Analog[10] = EE_Parameter.Gyro_I;

    // DebugOut.Analog[9] = KompassRichtung;

    // DebugOut.Analog[10] = GasMischanteil;

    // DebugOut.Analog[3] = HoeheD * 32;

    // DebugOut.Analog[4] = hoehenregler;

    }

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

    // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen//角速度和角度变化的归纳部分

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

    //DebugOut.Analog[26] = MesswertNick;

    //DebugOut.Analog[28] = MesswertRoll;

    /*对角度做PD，也就是对角速率做了PI*/

    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;

    else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;

    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;

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

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