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

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

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

    if(AdWertNick > 2034) MesswertNick = +2000;

    }

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

    // ADC einschaLTEn

    ANALOG_ON; //重新开始模拟量的采集

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

    /*上一步计算完了积分之后,现在将积分赋值,因此后面使用的就将是IntegralNick,IntegralNick2等数据了

    */

    Integral_Gier = Mess_Integral_Gier;

    IntegralNick = Mess_IntegralNick;

    IntegralRoll = Mess_IntegralRoll;

    IntegralNick2 = Mess_IntegralNick2;

    IntegralRoll2 = Mess_IntegralRoll2;

    /*这部分代码不执行,因为在EEPROM中CFG_DREHRATEN_BEGRENZER这一位为0*/

    if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)

    {

    if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);

    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);

    if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);

    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);

    }

    if(Poti1 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110) Poti1++;

    else if(Poti1 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110 && Poti1) Poti1--;

    if(Poti2 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in

    [EE_Parameter.Kanalbele×g[K_POTI2]] + 110 && Poti2) Poti2--;

    if(Poti3 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in

    [EE_Parameter.Kanalbele×g[K_POTI3]] + 110 && Poti3) Poti3--;

    if(Poti4 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in

    [EE_Parameter.Kanalbele×g[K_POTI4]] + 110 && Poti4) Poti4--;

    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;

    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;

    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;

    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;

    }

    //函数:校正平均值

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

    // Messwerte beim Ermitteln der Nullage

    /*确定零位*/

    /*记录中立点*/

    void CalibrierMittelwert(void)

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

    {

    // ADC auschalten, damit die Werte sich nicht w鋒rend der Berechnung 鋘dern

    ANALOG_OFF;

    MesswertNick = AdWertNick;

    MesswertRoll = AdWertRoll;

    MesswertGier = AdWertGier;

    /*要乘以 ACC_AMPLIFY 是为了进行数值的匹配*/

    Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;

    Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;

    Mittelwert_AccHoch = (long)AdWertAccHoch;

    // ADC einschalten

    ANALOG_ON; //开模拟量

    if(Poti1 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110) Poti1++;

    else if(Poti1 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110 && Poti1) Poti1--;

    if(Poti2 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110) Poti2++;

    else if(Poti2 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110 && Poti2) Poti2--;

    if(Poti3 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110) Poti3++;

    else if(Poti3 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110 && Poti3) Poti3--;

    if(Poti4 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110) Poti4++;

    else if(Poti4 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110 && Poti4) Poti4--;

    if(Poti1 < 0) Poti1 = 0;

    else if(Poti1 > 255) Poti1 = 255;

    if(Poti2 < 0) Poti2 = 0;

    else if(Poti2 > 255) Poti2 = 255;

    if(Poti3 < 0) Poti3 = 0;

    else if(Poti3 > 255) Poti3 = 255;

    if(Poti4 < 0) Poti4 = 0;

    else if(Poti4 > 255) Poti4 = 255;

    /*这两个数据是在对陀螺仪积分区域进行的限制,如果超过这个范围,说明就超出了+-90°的范围,则需要相应

    的改变*/

    Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;

    Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;

    }

    //发送电机数据

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

    // Senden der Motorwerte per I2C-Bus

    void SendMotorData(void)

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

    {

    if(MOTOR_OFF || !MotorenEin)//关机或未工作

    {

    Motor_Hinten = 0;

    Motor_Vorne = 0;

    Motor_Rechts = 0;

    Motor_Links = 0;//都置零

    if(MotorTest[0]) Motor_Vorne = MotorTest[0];

    if(MotorTest[1]) Motor_Hinten = MotorTest[1];

    if(MotorTest[2]) Motor_Links = MotorTest[2];

    if(MotorTest[3]) Motor_Rechts = MotorTest[3];//如果是试验就干。

    }

    DebugOut.Analog[12] = Motor_Vorne;

    DebugOut.Analog[13] = Motor_Hinten;

    DebugOut.Analog[14] = Motor_Links;

    DebugOut.Analog[15] = Motor_Rechts;

    //Start I2C Interrupt Mode

    twi_state = 0;

    motor = 0;

    i2c_start();

    }

    //函数:参数分配

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

    // Tr鋑t ggf. das Poti als Parameter ein

    void ParameterZuordnung(void)

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

    {

    //

    /*这个宏定义的作用是:将a中的值赋给b,并将b限制在max和min之间*/

    #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b =

    Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b =

    min; else if(b >= max) b = max;}

    CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);

    CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);

    CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);

    CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);

    CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);

    CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);

    CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);

    CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);

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


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