机器谱

S094】四旋翼飞行器

图文展示3264(1)

图文展示3264(1)

副标题

作品说明

作者:张坚 艾洪均 冯京京 栗放

单位:北京石油化工学院

指导老师:曹建树 代峰燕 黄松涛

1. 作品总体设计思路与方案

任何由人类制造、能飞离地面、在空间飞行并由人来控制的飞行物,都可称为飞行器而在大气层内飞行的飞行器称为航空器,如气球、滑翔机、飞艇、飞机、直升机等它们靠空气的静浮力或空气相对运动产生的空气动力升空飞行。飞行器不仅广泛应用于军事,在民用领域的作用也在增加。机载GPS和MEMS惯性传感器的飞行器甚至可以在没有人为控制的室外环境中飞行因此国内外对飞行器进行了大量研究对飞行器的研究目前主要包括固定翼、旋翼及扑翼式三种。四旋翼飞行器在布局形式上属于旋翼的一种,相对于别的旋翼式飞行器来说四旋翼飞行器结构紧凑,能产生更大的升力,而且不需要专门的反扭矩桨保持飞行器扭矩平衡。四旋翼飞行器能够垂直起降,不需要滑跑就可以起飞和着陆,从而不需要专门的机场和跑道,降低了使用成本,可以分散配置,军事上便于伪装,对敌进行突袭和侦察。

      四旋翼飞行器能够自由悬停和垂直起降,结构简单,易于控制,这些优势决定了其具有广泛的应用领域,不但具有一般战场需要的各种作战功能,比如侦察监视,为其他作战武器指示目标等,甚至可以作为投放武器的载体。在民用方向,也能发挥越来越大的作用。目前国内外四旋翼飞行器的研究蓬勃发展,美国、日本、法国和德国等均有此类研究项目且技术较为成熟。

1.1 作品总体方案

项目设计的四旋翼飞行器是一种以四个电机作为动力装置,通过调节电机转速来控制飞行的驱动系统。为了实现对四旋翼飞行器的自主飞行控制,我们对飞行控制系统进行了初步设计,并且以所提供的探索者系列主控板为计算控制单元,给出了飞行控制系统的编程设计而且还研究了设计中的所有关键技术,由于采用简易结构和低功耗的元器件,使飞行器具有重量轻、体积小、功耗低的优点。经过反复的室内试验,设计出性能可靠的硬件,使之能够满足飞行器起飞、悬停、降落等飞行模态的控制要求。

1.2 作品组成与功能

下图所示,就是本组待设计的四旋翼飞行器的整体外观。四旋翼飞行器主要由螺旋桨组、电机座、动力装置、传动装置、主控板、支板、连杆、连接螺母和连接螺钉等零部件装配而成。

     项目设计的四旋翼飞行器具有诸多特点。例如,外形时尚精美,可娱乐大众;结构简单,易制作、易维护;可自由切换多种飞行模式;可实现程序和遥控双重控制;可自动避障;拥有醒目LED指示灯;可悬挂微型相机,拍摄风景。拥有这样特点的四旋翼飞行器,可满足人们对于高空探测、空中数据采集、无人侦察、抗震救灾发放物资等功能的实现。

作品说明

总体结构

2. 机身飞行原理及三维设计

2.1 机械结构安装与调试

      四旋翼飞行器的安装相对简单,其中最为主要的是需要确保四个螺旋桨的位置安装正确和电路的连接正确。如若发生错误,那么就无法保证四旋翼飞行器的正常飞行和功能的执行。

安装完成后,给螺旋桨组接上电源,查看螺旋桨组的运转情况与四旋翼飞行器的整体情况。反复多次调试,把需要拧紧的地方拧紧,确保四个螺旋桨的旋转方向准确无误,从而完成初步的调试工作。

2.2 飞行原理

     四旋翼飞行器可以完成平稳起飞、上升、下降、俯仰、翻滚、偏航等动作,而且具备红外避障、机身自动调整、遥控控制等功能。

2.2.1 起飞原理

四旋翼飞行器通过四个螺旋桨产生的升力实现飞行, 原理与直升机类似。 四个旋翼位于一个几何对称的十字支架前,后,左,右四端下图所示。旋翼由电机控制整个飞行器依靠改变每个电机的转速来实现飞行姿态控制。

四旋翼飞行器旋转方向示意图

      在上图中,前端旋翼1和后端旋翼3逆时针旋转, 而左端旋翼2和右端的旋翼4顺时针旋转,以平衡旋翼旋转所产生的反相互抵消反扭力矩;同时等量地增大或减小四只旋翼的转速,会引起上升或下降运动;增大某一只旋翼的转速,同时等量地减小同组另一只旋翼的转速,则产生俯仰、横滚运动;增大某一组旋翼的转速,同时等量减小另一组旋翼的转速,将产生偏航运动。

      四旋翼飞行器受力分析如下图所示,旋翼机体所受外力和力矩为:

