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

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

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

    CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);

    CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);

    CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);

    CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);

    CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);

    CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);

    CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);

    CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);

    CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);

    CHK_POTI(Parameter_LooPGAsLimit,EE_Parameter.LoopGasLimit,0,255);

    CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);

    CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);

    CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);

    Ki = (float) Parameter_I_Faktor * 0.0001;

    MAX_GAS = EE_Parameter.Gas_Max;

    MIN_GAS = EE_Parameter.Gas_Min;

    }

    /*飞控核心*/

    //############################################################################

    //

    void MotorRegler(void)

    //############################################################################

    {

    int motorwert,pd_ergebnis,h,tmp_int;//电机数值,PI算法的计算数值

    int GierMischanteil,GasMischanteil;//偏航混合数值,油门混和数值

    static long SummeNick=0,SummeRoll=0;//俯仰积分总和,滚转积分总和

    static long sollGier = 0,tmp_long,tmp_long2;//标准偏航值,

    static long IntegralFehlerNick = 0;//俯仰误差积分

    static long IntegralFehlerRoll = 0;//滚转误差积分

    static unsigned int RcLostTimer;

    static unsigned char delay_neutral = 0;

    static unsigned char delay_einschalten = 0,delay_ausschalten = 0;//延迟接通,延迟关闭

    static unsigned int modell_fliegt = 0;//飞机飞行时间

    static int hoehenregler = 0;//高度调节

    static char TimerWerteausgabe = 0;//时间数值

    static char NeueKompaSSRichtungMerken = 0;//罗盘方向调整中立值

    static long ausgleichNick, ausgleichRoll;//俯仰均衡,滚转均衡

    /*根据测量值 计算陀螺仪和加速度计数据*/

    Mittelwert();

    GRN_ON;//打开端口

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

    // Gaswert ermitteln//判断油门数值

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

    GasMischanteil = StickGas;

    if(GasMischanteil < 0) GasMischanteil = 0;

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

    // Emfang schlecht//无线电故障,不好

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

    if(SenderOkay < 100)

    {

    if(!PcZugriff)

    {

    if(BeepMuster == 0xffff)

    {

    beeptime = 15000;

    BeepMuster = 0x0c00;

    }

    }

    if(RcLostTimer) RcLostTimer--;

    else

    {

    MotorenEin = 0;

    Notlandung = 0;

    }

    ROT_ON;

    if(modell_fliegt > 2000)

    {

    GasMischanteil = EE_Parameter.NotGas;

    Notlandung = 1;

    PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] = 0;

    PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] = 0;

    PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] = 0;

    }

    else

    MotorenEin = 0;

    } // end of if(SenderOkay < 100)

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

    // Emfang gut//

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

    else if(SenderOkay > 140)

    {

    Notlandung = 0;

    RcLostTimer = EE_Parameter.NotGasZeit * 50;

    if(GasMischanteil > 40)

    {

    if(modell_fliegt < 0xffff)

    modell_fliegt++;

    }

    if((modell_fliegt < 200) || (GasMischanteil < 40))

    {

    SummeNick = 0;

    SummeRoll = 0;

    Mess_Integral_Gier = 0;

    Mess_Integral_Gier2 = 0;

    }

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

    // auf Nullwerte kalibrieren

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

    if((PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] > 80) && MotorenEin == 0)

    {

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

    {

    if(++delay_neutral > 200)

    {

    GRN_OFF;

    MotorenEin = 0;

    delay_neutral = 0;

    modell_fliegt = 0;

    if(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > 70 || abs(PPM_in

    [EE_Parameter.Kanalbele×g[K_ROLL]]) > 70)

    {

    unsigned char setting=1;

    if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > 70 && PPM_in

    [EE_Parameter.Kanalbele×g[K_NICK]] < 70) setting = 1;

    if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > 70 && PPM_in

    [EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 2;

    if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < 70 && PPM_in

    [EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 3;

    if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] <-70 && PPM_in

    [EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 4;

    if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] <-70 && PPM_in

    [EE_Parameter.Kanalbele×g[K_NICK]] < 70) setting = 5;

    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);

    }

    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H鰄enregelung

    aktiviert?

    {

    if((MessLuftdruck > 950) || (MessLuftdruck < 750))

    SucheLuftruckOffset();

    }

    ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *)

    &EE_Parameter.Kanalbele×g[0], STRUCT_PARAM_LAENGE);

    SetNeutral();

    Piep(GetActiveParamSetNumber());

    }

    }

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


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