四旋翼飞行器受力分析示意图

       重力mg , 机体受到重力沿      方向; 四个旋翼旋转所产生的升力     (i= 1 , 2 , 3 , 4),旋翼升力沿    方向; 旋翼旋转会产生扭转力矩      (i= 1 , 2 , 3 , 4)。      垂直于叶片的旋翼平面,与旋转矢量相反。

2.2.2避障原理

 近红外传感器用近红外能量来直接检测物体的特定组元或成分,而这些组元会选择性吸收近红外能量的特定波长。近红外传感器可以反射并接收反射的近红外信号,有效检测范围在20cm以内,如下图所示就是探索者提供的近红外传感器

近红外传感器

2.2.3 机身自动调整原理

姿态传感器通过内嵌的低功耗ARM处理器输出校准过的角速度加速度磁数据等,通过基于四元数的传感器数据算法进行运动姿态测量,实时输出三维姿态数据。姿态传感器可以检测飞行器机身的倾斜变化,识别飞行器所处的姿态,而适时做出反应。例如摔倒了之后,姿态传感器就会被触发,通常倾斜超过45度时会被触发,如下图所示就是探索者提供的姿态传感器

姿态传感器

2.2.4 遥控飞行原理

红外线遥控是利用近红外光传送遥控指令的,用近红外作为遥控光源,是因为目前红外发射器件红外发光管与红外接收器件光敏二极管、三极管及光电池的发光与受光峰值波长一般为0.8um~0.94um,在近红外光波段内,二者的光谱正好重合,能够很好地匹配,可以获得较高的传输效率及较高的可靠性,如下图所示就是探索者提供的红外遥控器

红外线遥控

2.3 三维设计

      本项目运用三维制图软件对自行设计的四旋翼飞行器的所有零部件进行建模,并完成装配。如下图所示螺旋桨(a)螺旋桨(b)电机座动力装置传动装置主控板支板板件连接螺纹连接螺钉分别是四旋翼各个零部件三维图

螺旋桨(a)

主控板

螺旋桨(b)

电机座

动力装置

传动装置

支板

连杆

连接螺钉

连接螺母

如下图所示是四旋翼装配图的三维渲染图

四旋翼飞行器总装配

3. 控制系统结构分析与硬件系统设计
3.1 控制系统结构分析

      四旋翼飞行器采用四个旋翼作为飞行的直接动力源,旋翼对称分布在机体的前后、左右四个方向,四个旋翼处于同一高度平面,且四个旋翼的结构和半径都相同,旋翼1和旋翼3 逆时针旋转,旋翼2和旋翼4顺时针旋转,四个电机对称的安装在飞行器的支架端,支架中间空间安放飞行控制计算机和外部设备。

      典型的传统直升机配备有一个主转子和一个尾浆。们是通过控制舵机来改变螺旋桨的桨距角,从而控制直升机的姿态和位置。四旋翼飞行器与此不同,是通过调节四个电机转速来改变旋翼转速,实现升力的变化,从而控制飞行器的姿态和位置。由于飞行器是通过改变旋翼转速实现升力变化,这样会导致其动力不稳定,所以需要一种能够长期确保稳定的控制方法。四旋翼飞行器是一种六自由度的垂直起降机,因此非常适合静态和准静态条件下飞行。但是四旋翼飞行器只有四个输入力,同时却有六个状态输出,所以它又是一种欠驱动系统。

      四旋翼飞行器的结构形式,电机1和电机3逆时针旋转的同时,电机2和电机4顺时针旋转,因此当飞行器平衡飞行时,陀螺效应和空气动力扭矩效应均被抵消。与传统的直升机相比,四旋翼飞行器有下列优势:各个旋翼对机身所施加的反扭矩与旋翼的旋转方向相反,因此当电机1和电机3逆时针旋转的同时,电机2和电机4顺时针旋转,可以平衡旋翼对机身的反扭矩。

3.2 硬件系统设计

     由于四旋翼飞行器具有体积小、载荷轻等特点,因此进行系统硬件器件选型时,应遵循以下原则:功耗要低;体积要小、重量要轻;低成本;高稳定性。

      作为核心部件,主控制器必须具有足够的数据处理能力和系统控制能力,而且外设接口要丰富满足需要,有足够的程序和数据空间。最终我们选用了探索者主控板作为主控制器,它采用高性能技术,整合了处理单元和浮点处理单元的构架,浮点处理单元采用单精度浮点格式标准,使其具有很强的浮点运算能力。与定点处理器相比,精度高,速度快,效率高,而姿态解算需要大量的浮点运算。并且外设接口丰富,易于我们在主控制器写入程序后对外设借口的分配。


4. 软件系统设计
4.1 控制总体流程图

      飞行控制系统的中央控制模块主要完成系统初始化、系统自检、解算传感器数据、执行控制算法、计算并输出控制量等功能,系统程序流程如下图所示

系统程序流程图

4.2 示例程序

void Piep(unsigned char Anzahl)

{  

while(Anzahl--)  

{  

  if(MotorenEin) return; //auf keinen Fall im Flug!  

  beeptime = 100;  

  Delay_ms(250);  

}  

}  

//函数:SetNeutral设定传感器发出参数的中立数值,因为有漂移所以要使其每次工作都要测量出来。  

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

//   Nullwerte ermitteln

/*设置中立点*/  

void SetNeutral(void)  

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

{  

/*加速度计中立点*/

NeutralAccX = 0;  

NeutralAccY = 0;

NeutralAccZ = 0;

/*陀螺仪中立点*/

AdNeutralNick = 0;

AdNeutralRoll = 0;

AdNeutralGier = 0;

    Parameter_AchsKopplung1 = 0;

    Parameter_AchsGegenKopplung1 = 0;。。。  

/*这个地方我还没有弄得太明白,检测中立点的函数被调用了两次,但是第一次的数据好像没有保存,只用到了

第二次的数据*/

/*记录中立点*/

CalibrierMittelwert();

    Delay_ms_Mess(100);

/*记录中立点*/

CalibrierMittelwert();

/*既然只使用了后一次的数据,为什么要进行两次记录中立点的函数*/

    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  

     {

      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();//如果气压表输出在

950外750内,则设定气压初始的偏差。  

     }

/*将量测值作为陀螺仪的中立点*/  

AdNeutralNick= AdWertNick;

AdNeutralRoll= AdWertRoll;

AdNeutralGier= AdWertGier;

/*这两个参数在飞控程序中没有用到*/

StartNeutralRoll = AdNeutralRoll;

StartNeutralNick = AdNeutralNick;

    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) //  

    {

/*由于在函数CalibrierMittelwert()中加速度计的输出乘以了ACC_AMPLIFY,所以这里必须处以ACC_AMPLIFY,

在这段程序中,所有的对加速度计和陀螺仪的数值的衰减或者放大都是为了让

陀螺仪积分和加速度计数值在同样的角度偏差的情况下能基本匹配,如果不匹配,那么就谈不上用加速度计来补

偿陀螺仪积分了*/

      NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;

  NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;

  NeutralAccZ = Aktuell_az;

    }  

    else

    {  

/*如果发现变化不大,则仍然储存上一次的*/

      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)

eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);  

  NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)

eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);  

  NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)

eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);  

    }  

     

/*将所有数据清零,这里带2的变量都是加入了陀螺仪漂移补偿值之后得到的角度*/

Mess_IntegralNick = 0;

    Mess_IntegralNick2 = 0;

    Mess_IntegralRoll = 0;

    Mess_IntegralRoll2 = 0;  

    Mess_Integral_Gier = 0;  

    MesswertNick = 0;

    MesswertRoll = 0;

    MesswertGier = 0;

    StartLuftdruck = Luftdruck;

    HoeheD = 0;

    Mess_Integral_Hoch = 0;

    KompassStartwert = KompassValue;

    GPS_Neutral();

    beeptime = 50;   //  

/*从EEPROM中读取陀螺仪积分到达90°时候的值,并储存,当得到的姿态角度大于这个范围时,说明超过了90°

,就要进行相应的处理*/

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

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

    ExternHoehenValue = 0;  

}  

///////////////////////////////  

//函数描述 :求参数的平均数值  

//////////////////////////////  

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

// Bearbeitet die Messwerte  

void Mittelwert(void)  

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

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

{   

    static signed long tmpl,tmpl2;

/*将陀螺仪数据减去常值误差,得到实际的叫速率的倍数*/

    MesswertGier = (signed int) AdNeutralGier - AdWertGier;

    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;

    MesswertNick = (signed int) AdWertNick - AdNeutralNick;

//DebugOut.Analog[26] = MesswertNick;  

DebugOut.Analog[28] = MesswertRoll;

//加速度传感器输出  

/*加速度计数据滤波,ACC_AMPLIFY=12 得到的Mittelwert_AccNick是加速度计数值的12倍*/

/*AdWertAccNick为测量值*/

// Beschleuni×gssensor   ++++++++++++++++++++++++++++++++++++++++++++++++  

Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) /  

2L;//具有滤波功能的方法,用当前加速度和上次的加速度平均  

Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;  

Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;  

/*计算加速度计的积分,加速度计对运动十分敏感,采用加速度计积分,可以减少瞬间的运动加速度的影响*/

    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;

    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;  

    IntegralAccZ    += Aktuell_az - NeutralAccZ;  

// Gier   ++++++++++++++++++++++++++++++++++++++++++++++++

/*偏航方向无法进行滤波,因此直接进行积分得到偏航角度*/

            Mess_Integral_Gier +=   MesswertGier;  

            Mess_Integral_Gier2 += MesswertGier;  

/*耦合项应该是另外两个陀螺仪对这个轴上陀螺仪的影响,当四轴在稳定姿态不为水平的时候,进行偏航运动时

候所进行的补偿*/

/*假设目前的俯仰角是30°,而横滚角是0°,这时候如保持俯仰和横滚轴没有任何运动,而将偏航轴转动90°

,那么实际的俯仰角就会变为0°,横滚角就会变为30°

  但是,按照目前的算法,由于俯仰和横滚方向没有运动,因此就不会有陀螺仪的积分,俯仰和横滚角是不变的

,这就是采用陀螺仪直接积分测角度的不完善性,这时候

  使用加速度计对姿态进行修正能够起到作用,但是需要一段时间,使用下面的这段话,就是将偏航轴的运动耦

合在另外两个轴上,使姿态角度能够迅速收敛到真实值上*/

/*注:使用四元数法进行姿态结算可以避免出现这种问题,但这种方法需要有准确的陀螺仪和加表的数学模型,四元数法还需要进行大量的矩阵计算,

  而且对四元数姿态进行加速度计的融合不太方便*/

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

CFG_ACHSENKOPPLUNG_AKTIV))//不在俯仰滚转控制循环中  

         {

            tmpl = Mess_IntegralNick / 4096L;  

            tmpl *= MesswertGier;  

            tmpl *= Parameter_AchsKopplung1;   //125  

            tmpl /= 2048L;  

            tmpl2 = Mess_IntegralRoll / 4096L;  

            tmpl2 *= MesswertGier;  

            tmpl2 *= Parameter_AchsKopplung1;  

            tmpl2 /= 2048L;  

         }  

      else   tmpl = tmpl2 = 0;

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

            MesswertRoll += tmpl;

            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L;  

            Mess_IntegralRoll2 += MesswertRoll;

            Mess_IntegralRoll +=   MesswertRoll - LageKorrekturRoll;

/*积分超过半圈的情况*/

            if(Mess_IntegralRoll > Umschlag180Roll)  

            {  

             Mess_IntegralRoll   = -(Umschlag180Roll - 10000L);  

             Mess_IntegralRoll2 = Mess_IntegralRoll;  

            }   

            if(Mess_IntegralRoll <-Umschlag180Roll)//一样  

            {  

             Mess_IntegralRoll =   (Umschlag180Roll - 10000L);  

             Mess_IntegralRoll2 = Mess_IntegralRoll;  

            }   

            if(AdWertRoll < 15)   MesswertRoll = -1000;

            if(AdWertRoll <   7)   MesswertRoll = -2000;

            if(PlatinenVersion == 10)

{  

              if(AdWertRoll > 1010) MesswertRoll = +1000;

              if(AdWertRoll > 1017) MesswertRoll = +2000;  

}  

else   

{  

              if(AdWertRoll > 2020) MesswertRoll = +1000;  

              if(AdWertRoll > 2034) MesswertRoll = +2000;  

}  

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

            MesswertNick -= tmpl2;  

            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;  

            Mess_IntegralNick2 += MesswertNick;  

/*LageKorrekturNick是通过加速度计积分和角速率积分的积分进行做差比较得到

,*/

            Mess_IntegralNick   += MesswertNick - LageKorrekturNick;

            if(Mess_IntegralNick > Umschlag180Nick)   

            {  

             Mess_IntegralNick = -(Umschlag180Nick - 10000L);  

             Mess_IntegralNick2 = Mess_IntegralNick;  

            }   

            if(Mess_IntegralNick <-Umschlag180Nick)   

            {  

             Mess_IntegralNick =   (Umschlag180Nick - 10000L);  

             Mess_IntegralNick2 = Mess_IntegralNick;  

            }   

            if(AdWertNick < 15)   MesswertNick = -1000;  

            if(AdWertNick <   7)   MesswertNick = -2000;  

            if(PlatinenVersion == 10)  

{  

              if(AdWertNick > 1010) MesswertNick = +1000;  

              if(AdWertNick > 1017) MesswertNick = +2000;  

}  

else   

{  

              if(AdWertNick > 2020) MesswertNick = +1000;  

              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);   

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());   

                }   

}

else if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] < -75)   

            {   

if(++delay_neutral > 200)   // nicht sofort   

                {   

GRN_OFF;   

                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte

l鰏chen   

                    MotorenEin = 0;   

                    delay_neutral = 0;   

                    modell_fliegt = 0;   

                    SetNeutral();//设立中性点。   

                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); //   

ACC-NeutralWerte speichern   

                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); //   

ACC-NeutralWerte speichern   

                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);   

                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);   

                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);   

                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);   

                    Piep(GetActiveParamSetNumber());   

                }   

            }  


//    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;  

    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;  

    MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;  

DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;  

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

DebugOut.Analog[28] = MesswertRoll;  

/*对控制器输出进行幅度限制*/

    #define MAX_SENSOR   2048  

    if(MesswertNick >   MAX_SENSOR) MesswertNick =   MAX_SENSOR;

    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;  

    if(MesswertRoll >   MAX_SENSOR) MesswertRoll =   MAX_SENSOR;  

    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;  

    if(MesswertGier >   MAX_SENSOR) MesswertGier =   MAX_SENSOR;  

    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;  

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

// H鰄enregelung  

// Die H鰄enregelung schw鋍ht lediglich das Gas ab, erh鰄t es allerdings nicht  

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

//OCR0B = 180 - (Poti1 + 120) / 4;  

//DruckOffsetSetting = OCR0B;  

if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))

{  

int tmp_int;  

    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  

    {  

if(Parameter_MaxHoehe < 50)   

{  

SollHoehe = HoehenWert - 20;

HoehenReglerAktiv = 0;  

}  

else   

HoehenReglerAktiv = 1;  

}  

    else

    {  

SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)

EE_Parameter.Hoehe_Verstaerkung - 20;  

HoehenReglerAktiv = 1;  

    }

if(Notlandung)  

SollHoehe = 0;  

    h = HoehenWert;  

    if((h > SollHoehe) && HoehenReglerAktiv)  

    {       

h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16;  

h = GasMischanteil - h;   

h -= (HoeheD * Parameter_Luftdruck_D)/8;  

tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung)  

/ 32;

if(tmp_int > 50)  

tmp_int = 50;  

else if(tmp_int < -50) tmp_int = -50;  

h -= tmp_int;  

hoehenregler = (hoehenregler*15 + h) / 16;

if(hoehenregler < EE_Parameter.Hoehe_MinGas)

{  

if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler =  

EE_Parameter.Hoehe_MinGas;  

if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler =  

GasMischanteil;  

}   

if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil;  

GasMischanteil = hoehenregler;  

}   

} // 高度调节器工作完成

if(GasMischanteil > MAX_GAS - 20)  

GasMischanteil = MAX_GAS - 20;

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

// + Mischer und PI-Regler 在PI控制器下的混合数值  

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

DebugOut.Analog[7] = GasMischanteil;  

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

// Gier-Anteil//偏航部分  

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

#define MUL_G   1.0  

    GierMischanteil = MesswertGier - sollGier;   

// GierMischanteil = 0;  

/*对偏航数值进行限制,尽量避免最后计算出的四个电机的转速小于0*/

    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;  

    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);   

    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS -  

GasMischanteil));  

    if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS -  

GasMischanteil));  

/*油门本身如果太小了,就限制偏航为0*/

    if(GasMischanteil < 20) GierMischanteil = 0;//

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

// Nick-Achse俯仰轴  

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

/*PI调节器*/

/*这个控制算法实际上是位置环为PI控制,速率环为P控制*/

    DiffNick = MesswertNick - (StickNick - GPS_Nick);

    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick);

    else   SummeNick += DiffNick;  

    if(SummeNick >   16000) SummeNick =   16000;  

    if(SummeNick < -16000) SummeNick = -16000;  

    pd_ergebnis = DiffNick + Ki * SummeNick; //PD控制结果为比例+积分控制  

    // Motor Vorn  

    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs

(GierMischanteil)/2)) / 64;

    if(pd_ergebnis >   tmp_int) pd_ergebnis =   tmp_int; //如果控制器输出太大,则要限制幅度

    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;   

/*前后两个电机的实际输出*/

    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;

/*对电机数值进行限幅*/

if ((motorwert < 0))  

motorwert = 0;  

else if(motorwert > MAX_GAS)       

motorwert = MAX_GAS;  

if (motorwert < MIN_GAS)             

motorwert = MIN_GAS;  

Motor_Vorne = motorwert;     

    // Motor Heck  

motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;

if ((motorwert < 0))  

motorwert = 0;  

else if(motorwert > MAX_GAS)     

motorwert = MAX_GAS;  

if (motorwert < MIN_GAS)           

motorwert = MIN_GAS;  

Motor_Hinten = motorwert;  

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

// Roll-Achse横滚轴

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

DiffRoll = MesswertRoll - (StickRoll   - GPS_Roll); // Differenz bestimmen  

    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll   - GPS_Roll);// I-

Anteil bei Winkelregelung  

    else           SummeRoll += DiffRoll;   // I-Anteil bei HH  

    if(SummeRoll >   16000) SummeRoll =   16000;  

    if(SummeRoll < -16000) SummeRoll = -16000;  

    pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler f黵 Roll  

    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs

(GierMischanteil)/2)) / 64;  

    if(pd_ergebnis >   tmp_int) pd_ergebnis =   tmp_int;   

    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;   

    // Motor Links  

    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;  

#define GRENZE Poti1  

if ((motorwert < 0)) motorwert = 0;  

else if(motorwert > MAX_GAS) motorwert = MAX_GAS;  

if (motorwert < MIN_GAS)            motorwert = MIN_GAS;  

    Motor_Links = motorwert;  

    // Motor Rechts  

motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;  

if ((motorwert < 0)) motorwert = 0;  

else if(motorwert > MAX_GAS)     motorwert = MAX_GAS;  

if (motorwert < MIN_GAS)            motorwert = MIN_GAS;  

    Motor_Rechts = motorwert;  

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

}


delay_neutral = 0;   

} // end if of if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] > 75)

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

// Gas ist unten   

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

            if(PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] < 35-120)

            {   

// Starten   

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

                {   

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

// Einschalten   

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

                    if(++delay_einschalten > 200)   

{   

                        delay_einschalten = 200;   

                        modell_fliegt = 1;   

                        MotorenEin = 1;   

                        sollGier = 0;   

                        Mess_Integral_Gier = 0;   

                        Mess_Integral_Gier2 = 0;   

                        Mess_IntegralNick = 0;   

                        Mess_IntegralRoll = 0;   

                        Mess_IntegralNick2 = IntegralNick;   

                        Mess_IntegralRoll2 = IntegralRoll;   

                        SummeNick = 0;   

                        SummeRoll = 0;   

                    }           

                }   

                else   

delay_einschalten = 0;//没事,就让其延迟关闭为0   

                //Auf Neutralwerte setzen   

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

// Auschalten   

/*切换*/

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

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

                {   

if(++delay_ausschalten > 200)   // nicht sofort   

                    {   

                    MotorenEin = 0;   

                    delay_ausschalten = 200;   

                    modell_fliegt = 0;   

                    }   

                }   

                else delay_ausschalten = 0;   

            }   

        } // end if of else if(SenderOkay > 140)

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

// neue Werte von der Funke   

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

if(!NewPpmData-- || Notlandung)   

   {   

    int tmp_int;   

static int stick_nick,stick_roll;//俯仰杆,倾斜杆   

    ParameterZuordnung();

/*新老数据滤波混合,这里改变的应该是期望角位置,必须知道EE_Parameter.Stick_P的数值才可以得到滤波效

*/

    StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] *   

EE_Parameter.Stick_P) / 4; //新数据和老数据混合起滤波作用   

/*将期望角位置的微分加入操纵杆变量上,这里必须知道EE_Parameter.Kanalbele×g[K_ROLL]的求法,和

EE_Parameter.Stick_D得数值*/

    StickNick += PPM_diff[EE_Parameter.Kanalbele×g[K_NICK]] * EE_Parameter.Stick_D;//增加上微分

量,用于提高反应的快速性。   

    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] *   

EE_Parameter.Stick_P) / 4;   

    StickRoll += PPM_diff[EE_Parameter.Kanalbele×g[K_ROLL]] * EE_Parameter.Stick_D;   

    StickGier = -PPM_in[EE_Parameter.Kanalbele×g[K_GIER]];   

    StickGas   = PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] + 120;   

/*用此记录历史上的最大给杆量,如果给杆量很小,则Max数值会不断减小,用于在后面给陀螺仪积分做补偿时

,对加速度计数据和陀螺仪积分的差值做衰减*/

   if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]]) > MaxStickNick)   

     MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]]); else MaxStickNick--;   

   if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]]) > MaxStickRoll)   

     MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]]); else MaxStickRoll--;   

/*如果在降落过程中,则数据为0,也就是说降落的过程中不需要衰减,降落时候的保持位置全部为0,所以不需

要衰减*/   

   if(Notlandung)

   {

   MaxStickNick = 0; MaxStickRoll = 0;

   }

/*可以认为是控制参数,前一个是陀螺仪的比例项(速率环参数) 后一个是陀螺仪积分即姿态角的比例(位置

环参数)*/

    GyroFaktor     = ((float) Parameter_Gyro_P + 10.0) / 256.0;

    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;

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

//+ Digitale Steuerung per DubWise   

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

#define KEY_VALUE (Parameter_UserParam1 * 4)   //(Poti3 * 8)//为了增加杆的输入的丰富性,提供了扩展

的杆的描述,对最终杆的描述更加丰富。   

if(DubWiseKeys[1])   

beeptime = 10;   

if(DubWiseKeys[1] & DUB_KEY_UP)     

tmp_int = KEY_VALUE;   

else if(DubWiseKeys[1] & DUB_KEY_DOWN)   

tmp_int = -KEY_VALUE;   

else   

tmp_int = 0;   

ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;   

if(DubWiseKeys[1] & DUB_KEY_LEFT)   

tmp_int = KEY_VALUE;   

else if(DubWiseKeys[1] & DUB_KEY_RIGHT)   

tmp_int = -KEY_VALUE;   

else   

tmp_int = 0;   

ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;   

if(DubWiseKeys[0] & 8)   

ExternStickGier = 50;

else if(DubWiseKeys[0] & 4)   

ExternStickGier =-50;

else   

ExternStickGier = 0;

   

if(DubWiseKeys[0] & 2)   

ExternHoehenValue++;   

if(DubWiseKeys[0] & 16)   

ExternHoehenValue--;   

StickNick += ExternStickNick / 8;   

StickRoll += ExternStickRoll / 8;   

StickGier += ExternStickGier;   

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

//+ Analoge Steuerung per Seriell   

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

if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)//同上,具有扩展功能的控制输入   

{   

StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;   

StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;   

StickGier += ExternControl.Gier;   

    ExternHoehenValue =   (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;   

    if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;   

}   

/*陀螺仪积分比例为零,应该是Looping的情况?*/

if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)   

IntegralFaktor =   0;   

if(GyroFaktor < 0) GyroFaktor = 0;   

if(IntegralFaktor < 0) IntegralFaktor = 0;   

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

// Looping?//这里是在空中转圈的情况   

//

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++   

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

EE_Parameter.LoopConfig & CFG_LOOP_LINKS)   Looping_Links = 1;   

   else   

   {   

     {   

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

EE_Parameter.LoopHysterese))) Looping_Links = 0;   

     }   

   }   

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

EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;   

   else   

   {   

   if(Looping_Rechts) // Hysterese   

     {   

      if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > -(EE_Parameter.LoopThreshold -   

EE_Parameter.LoopHysterese)) Looping_Rechts = 0;   

     }   

   }   

   if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > EE_Parameter.LoopThreshold) &&   

EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;   

   else   

   {   

    if(Looping_Oben)   // Hysterese   

     {   

      if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < (EE_Parameter.LoopThreshold -   

EE_Parameter.LoopHysterese))) Looping_Oben = 0;   

     }   

   }   

   if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < -EE_Parameter.LoopThreshold) &&   

EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;   

   else   

   {   

    if(Looping_Unten) // Hysterese   

     {   


      if(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > -(EE_Parameter.LoopThreshold -   

EE_Parameter.LoopHysterese)) Looping_Unten = 0;   

     }   

   }   

/*不应该出现轴都是Looping的情况*/

   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;

   if(Looping_Oben   || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0;   

Looping_Rechts = 0;} else Looping_Nick = 0;   

} // end if of if(!NewPpmData-- || Notlandung)   

if(Looping_Roll) beeptime = 100;

if(Looping_Roll || Looping_Nick)

{   

if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;   

}   

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

// Bei Empfangsausfall im Flug   

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

if(Notlandung)

{   

/*如果出现紧急降落,则将三个期望位置全部置零,即让飞行器向最稳定的方向调整,同时改变控制参

数,并且不让飞行器处在空中打转的状态*/

StickGier = 0;   

StickNick = 0;   

StickRoll = 0;   

GyroFaktor   = 0.1;   

IntegralFaktor = 0.005;   

Looping_Roll = 0;   

    Looping_Nick = 0;   

}

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

// Integrale auf ACC-Signal abgleichen//加速度信号的积分校准   

//

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++   

#define ABGLEICH_ANZAHL 256L   

/*计算陀螺仪积分的积分,为了和加速度计的积分做比较,进行角速率的补偿和陀螺仪中立点的修正*/

MittelIntegralNick   += IntegralNick;   

MittelIntegralRoll   += IntegralRoll;   

MittelIntegralNick2 += IntegralNick2;   

MittelIntegralRoll2 += IntegralRoll2;   

/*在空中打转过程中,让所有的积分项都为零,因为机动过程会产生很大的误差,因此需要尽快结束其控制,然

后自动调平。*/

if(Looping_Nick || Looping_Roll)

   {   

    IntegralAccNick = 0;   

    IntegralAccRoll = 0;   

    MittelIntegralNick = 0;   

    MittelIntegralRoll = 0;   

    MittelIntegralNick2 = 0;   

    MittelIntegralRoll2 = 0;   

    Mess_IntegralNick2 = Mess_IntegralNick;

    Mess_IntegralRoll2 = Mess_IntegralRoll;   

    ZaehlMessungen = 0;

    LageKorrekturNick = 0;

    LageKorrekturRoll = 0;

   }   

//

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++   

if(!Looping_Nick && !Looping_Roll)

{   

long tmp_long, tmp_long2;   

/*使用加速度计的值去补偿陀螺仪的积分,这里必须知道EE_Parameter.GyroAccFaktor参数,才能够知道补偿了

多少*/

/*其中IntegralNick应该是陀螺仪积分

Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;

是滤波后的加速度,用当前加速度和上次的加速度平均*/

tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)

Mittelwert_AccNick);//

tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)


5. 作品总体调试

在完成四旋翼飞行器的总装配和程序写入后,我们对自行设计的飞行器进行了试飞调试实验,从中发现了一些问题。由于探索者公司提供的电池电压小,所以使得四旋翼螺旋桨转速不够,达不到起飞要求,很多程序写入时设计的动作也都不能实现。小组成员遂自行购买了电池并对主控板稍加改进,结果再次的试飞中,主控板一部分烧毁了。经与企业老师的交流,发现公司所提供的芯片也并非是专业的飞控芯片,所给电流可满足的范围比较低,所以只能选择其它飞控类芯片。在接下来的试飞调试中,四旋翼飞行器终于能够正常起飞,并实现了设计时所要求达到的功能。最终经过反复调试和修改,四旋翼飞行器运转趋于稳定。


4. 结论

     近年来,四旋翼飞行器的控制问题已经引起了国内外人们的极大关注,并且不断有新的研究成果涌现,可见该课题的研究具有理论意义和实际价值。经过主题实践课程上的学习研究,我们对四旋翼飞行器飞行控制系统做了初步的研究和设计工作,并制作了实验样机。四旋翼飞行器是一种结构特殊的飞行器类似碟形能够和别的飞行器一样实现各种飞行动作。它属于旋翼飞行器的一种,但控制模式与旋翼飞行器有所不同,不需要通过控制旋翼的桨距来控制飞行,完全可以通过协调各旋翼之间的速度来实现各种飞行动作。四旋翼飞行器设计的主要工作主要有以下几点

      ① 对四旋翼飞行器进行资料搜索,了解国内外发展现状

      ② 借鉴常规旋翼动力学,对四桨碟形飞行器动力学进行初步探讨

      ③ 完成四旋翼飞行器的结构设计。

      ④ 完成旋翼升力测试

      ⑤ 对控制频率进行初步研究和设计

      ⑥ 完成飞行控制系统软硬件的实现与调试

      ⑦ 对飞行器进行试飞工作

      在整个项目完成期间,我们遇到了很多设计上的难题例如气流、重力、平衡等物理效应;驱动能力不够;稳定性不好掌控等等,但通过大家的研究努力,逐一克服,最终保证四旋翼飞行器试飞成功。但由于时间仓促,大家水平也有限,对飞行器动力学特性了解还不够深入,理论知识也较为薄弱,许多工作做的并不是非常出色,因此对控制系统的研究还有不足之处,许多方面还有进一步提升和改进的空间。


参考文献

[1] 黄智伟,全国大学生电子设计竞赛训练教程,电子工业出版社,2010 []

[2] 李晓林,单片机原理与接口技术,电子工业出版社,2010

[3] 彭军桥, 四桨碟形飞行器控制系统研究[D],上海大学, 2004

[4] 李俊峰、张雄,理论力学[M],第二版,北京,清华大学出版社,2010:213-216

[5] 陈天华,小型无人机自主飞行控制系统的实现[J],航天控制.2006

[6] 古月徐,基于DSP的飞行控制器的设计[J],自动化技术与应用,2005

[7] 韩京清,自抗扰控制器及其应用[J],控制与决策,1998

[8] 肖永力、张琛,微型飞行器的研究现状与关键技术[J],宇航学报,2001

* 本项目未获得作者开源授权,无法提供资料下载

© 2022 机器时代(北京)科技有限公司  版权所有
机器谱——机器人共享方案网站
学习  开发  设计  应用