匿名飞控代码宏观解析1
Pixhawk和Paparazzi从入门到精通：开源飞控概述
Pixhawk和Paparazzi从入门到精通：开源飞控概述概述无人机是无人驾驶飞机的简称（Unmanned Aerial Vehicle，UAV），是利用无线电遥控设备和自备的程序控制装置的不载人飞机，包括无人直升机、固定翼机、多旋翼飞行器、无人飞艇、无人伞翼机。
广义地看也包括临近空间飞行器（20-100 公里空域），如平流层飞艇、高空气球、太阳能无人机等。
从某种角度来看，无人机可以在无人驾驶的条件下完成复杂空中飞行任务和各种负载任务，可以被看做是“空中机器人”。
飞控系统是无人机完成起飞、空中飞行、执行任务和返场回收等整个飞行过程的核心系统，飞控对于无人机相当于驾驶员对于有人机的作用，我们认为是无人机最核心的技术之一。
飞控一般包括传感器、机载计算机和伺服作动设备三大部分，实现的功能主要有无人机姿态稳定和控制、无人机任务设备管理和应急控制三大类。
飞控按照是否公开源代码的方式分为开源飞控和商品飞控：（1）开源飞控世界各地的精英本着开源的精神，将所做的无人机工作开源开放，主要有MicroCopter, Paparazzi, AutoQuad, OpenPilot,MWC,KK, APM,PX4....MicroCopter, Paparazzi算是飞控开源的开山鼻祖，尤其Paparazzi以算法强大和稳定性著称，其下面也有很多版本，MCU采用ST和NXP的方案。
用户最多的应算APM，在经历基于Arduino的发展后，现在已经逐步过渡到PX4/Pixhawk平台，采用双ST MCU、冗余电源及传感器的方案，以满足未来更苛刻的需求和功能。
（2）商品飞控国外没有找到什么有名的商品飞控，国内到是有一堆，比如大疆的、极飞、零度、亿航....扒开外壳看下，大疆采用NXP方案(可以类比Paparazzi)，极飞ST，零度有个AT91+FPGA，亿航(呵呵，似乎是APM，只是不公开)，都可以从开源飞控找到他们影子，只要商品飞控有的功能，开源飞控都有，但是开源飞控有的功能，商品不一定有，那么商品飞控有什么特点呢？1)加了一个漂亮的外壳，尤其是铝合金的；2)加了个优化算法，比如大疆似乎有个H∞；3)简化了调参和线束。
APM飞控源码讲解
APM飞控系统介绍APM飞控系统是国外的一个开源飞控系统，能够支持固定翼，直升机，3轴，4轴，6轴飞行器。
在此我只介绍固定翼飞控系统。
飞控原理在APM飞控系统中，采用的是两级PID控制方式，第一级是导航级，第二级是控制级，导航级的计算集中在medium_loop( ) 和fastloop( )的update_current_flight_mode( )函数中，控制级集中在fastloop( )的stabilize( )函数中。
导航级PID控制就是要解决飞机如何以预定空速飞行在预定高度的问题，以及如何转弯飞往目标问题，通过算法给出飞机需要的俯仰角、油门和横滚角，然后交给控制级进行控制解算。
控制级的任务就是依据需要的俯仰角、油门、横滚角，结合飞机当前的姿态解算出合适的舵机控制量，使飞机保持预定的俯仰角，横滚角和方向角。
最后通过舵机控制级set_servos_4( )将控制量转换成具体的pwm信号量输出给舵机。
值得一提的是，油门的控制量是在导航级确定的。
控制级中不对油门控制量进行解算，而直接交给舵机控制级。
而对于方向舵的控制，导航级并不给出方向舵量的解算，而是由控制级直接解算方向舵控制量，然后再交给舵机控制级。
以下，我剔除了APM飞控系统的细枝末节，仅仅将飞控系统的重要语句展现，只浅显易懂地说明APM飞控系统的核心工作原理。
一，如何让飞机保持预定高度和空速飞行要想让飞机在预定高度飞行，飞控必须控制好飞机的升降舵和油门，因此，首先介绍固定翼升降舵和油门的控制，固定翼的升降舵和油门控制方式主要有两种：一种是高度控制油门，空速控制升降舵方式。
实际飞行存在四种情况，第一种情况是飞机飞行过程中，如果高度低于目标高度，飞控就会控制油门加大，从而导致空速加大，然后才导致拉升降舵，飞机爬升；第二种情况与第一种情况相反；第三种情况是飞机在目标高度，但是空速高于目标空速，这种情况飞控会直接拉升降舵，使飞机爬升，降低空速，但是，高度增加了，飞控又会减小油门，导致空速降低，空速低于目标空速后，飞控推升降舵，导致飞机降低高度。
深度解密MP-201M
重剑无锋大巧不工深度解析MP-201M概述成都纵横倾注三年心血、纠结三年的一套多旋翼自驾仪在本月总算修成正果。
三年心血、三年纠结所为何来？为啥要为这么一款产品纠结三年？要开发任何一款优秀的产品，都必须首先秉持一种理念、甚至是一种信仰；其次就是要在众多制约因素中进行折中和妥协，达到一种平衡。
那在这款MP-201M中，我们秉持了一种什么理念呢？我们秉持的是成都纵横自动化一贯的产品理念：“用户价值最大化”，就是要用我们的产品尽可能地为用户创造价值。
我们深信无法为用户创造价值的产品，无论多么便宜，用户都不会购买；反之只要产品所创造的价值远远超过用户为获取该产品所付出的代价，用户都是会购买的。
那我们在纠结什么呢？其实我们一直在拷问这三个问题：其一是多旋翼自驾仪还有我们的市场吗？其二是多旋翼自驾仪究竟该是个啥样？其三是多旋翼自驾仪真的就该是廉价的吗？现在无人机言必称多旋翼，自驾仪言必称开源，市场可以说是到了开始泛滥的地步了，一款最廉价的自驾仪只有几十块钱，一款最廉价的无人机可以不足一千块钱。
相比这些，我们的自驾仪最便宜也得三五万，贵的要到三五十万。
我们再做多旋翼自驾仪，卖个七八万的，谁还买啊？现在的多旋翼自驾仪都是一块海绵减振垫上粘个IMU组合模块、一块小MCU、几根插针一引就成了，不管有无干扰都伸出根棍子放上磁传感器和GPS天线，什么都是一副对付的架势。
而且还在毫无技术地彼此山寨，甚至不惜为一个海绵垫引起“专利血案”。
自驾仪的简陋到了一种让人心酸的地步，而用者浑然不知这种产品的危害，还在为这种廉价叫好。
难道多旋翼自驾仪就该是这个样子吗？这款MP-201M多旋翼自驾仪就是我们对上述三个问题回答。
为啥要为这么一款产品倾注三年心血？三年研制一款产品，而且还有大量的开源软硬件可以抄袭，还有同行同类产品可以山寨，确实是有些笨拙了（就像郭靖一直在那一招一招地练习潜龙勿用一样？）。
其实貌似原理简单的多旋翼自驾仪深究起来也是很有技术含量的，光一个姿态控制，就可以写好多篇博士论文了（而且都是国外名校的博士论文呢）。
Eterm学习笔记
状态及行动代码：DA指令显示营业员的工作号和本台终端的pidOffice号是pek099 改变office号 uc:pek001 修改成功会提是accepted改变airline ual:cz 修改成功没有提示，等待其他输入，修改不成功，提示数据说明: office号决定主控区域1-5行(A-E):五个工作区,用于使用者输入专用工作号的显示.其后面的英文AVAIL(或者UNAVAIL)表示工作区的状态为可用(不可用)，在pid建立时定义。
6行 PID：当前配置在系统中的设备号。
每一个E-TERM帐号的黑屏，在进入系统后都有一个固定的PID编号。
在账号建立时就分配好的HARDCOPY：当前PID相连的打印机设备号。
一般定义为1112，在用户需要使用自己的航信系统打印机配置时，才定义其他号。
7行 TIME:输入da指令时的系统时间DATE:输入da指令时的系统日期HOST:代理人分销系统名CAAC 如果是，表示所在主机分区“A”，航信分销系统将会进入系统的pid分配不同的处理主机，因此还可能显示B，C 。
8行 AIRLINE: 如果是1E表示代理人分销系统，1Es是代理分销系统的通用两字代码。
也有可能使航空公司的两字代码SYSTEM：系统版本号APPLICATION：主机分区情况登陆系统：$$open tipdxX=1 表示进入ics系统X=2 表示进入离港系统X=3 表示进入crs系统Eterm c 系统代理人系统Eterm b 系统航空公司系统Eterm j 系统离港系统1 AV指令:AV指令用于查询航班座位可利用情况,及其相关航班信息,如航班号、舱位、起飞时间、经停点等，是一个非常重要的指令。
指令格式：AV:选择项/城市对(两个城市的三字代码)/日期/起飞时间/航空公司代码(二字代码)/经停标识/座位等级上述各项中只用城市对必须写其他各项都是可选的选择项有三种：（默认是p）p 显示结果按照起飞时间先后顺序排列a 显示结果按照到达时间先后顺序排列e 显示结果按照飞行时间由短到长排列查询结果中如果航班号前面有*，则表示是代码共享2 FV指令：FV功能提供了最早有座位的航班信息，它显示内容与AV相似。
飞控代码
12 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44/*MultiWiiCopter by Alexandre DubusOctober 2011 V1.devThis program is free software: you can redistribute it and/or modifyit under the terms of the GNU General Public License as published bythe Free Software Foundation, either version 3 of the License, orany later version. see </licenses/>*/#include "config.h"#include "def.h"#define VERSION 19/*********** RC alias *****************/#define ROLL 0#define PITCH 1#define YAW 2#define THROTTLE 3#define AUX1 4#define AUX2 5#define CAMPITCH 6#define CAMROLL 7#define PIDALT 3#define PIDVEL 4#define PIDLEVEL 5#define PIDMAG 6#define BOXACC 0#define BOXBARO 1#define BOXMAG 2#define BOXCAMSTAB 3#define BOXCAMTRIG 4#define BOXARM 5#define BOXGPSHOME 6#define BOXGPSHOLD 7static uint32_t currentTime = 0;static uint16_t previousTime = 0;static uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loopstatic uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88decreases at each cycle down to 0, then we enter in a normal mode.static uint8_t calibratingM = 0;static uint16_t calibratingG;static uint8_t armed = 0;static uint16_t acc_1G; // this is the 1G measured accelerationstatic int16_t acc_25deg;static uint8_t nunchuk = 0;static uint8_t accMode = 0; // if level mode is a activatedstatic uint8_t magMode = 0; // if compass heading hold is a activatedstatic uint8_t baroMode = 0; // if altitude hold is activatedstatic uint8_t GPSModeHome = 0; // if GPS RTH is activatedstatic uint8_t GPSModeHold = 0; // if GPS PH is activatedstatic int16_t gyroADC[3],accADC[3],magADC[3];static int16_t accSmooth[3]; // projection of smoothed and normalized gravitation force vector on x/y/z axis, as measured by accelerometerstatic int16_t accTrim[2] = {0, 0};static int16_t heading,magHold;static uint8_t calibratedACC = 0;static uint8_t vbat; // battery voltage in 0.1V stepsstatic uint8_t okToArm = 0;static uint8_t rcOptions;static int32_t pressure;static int32_t BaroAlt;static int32_t EstVelocity;static int32_t EstAlt; // in cmstatic uint8_t buzzerState = 0;//for logstatic uint16_t cycleTimeMax = 0; // highest ever cycle timenstatic uint16_t cycleTimeMin = 65535; // lowest ever cycle timenstatic uint16_t powerMax = 0; // highest ever currentstatic uint16_t powerAvg = 0; // last known currentstatic uint8_t i2c_errors_count = 0; // count of wmp/nk resets// **********************// power meter// **********************#define PMOTOR_SUM 8 // index into pMeter[] for sumstatic uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sumstatic uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()static uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]89 90 91 92 93 94 95 96 97 98 99 10 0 10 1 10 2 10 3 10 4 10 5 10 6 10 7 10 8 10 9 11 0 11 1 11 2 11 3 11 4 11 5 11static uint8_t powerTrigger1 = 0; // trigger for alarm based on power consumption// **********************// telemetry// **********************static uint8_t telemetry = 0;static uint8_t telemetry_auto = 0;// ******************// rc functions// ******************#define MINCHECK 1100#define MAXCHECK 1900volatile int16_t failsafeCnt = 0;static int16_t failsafeEvents = 0;static int16_t rcData[8]; // interval [1000;2000]static int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE a nd [-500;+500] for ROLL/PITCH/YAWstatic uint8_t rcRate8;static uint8_t rcExpo8;static int16_t lookupRX[7]; // lookup table for expo & RC ratevolatile uint8_t rcFrameComplete; //for serial rc receiver Spektrum// **************// gyro+acc IMU// **************static int16_t gyroData[3] = {0,0,0};static int16_t gyroZero[3] = {0,0,0};static int16_t accZero[3] = {0,0,0};static int16_t magZero[3] = {0,0,0};static int16_t angle[2] = {0,0}; // absolute angle inclination in multiple of 0.1degree 180 deg = 1800static int8_t smallAngle18;// *************************// motor and servo functions// *************************static int16_t axisPID[3];static int16_t motor[8];static int16_t servo[4] = {1500,1500,1500,1500};// **********************// EEPROM & LCD functions6 117 118 119 12 0 12 1 12 2 12 3 12 4 12 5 12 6 12 7 12 8 12 9 13 0 13 1 13 2 13 3 13 4 13 5 13 6 13 7 13// **********************static uint8_t P8[7], I8[7], D8[7]; //8 bits is much faster and the code is much shorter static uint8_t dynP8[3], dynI8[3], dynD8[3];static uint8_t rollPitchRate;static uint8_t yawRate;static uint8_t dynThrPID;static uint8_t activate[8];void blinkLED(uint8_t num, uint8_t wait,uint8_t repeat) {uint8_t i,r;for (r=0;r<repeat;r++) {for(i=0;i<num;i++) {LEDPIN_SWITCH //switch LEDPIN stateBUZZERPIN_ON delay(wait); BUZZERPIN_OFF}delay(60);}}// **********************// GPS// **********************static int32_t GPS_latitude,GPS_longitude;static int32_t GPS_latitude_home,GPS_longitude_home;static uint8_t GPS_fix , GPS_fix_home = 0;static uint8_t GPS_numSat;static uint16_t GPS_distanceToHome;static int16_t GPS_directionToHome = 0;static uint8_t GPS_update = 0;void annexCode() { //this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microsecondsstatic uint32_t serialTime;static uint32_tbuzzerTime,calibratedAccTime,telemetryTime,telemetryAutoTime,psensorTime;static uint8_t buzzerFreq; //delay between buzzer ringuint8_t axis,prop1,prop2;uint16_t pMeterRaw, powerValue; //used for current reading//PITCH & ROLL only dynamic PID adjustemnt, depending on throttle valueif (rcData[THROTTLE]<1500) prop2 = 100;else if (rcData[THROTTLE]<2000) prop2 = 100 -(uint16_t)dynThrPID*(rcData[THROTTLE]-1500)/500;else prop2 = 100 - dynThrPID;813 9 14 0 14 1 14 2 14 3 14 4 14 5 14 6 14 7 14 8 14 9 15 0 15 1 15 2 15 3 15 4 15 5 15 6 15 7 15 8 15 9 16for(axis=0;axis<3;axis++) {uint16_t tmp = min(abs(rcData[axis]-MIDRC),500);#if defined(DEADBAND)if (tmp>DEADBAND) { tmp -= DEADBAND; }else { tmp=0; }#endifif(axis!=2) { //ROLL & PITCHuint16_t tmp2 = tmp/100;rcCommand[axis] = lookupRX[tmp2] + (tmp-tmp2*100) *(lookupRX[tmp2+1]-lookupRX[tmp2]) / 100;prop1 = 100-(uint16_t)rollPitchRate*tmp/500;prop1 = (uint16_t)prop1*prop2/100;} else { //YAWrcCommand[axis] = tmp;prop1 = 100-(uint16_t)yawRate*tmp/500;}dynP8[axis] = (uint16_t)P8[axis]*prop1/100;dynD8[axis] = (uint16_t)D8[axis]*prop1/100;if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis];}rcCommand[THROTTLE] = MINTHROTTLE +(int32_t)(MAXTHROTTLE-MINTHROTTLE)*(rcData[THROTTLE]-MINCHECK)/(2000-MINCHECK);#if (POWERMETER == 2)if (micros() > psensorTime + 19977 /*20000*/) { // 50Hz, but avoid bulking of timed taskspMeterRaw = analogRead(PSENSORPIN);powerValue = ( PSENSORNULL > pMeterRaw ? PSENSORNULL - pMeterRaw : pMeterRaw - PSENSORNULL); // do not use abs(), it would induce implicit cast to uint and overrun#ifdef LOG_VALUESif ( powerValue < 333) { // only accept reasonable values. 333 is empiricalif (powerValue > powerMax) powerMax = powerValue;powerAvg = powerValue;}#endifpMeter[PMOTOR_SUM] += (uint32_t) powerValue;psensorTime = micros();}#endif#if defined(VBAT)0 16 1 16 2 16 3 16 4 16 5 16 6 16 7 16 8 16 9 17 0 17 1 17 2 17 3 17 4 17 5 17 6 17 7 17 8 17 9 18 0 18 1 18static uint8_t ind;uint16_t vbatRaw = 0;static uint16_t vbatRawArray[8];vbatRawArray[(ind++)%8] = analogRead(V_BATPIN);for (uint8_t i=0;i<8;i++) vbatRaw += vbatRawArray[i];vbat = vbatRaw / (VBATSCALE/2); // result is Vbatt in 0.1V stepsif ( (vbat>VBATLEVEL1_3S)#if defined(POWERMETER)&& ( (pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0) ) #endif|| (NO_VBAT>vbat) ) // ToLuSe { //VBAT ok AND powermeter ok, buzzer offbuzzerFreq = 0; buzzerState = 0; BUZZERPIN_OFF;#if defined(POWERMETER)} else if (pMeter[PMOTOR_SUM] > pAlarm) { // sound alarm for powermeterbuzzerFreq = 4;#endif} else if (vbat>VBATLEVEL2_3S) buzzerFreq = 1;else if (vbat>VBATLEVEL3_3S) buzzerFreq = 2;else buzzerFreq = 4;if (buzzerFreq) {if (buzzerState && (currentTime > buzzerTime + 250000) ) {buzzerState = 0;BUZZERPIN_OFF;buzzerTime = currentTime;} else if ( !buzzerState && (currentTime > (buzzerTime +(2000000>>buzzerFreq))) ) {buzzerState = 1;BUZZERPIN_ON;buzzerTime = currentTime;}}#endifif ( (calibratingA>0 && (ACC || nunchuk) ) || (calibratingG>0) ) { // Calibration phasis LEDPIN_SW ITCH} else {if (calibratedACC == 1) {LEDPIN_OFF}if (armed) {LEDPIN_ON}}if (abs(angle[ROLL])>180 || abs(angle[PITCH])>180) smallAngle18 = 0; else smallAngle18 = 1; //more than 18 deg detectionif ( currentTime > calibratedAccTime ) {if (smallAngle18 == 0) {calibratedACC = 0; //the multi uses ACC and is not calibrated or is too much2 183 184 185 186 187 18 8 189 19 0 19 1 19 2 19 3 19 4 19 5 19 6 19 7 19 8 19 9 20 0 20 1 20 2 20 3 20inclinatedLEDPIN_SWITCHcalibratedAccTime = currentTime + 500000;} elsecalibratedACC = 1;}if (micros() > serialTime + 20000) { // 50HzserialCom();serialTime = currentTime + 20000;}#ifdef LCD_TELEMETRY_AUTOif ( (telemetry_auto) && (micros() > telemetryAutoTime +LCD_TELEMETRY_AUTO) ) { // every 2 secondstelemetry++;if (telemetry == 'E') telemetry = 'Z';else if ( (telemetry < 'A' ) || (telemetry > 'D' ) ) telemetry = 'A';telemetryAutoTime = micros(); // why use micros() and not the variable currentTime ?}#endif#ifdef LCD_TELEMETRYif (micros() > telemetryTime + LCD_TELEMETRY) { // 10Hzif (telemetry) lcd_telemetry();telemetryTime = micros();}#endif}void setup() {Serial.begin(SERIAL_COM_SPEED);LEDPIN_PINMODEPOWERPIN_PINMODEBUZZERPIN_PINMODESTABLEPIN_PINMODEPOWERPIN_OFFinitOutput();readEEPROM();checkFirstTime();configureReceiver();initSensors();previousTime = micros();#if defined(GIMBAL)calibratingA = 400;4 205 206 207 208 209 21 0 21 1 21 2 21 3 21 4 21 5 21 6 21 7 21 8 21 9 22 0 22 1 22 2 22 3 22 4 22 5 22#endifcalibratingG = 400;#if defined(POWERMETER)for(uint8_t i=0;i<=PMOTOR_SUM;i++)pMeter[i]=0;#endif#if defined(GPS)GPS_SERIAL.begin(GPS_BAUD);#endif#if defined(LCD_ETPP)i2c_ETPP_init();i2c_ETPP_set_cursor(0,0);LCDprintChar("MultiWii");i2c_ETPP_set_cursor(0,1);LCDprintChar("Ready to Fly!");#endif}// ******** Main Loop *********void loop () {static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motorsuint8_t axis,i;int16_t error,errorAngle;int16_t delta,deltaSum;int16_t PTerm,ITerm,DTerm;static int16_t lastGyro[3] = {0,0,0};static int16_t delta1[3],delta2[3];static int16_t errorGyroI[3] = {0,0,0};static int16_t errorAngleI[2] = {0,0};static uint32_t rcTime = 0;static int16_t initialThrottleHold;static int16_t errorAltitudeI = 0;int16_t AltPID = 0;static int16_t lastVelError = 0;static int32_t AltHold = 0.0;#if defined(ESC_CALIBRATE)blinkLED(20,30,1);while (true) {if (currentTime > (rcTime + 20000) ) { // 50HzrcTime = currentTime;computeRC();rcData[THROTTLE] = constrain(rcData[THROTTLE],900,MAXTHROTTLE);for (uint8_t i =0;i<NUMBER_MOTOR;i++) motor[i]=rcData[THROTTLE];6 227 228 229 23 0 23 1 23 2 23 3 23 4 23 5 23 6 23 7 23 8 23 9 24 0 24 1 24 2 24 3 24 4 24 5 24 6 24 7 24writeMotors();}currentTime = micros();}#endif#if defined(SPEKTRUM)if (rcFrameComplete) computeRC();#endifif (currentTime > rc Time ) { // 50HzrcTime = currentTime + 20000;#if !(defined(SPEKTRUM) || (BTSERIAL))computeRC();#endif// Failsafe routine - added by MIS#if defined(FAILSAFE)if ( failsafeCnt > (5*FAILSAVE_DELAY) && armed==1) { // Stabilize, and set Throttle to specified levelfor(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)rcData[THROTTLE] = FAILSAVE_THR0TTLE;if (failsafeCnt > 5*(FAILSAVE_DELAY+FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)armed = 0; //This will prevent the copter to automatically rearm if failsafe shuts it down and preventsokToArm = 0; //to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm}failsafeEvents++;}failsafeCnt++;#endif// end of failsave routine - next change is made with RcOptions settingif (rcData[THROTTLE] < MINCHECK) {errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;rcDelayCommand++;if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && armed == 0) { if (rcDelayCommand == 20) calibratingG=400;} else if (rcData[YAW] > MAX CHECK && rcData[PITCH] > MAXCHECK && armed == 0) {if (rcDelayCommand == 20) {servo[0] = 1500; //we center the yaw gyro in conf modewriteServos();8 24 9 25 0 25 1 25 2 25 3 25 4 25 5 25 6 25 7 25 8 25 9 26 0 26 1 26 2 26 3 26 4 26 5 26 6 26 7 26 8 26 9 27#if defined(LCD_CONF)configurationLoop(); //beginning LCD configuration#endifpreviousTime = micros();}} else if (activate[BOXARM] > 0) {if ((rcOptions & activate[BOXARM]) && okToArm ) armed = 1;else if (armed) armed = 0;rcDelayCommand = 0;} else if ( (rcData[YAW] < MINCHECK || rcData[ROLL] < MINCHECK) && armed == 1) {if (rcDelayCommand == 20) armed = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged} else if ( (rcData[YAW] > MAX CHECK || rcData[ROLL] > MAXCHECK) && rcData[PITCH] < MAX CHECK && armed == 0 && calibratingG == 0 && calibratedACC == 1) {if (rcDelayCommand == 20) armed = 1;#ifdef LCD_TELEMETRY_AUTO} else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {if (rcDelayCommand == 20) {if (telemetry_auto) {telemetry_auto = 0;telemetry = 0;} elsetelemetry_auto = 1;}#endif} elsercDelayCommand = 0;} else if (rcData[THROTTLE] > MAXCHECK && armed == 0) {if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) {if (rcDelayCommand == 20) calibratingA=400;rcDelayCommand++;} else if (rcData[PITCH] > MAXCHECK) {accTrim[PITCH]+=2;writeParams();} else if (rcData[PITCH] < MINCHECK) {accTrim[PITCH]-=2;writeParams();} else if (rcData[ROLL] > MAXCHECK) {accTrim[ROLL]+=2;writeParams();} else if (rcData[ROLL] < MINCHECK) {accTrim[ROLL]-=2;writeParams();} else {rcDelayCommand = 0;0 27 1 27 2 27 3 27 4 27 5 27 6 27 7 27 8 27 9 28 0 28 1 28 2 28 3 28 4 28 5 28 6 28 7 28 8 28 9 29 0 29 1 29}}#ifdef LOG_VALUESelse if (armed) { // update min and max values here, so do not get cycle time of the motor arming (which is way higher than normal)if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // remember highscore if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // remember lowscore }#endifrcOptions = (rcData[AUX1]<1300) + (1300<rcData[AUX1] &&rcData[AUX1]<1700)*2 + (rcData[AUX1]>1700)*4+(rcData[AUX2]<1300)*8 + (1300<rcData[AUX2] &&rcData[AUX2]<1700)*16 + (rcData[AUX2]>1700)*32;//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false if (((rcOptions & activate[BOXACC]) || (failsafeCnt > 5*FAILSAVE_DELAY) ) && (ACC || nunchuk)) {// bumpless transfer to Level modeif (!accMode) {errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;accMode = 1;}} else accMode = 0; // modified by MIS for failsave supportif ((rcOptions & activate[BOXARM]) == 0) okToArm = 1;if (accMode == 1) STABLEPIN_ON else STABLEPIN_OFF;if(BARO) {if (rcOptions & activate[BOXBARO]) {if (baroMode == 0) {baroMode = 1;AltHold = EstAlt;initialThrottleHold = rcCommand[THROTTLE];errorAltitudeI = 0;lastVelError = 0;EstVelocity = 0;}} else baroMode = 0;}if(MAG) {if (rcOptions & activate[BOXMAG]) {if (magMode == 0) {magMode = 1;2 293 294 295 296 297 298 29 9 30 0 30 1 30 2 30 3 30 4 30 5 30 6 30 7 30 8 30 9 31 0 31 1 31 2 31 3 31magHold = heading;}} else magMode = 0;}#if defined(GPS)if (rcOptions & activate[BOXGPSHOME]) {GPSModeHome = 1;}else GPSModeHome = 0;if (rcOptions & activate[BOXGPSHOLD]) {GPSModeHold = 1;}else GPSModeHold = 0;#endif}if (MAG) Mag_getADC();if (BARO) Baro_update();computeIMU();// Measure loop rate just afer reading the sensorscurrentTime = micros();cycleTime = currentTime - previousTime;previousTime = currentTime;if(MAG) {if (abs(rcCommand[YAW]) <70 && magMode) {int16_t dif = heading - magHold;if (dif <= - 180) dif += 360;if (dif >= + 180) dif -= 360;if ( smallAngle18 ) rcCommand[YAW] -= dif*P8[PIDMAG]/30; //18 deg} else magHold = heading;}if(BARO) {if (baroMode) {if (abs(rcCommand[THROTTLE]-initialThrottleHold)>20) {baroMode = 0;errorAltitudeI = 0;}//**** Alt. Set Point stabilization PID ****error = constrain( AltHold - EstAlt, -1000, 1000); // +/-10m, 1 decimeter accuracy errorAltitudeI += error;errorAltitudeI = constrain(errorAltitudeI,-30000,30000);PTerm = P8[PIDALT]*error/100; // 16 bits is ok hereITerm = (int32_t)I8[PIDALT]*errorAltitudeI/40000;AltPID = PTerm + ITerm ;431 5 31 6 31 7 31 8 31 9 32 0 32 1 32 2 32 3 32 4 32 5 32 6 32 7 32 8 32 9 33 0 33 1 33 2 33 3 33 4 33 5 33//**** Velocity stabilization PD ****error = constrain(EstVelocity*2, -30000, 30000);delta = error - lastVelError;lastVelError = error;PTerm = (int32_t)error * P8[PIDVEL]/800;DTerm = (int32_t)delta * D8[PIDVEL]/16;rcCommand[THROTTLE] = initialThrottleHold + constrain(AltPID - (PTerm - DTerm),-100,+100);}}//**** PITCH & ROLL & YAW PID ****for(axis=0;axis<3;axis++) {if (accMode == 1 && axis<2 ) { //LEVEL MODE// 50 degrees max inclinationerrorAngle = constrain(2*rcCommand[axis],-500,+500) - angle[axis] +accTrim[axis]; //16 bits is ok here#ifdef LEVEL_PDFPTerm = -(int32_t)angle[axis]*P8[PIDLEVEL]/100 ;#elsePTerm = (int32_t)errorAngle*P8[PIDLEVEL]/100 ; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result#endiferrorAngleI[axis] =constrain(errorAngleI[axis]+errorAngle,-10000,+10000); //WindUp //16 bits is ok hereITerm = ((int32_t)errorAngleI[axis]*I8[PIDLEVEL])>>12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result } else { //ACRO MODE or YAW axisif (abs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/P8[axis] ; //16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)else error = (int32_t)rcCommand[axis]*10*8/P8[axis] ; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)error -= gyroData[axis];PTerm = rcCommand[axis];6 337 338 339 34 0 34 1 34 2 34 3 34 4 34 5 34 6 34 7 34 8 34 9 35 0 35 1 35 2 35 3 35 4 35 5 35 6 35 7 35errorGyroI[axis] =constrain(errorGyroI[axis]+error,-16000,+16000); //WindUp //16 bits is ok here if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;ITerm = (errorGyroI[axis]/125*I8[axis])>>6; //16 bits is ok here 16000/125 = 128 ; 128*250 = 32000}if (abs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; //16 bits is needed for calculation 160*200 = 32000 16 bits is ok for resultelse PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; //32 bits is needed for calculationdelta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800lastGyro[axis] = gyroData[axis];deltaSum = delta1[axis]+delta2[axis]+delta;delta2[axis] = delta1[axis];delta1[axis] = delta;if (abs(deltaSum)<640) DTerm =(deltaSum*dynD8[axis])>>5; //16 bits is needed for calculation640*50 = 32000 16 bits is ok for resultelse DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; //32 bits is needed for calculationaxisPID[axis] = PTerm + ITerm - DTerm;}mixTable();writeServos();writeMotors();//GPS#if defined(GPS)while (GPS_SERIAL.available()) {if (GPS_newFrame(GPS_SERIAL.read())) {if (GPS_update == 1) GPS_update = 0; else GPS_update = 1;if (GPS_fix == 1) {if (GPS_fix_home == 0) {GPS_fix_home = 1;GPS_latitude_home = GPS_latitude;GPS_longitude_home = GPS_longitude;}GPS_distance(GPS_latitude_home,GPS_longitude_home,GPS_latitude,GPS_l ongitude, &GPS_distanceToHome, &GPS_directionToHome);8 35 9 36 0}}}#endif}Some of my Experiences with Kalman FiltersRonald YannoneWhen I first started working at General Electric Company in Utica, NY in 1976, I had the opportunity to code up a Kalman filter for a target tracking application in BASIC programming language. Since that first experience, I was in love with the Kalman filter and have designed many for a variety of military applications since. In this introductory article I would like to share some of my experiences with Kalman filter design over the past 20 years.R. Kalman put out the first article in 1960. A Kalman filter is simply an optimal recursive data processing algorithm. There are many ways of defining optimal, dependent upon the criteria chosen to evaluate performance. One aspect of this optimality is that the Kalman filter incorporates all information that can be provided to it. It processes all available measurements, regardless of their precision, to estimate the current value of all the variables of interest, with the use of (1) knowledge of the system and measurement device dynarnics, (2) the statistical description of the system noises, measurement errors, and uncertainty in the dynamics models, and (3) any available information about initial conditions of the variables of interest.The word recursive means that, unlike certain data processing concepts, the Kalman filter does not require all previous data to be kept in storage and reprocessed every time a new measurement is taken. The "filter" is actually a data processing algorithm. Despite the typical connotation of a filter as a "black box" containing electrical networks, the fact is that in most practical applications, the "filter" is just a computer program in the central processor. As such, it inherently incorporates discrete time measurement samples rather than continuous time inputs.The five extended Kalman filter (EKF) equations are given in the below. They are called extended because the partial derivatives of the measurement with respect to the state variables is used in generating the measurement transformation matrix (H). The nomenclature above the various matrices indicate "prior to" or "after" a measurement vector has been incorporated. The overall "control" mechanisms for the Kalman filter are given in the table. The Kalman filter can be implemented in a variety of ways. Some include different coordinate frames such as polar or Cartesian, standard or square root formulations to handle single precision arithmetic processing systems, aswell as the use of dedicated hardware boards that exploit the highly parallel arithmetic structure for purposes of order-of-magnitude processing speed.Applications I have designed Kalman filters for include radar and infrared search and track (IRST) for both fire control on tactical aircraft and surveillance on surveillance platforms, electronic warfare (EW) related sensors such as electronic support measures (ESM) and missile warning receivers (MVM) for tactical aircraft, and ground- and airbased systems. The targets tracked include other airplanes, ground emitter sites, and missiles (surface-to-air [SAM], air-to-air [AAM], and ballistic). The application areas include both passive and active sensors and the limitations they bring in actual mission scenarios.Some applications are considered unobservable in the estimation sense of the word. An example includes passive ranging of an airborne target. In this application, the use of angle-only data are used to estimate the range of the target airplane. Assuming the ownship (friendly aircraft) and target aircraft fly straight and level with constant velocities, range is unobservable (i.e., cannot be estimated). To turn the tracking problem into an observable one, the ownship must induce certain maneuvers. Namely, by inducing one derivative higher in its dynamics than the target aircraft range becomes observable. The ideal maneuver is the corkscrew where the observability capitalizes on the dimensional aspects of the problem at the same time. Many hours have led to very effective solutions to this problem. For the IRST application, the use of snake maneuvers with prescribed time-constraints and range estimation convergence limits were met.Designing Kalman filters is an art, despite its strong theoretical and mathematical basis. Each application has its challenges. Common to all applications I have dealt with is the initialization of the error covariance matrix (P) and state vector (X). The more difficult problems are the passive sensor air-to-air applications - whereangle-only measurements are available and when the measurement accuracy is poor. This forces the designer to make educated guesses of the target aircraft's range, speed and heading.In some applications where the onboard sensor's detection range is well known, or the corridor where the target aircraft is approaching from is known, and target identification information is available, the "guessing" can be constrained. This applies to the passive time-to-interccpt (Tn) estimation for AAM's as well, where the additional unknown is the acceleration profile. In such difficult applications, the use of multiple model designs are useful. In essence, we run multiple Kalman filters in parallel and "blend" their results using a variety of methods. The idea behind this approach is to "capture" the spread of initial conditions and target dynamics possible. Although the approach requires considerably more processing intensive, its effectiveness is well worth its use.。
F2S飞控使用说明飞控OSD显示界面数据定义OSD数据解释飞机
F2S飞控使用说明飞控OSD显示界面数据定义：OSD数据解释飞机所在位置方位角：指飞机相对于起飞地点，现在所处的位置，如：，指飞机现处于相对于飞起地点正南方向（正北为0度，180度为正南）。
飞机前进方向：指飞机现在正在前进的方向，如：，指飞机正在向53度的方向（正北为0度）前进。
飞机返航角度：指飞机当时回家的偏差角度，如，指飞机向左53度，为回家的方向。
再如：，指飞机现在向右68度，为回家的方向！返航角度是最重要的数据之一！尤其要注意。
直线距离：指飞机距离回家位置的地面直线距离，单位是米。
高度：相对于“家”的位置的相对高度。
单位是米。
航程：飞机累计飞行距离。
T: 飞控油门大小，此数值与设置里的最低油门相一致，便于飞行时候判断油门大小RSSI：这个是飞控RSSI接线端口的电压值，单位为0.1V，测量电压范围为0-3.3V。
如显示R32,则表示电压为3.2V。
这个端口常用于连接接收机场强段，以显示接收机信号的好坏情况，如果是433接收机，则显示的是丢包率，电压3.2（或者3.3）为信号100%接收，0为失控。
根据这个数值，可以看出接收机的状况。
飞机姿态：显示的是飞机俯仰的角度和左右倾斜的角度，根据向小飞机的图标，可以看出飞机实时姿态。
如，表示飞机右倾3度，飞机机头向下1度。
起飞计时器：当油门超过30%后，保持1秒以上，计时器就开始计时。
当开始计时后，设置模式就进不去了，必须重新接电，在计时器没有开始计时前，才能进入设置模式。
北京时间：指飞行的时间，可以用于记录。
雷达：中间表示“家”的位置，小飞翼表示飞机，飞翼的方向表示飞机前进的方向。
雷达屏幕上，上北下南左西右东，表示飞机做处的位置。
角度说明图：安装飞控的时候F2飞控箭头方向朝向机头。
OSD设置界面定义(仅供参考，一样的)：有关设置方法和接线方法请参考以下视频链接（此视频是F2飞控的视频教程，F2S接线方法可以仅供参考，具体详细细节需要亲们对照着F2S的定义图来接线，大致上都是差不多的）：/u/2388349636/v（视频链接）飞控主板接线定义：图文详解:输入信号：1：副翼通道2：水平尾翼通道3：油门通道4：模式选择通道(GPS没有定位之前，三段开关控全部都是平衡模式，只有收到6颗星以上，才可以转换出其他控制模式。
飞行品质监控的基础记录参数解析
飞行品质监控的基础—记录参数解析民航总局航空安全技术中心李斌山东航空股份有限公司王群宁引言迄今为止航空公司开展飞行品质监控（Flight Operational Quality Assurance，以下简称FOQA）工作已有多年，FOQA在航空公司的日常运营、安全管理和飞行部队的技术管理等方面发挥着重要作用。
FOQA的核心内容是应用实际飞行产生的飞行数据，按照设定的项目与标准，对飞行操纵、飞机性能等进行监控。
目前国内航空公司使用的FOQA监控软件主要是FLIDRAS、AGS、GRAF、LOMS等。
无论航空公司使用哪一种监控软件，一个必须完成的基础前提工作就是在监控软件上建立译码参数库。
飞行数据以二进制数据流的形式记录于FDR或QAR中，在监控软件上按照波音或空客的记录参数定义规范建立译码参数库，就可以把二进制数据转换为工程值数据。
译码参数库的准确性将直接影响飞行品质监控的结果。
因此，从事飞行品质监控工作的人员必须对记录参数有准确的理解和认识，包括参数的定义、数据来源、记录精度、记录频率、转换方法等内容，还要及时总结建立译码参数库的注意事项。
记录参数的发展记录参数的发展经历了从5参数～11参数～17参数～28参数～34参数～57参数～88参数的过程，这里所指的参数是强制性参数，即必须记录的参数。
按照ARINC542记录器规范，数据采集和记录组成一个部件，即数据记录器。
模拟参数直接传输到记录器，数据量很小。
最初只记录五个参数：高度、空速、航向、垂直加速度和时间计数。
1991年10月国际民航组织（ICAO）、美国联邦航空局（FAA）等一些民航组织机构提高了记录参数标准，强制性要求在1991年以前生产的飞机上安装的记录器至少要记录11个（类）参数，1991年以后生产的飞机上安装的记录器至少要记录28个（类）参数。
记录参数增加了俯仰、滚转、纵向加速度、发动机推力和驾驶杆位置等，但这些参数还都是模拟信号，即从机载传感器直接获得信号。
飞控算法讲解
四旋翼垂直运动示意图
六种飞行状态
俯仰运动
在四旋翼完成4轴平衡的条件下， 在图中，电机1、4的转速上升，电 机2、3的转速下降（改变量大小应 相等，在PID程序的实现中也有体 现）。由于旋翼1、4的转速即升力 上升，旋翼 2、3的转速即升力下降， 产生的不平衡力矩使机身绕Roll轴旋 转，同理，当电机1、4的转速下降， 电机 2、3的转速上升，机身便绕 Roll轴向另一个方向运动，实现飞行 器的俯仰运动。
思考： 如果油门基础量较小，控制量较大，会发生什么情况？ 答： 一旦控制飞行时，调节平衡能力就较差，且受到外界干扰后，控制量较大， 四旋翼处于较不稳定状态。
控制难点
• 自身中心偏移 • 自身气流干扰 • 外界气流干扰 • 多变量、非线性、强耦合 • 欠驱动系统 • 传感器精度低、误差积累
PID控制器
Throttle_Info.M2 = + pidRatePitch.value - pidRateRoll.value + pidAccHeight.value + pidRateYaw.value + throttleBasic;
Throttle_Info.M3 = + pidRatePitch.value + pidRateRoll.value + pidAccHeight.value - pidRateYaw.value + throttleBasic;
单级PID 例程请见：Single-loop PID_control
姿态控制要点
+ 期望角 度 -
比 例
积 分
++
+ -
微
分
CUSTOMIZE Conveniently iterate top-line alignments for wireless metrics.
飞控代码
u8 TIM4CH3_CAPTURE_STA=0;//输入捕获状态
u16TIM4CH3_CAPTURE_VAL;//输入捕获值
u8 TIM4CH4_CAPTURE_STA=0;//输入捕获状态
u16TIM4CH4_CAPTURE_VAL;//输入捕获值
//定时器4中断服务程序
void TIM4_IRQHandler(void)
{
if (TIM_GetITStatus(TIM4, TIM_IT_CC1) != RESET) //捕获1发生捕获事件
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC1); //清除中断标志位
if(TIM4CH1_CAPTURE_STA&0X40)//捕获到一个下降沿
{
TIM4CH4_CAPTURE_STA|=0X80;//标记成功捕获到一次上升沿
TIM4CH4_CAPTURE_VAL=TIM_GetCapture4(TIM4);
TIM_OC4PolarityConfig(TIM4,TIM_ICPolarity_Rising);//CC1P=0设置为上升沿捕获
}else//还未开始,第一次捕获上升沿
{
TIM4CH4_CAPTURE_STA=0;//清空
TIM4CH4_CAPTURE_VAL=0;
TIM_SetCounter(TIM4,0);
TIM4CH4_CAPTURE_STA|=0X40;//标记捕获到了上升沿
TIM_ICInit(TIM4, &TIM4_ICInitStructure);
TIM4_ICInitStructure.TIM_Channel = TIM_Channel_2; //CC1S=01选择输入端IC1映射到TI1上
python编写无人机程序代码
Python编写无人机程序代码一、概述随着科技的不断发展，无人机在军事、航空航天、自然资源调查等领域的应用越来越广泛。
作为一种可以自主飞行的飞行器，无人机的应用场景非常多样化，而程序代码则是无人机能够完成各种任务的重要保障。
Python作为一种易学易用的编程语言，也被广泛应用于无人机程序开发中。
二、Python在无人机编程中的应用1. 作为通用编程语言，Python可以被应用在无人机的各个环节。
它可以用来处理无人机传感器获取的数据，进行飞行控制、路径规划、图像处理等操作。
Python还可以用来实现无人机的地面控制软件，以及与无人机相关的数据分析和可视化工作。
2. 在无人机的飞行控制方面，Python可以通过调用各种无人机控制库来实现飞行控制的功能。
使用DroneKit可以让开发者通过Python代码来控制无人机的飞行、姿态、导航等功能，而使用MAVLink库则可以让无人机和地面站之间进行通信和数据交换。
3. 对于无人机的图像处理及计算机视觉方面，Python也有着丰富的应用。
可以使用OpenCV库对无人机拍摄的图像进行处理，实现目标识别、跟踪、避障等功能。
Python还可以结合深度学习框架，如TensorFlow和PyTorch，来进行复杂的图像识别和处理任务。
三、Python编写无人机程序代码的优势和挑战1. 优势：Python具有简洁明了的语法，易于学习和使用。
Python也有着强大的社区支持，丰富的第三方库和模块可以为无人机程序开发提供便利。
Python还是一种跨评台的编程语言，可以在各种操作系统上运行，为无人机控制系统的开发提供了便利。
2. 挑战：虽然Python有着不少优势，但在无人机程序开发中也面临一些挑战。
Python的执行效率相对较低，对于一些对计算速度要求较高的无人机任务而言，可能需要考虑使用C++等性能更高的语言来编写程序。
Python的动态性和灵活性也为程序的稳定性和安全性带来了一定的挑战，需要开发者对程序设计和测试有着更高的要求。
kelm代码解析
kelm代码解析一、KELM代码简介KELM（Kernelized Extreme Learning Machine）是一种基于核方法的极限学习机算法。
与其他机器学习算法类似，KELM旨在通过学习输入特征和输出标签之间的非线性关系，从而实现对未知数据的分类或回归任务。
在我国科研人员的努力下，KELM代码已经得以实现并广泛应用于各个领域。
二、KELM算法原理KELM算法的核心思想是基于核方法的非线性映射。
核方法通过将输入数据映射到高维特征空间，从而使得原本在低维空间中难以解决的分类或回归问题在高维空间中变得容易。
KELM算法在此基础上，利用极限学习机（ELM）的快速学习特性，实现对高维特征空间中的非线性关系进行学习。
三、KELM代码实现详解KELM代码的实现主要包括以下几个步骤：1.数据预处理：对输入数据和标签进行归一化处理，使得数据在同一尺度上进行计算。
2.选择核函数：根据问题需求，选择合适的核函数（如径向基核、Sigmoid核等）。
3.初始化参数：设置极限学习机的输入权重、偏置项和核参数。
4.迭代优化：利用梯度下降法或其他优化算法，不断更新极限学习机的参数，直至收敛。
5.分类或回归预测：利用训练好的极限学习机对未知数据进行分类或回归预测。
四、KELM代码应用场景KELM代码在许多实际应用场景中表现出良好的性能，如文本分类、图像识别、生物信息学和金融风险管理等。
相较于其他算法，KELM具有较快的收敛速度和较高的预测准确性，能够有效地解决非线性分类和回归问题。
五、总结与展望KELM代码作为一种基于核方法的极限学习机算法，在非线性分类和回归领域具有广泛的应用前景。
随着科研人员对KELM算法的不断优化和改进，其在实际应用中的性能也将进一步提升。
无人机中的PID控制代码略解
⽆⼈机中的PID 控制代码略解⽆⼈机中的PID 控制代码略解PID 的控制规律：参考：Amov实验室-PX4中级课程-PID基础频域函数：写成时域的函数：假设采样间隔为T，则在第K个T时刻，有：=err (s )u (s )k +p +s k ik sd u (t )=k (err (t )+p err (t )dt +T i 1∫T )d dt derr (t )(1)err (K )=R (K )−in C (K )out对于积分环节，按照定义则有：对于微分环节，按照定义则有：将代⼊回则有：化简⼀下则有:为了减⼩存储空间，还有增量式PID，可写为：则还可以写为：PID 的C 语⾔代码初步实现：I (K )=T ∗(err (0)+err (1)+⋯+err (K ))=T err (i )i =0∑K(2)D (K )=Terr (K )−err (K −1)(3)(2),(3)(1)u (K )=k [err (K )+p err (i )+T i Ti =0∑K T ]d T err (K )−err (K −1)u (K )=k err (K )+p k err (i )+i i =0∑K k [err (K )−d err (K −1)](4)Δu (K )=k [err (K )−p err (K −1)]+k ⋅i err (K )+k [err (K )−d 2err (K −1)+err (K −2)]u (K )u (K )=u (K −1)+Δu (K )//定义pid结构体，定义主要参数struct _pid{float SetSpeed;float ActualSpeed;float err;float err_last; //err(K-1)float Kp,Ki,Kd;float voltage;//Object Signalfloat integral;float err_llast;//err(K-2)}pid;//初始化pid模块void PID_init(){pid.SetSpeed = 0;pid.ActualSpeed=0.0;pid.err = 0;pid.err_last = 0;pid.voltage = 0;pid.integral = 0;//Set Each PID Gainpid.Kp = 0.4;pid.Ki = 0.3;pid.Kd = 0.2;pid.err_llast = 0;}//pid实现，包括增量形式的pid，输⼊为⽬标速度，输出为当前的速度float PID_realize(float speed){pid.SetSpeed = speed;pid.err = pid.SetSpeed - pid.ActualSpeed;//PID Calculatepid.voltage = pid.Kp*pid.err + \pid.Ki*pid.integral + \pid.Kd*(pid.err-pid.err_last);//Increment Type/*float incrementVoltage;incrementVoltage = pid.Kp*(pid.err-pid.err_last)+\pid.Ki*pid.err+\pid.Kd*(pid.err-2*pid.err_last+pid.err_llast);pid.voltage += incrementVoltage;pid.err_llast = pid.err_last;*///Update Parameterspid.err_last = pid.err;pid.ActualSpeed = pid.voltage*1.0;return pid.ActualSpeed;}PX4中姿态控制介绍：PIDPX4中的控制主要分为***姿态控制*** 和***位置控制***，这两者都采⽤***串级*** 控制，这样的好处是可以增加系统的稳定性，抗⼲扰，因为参数更多了，直观上⾃然能够适应的情况就相对来说多⼀点。
飞控控制方法学习
飞控控制方法学习“飞控”是指无人机的飞行控制器，用于自动化保持飞行器处于一个特定的状态（悬停、飞行等）。
由于无人机经常处于“超视距”的环境飞行，所以自动化控制的飞控对于无人机来说是不能缺少的。
好的飞控，还会搭配不少有用的功能，方便控制者进行复杂的运动。
多轴飞行器的飞控主要由主控器、姿态感应器、GPS天线以及电子罗盘组成，周边还会有电源控制模块、LED提示灯、蓝牙连接模块等等。
一个旋翼类无人机系统的算法主要有两类：姿态检测算法、姿态控制算法。
姿态控制、被控对象（即四旋翼无人机）、姿态检测三个部分构成一个闭环控制系统。
被控对象的模型是由其物理系统决定，我们设计无人机的算法就是设计姿态控制算法、姿态检测算法。
姿态检测算法姿态的三个自由度可以用欧拉角表示，也可以用四元数表示。
姿态检测算法的作用就是将加速度计、陀螺仪等传感器的测量值解算成姿态，进而作为系统的反馈量。
常用的姿态检测算法有卡尔曼滤波、互补滤波等。
姿态控制算法控制飞行器姿态的三个自由度，以给定姿态与姿态检测算法得出的姿态偏差作为输入，被控对象模型的输入量作为输出（例如姿态增量），从而达到控制飞行器姿态的作用。
最常用的就是PID控制及其各种PID扩展（分段、模糊等）、高端点的有自适应控制。
当然，姿态控制算法里面又常用角速度、角度双闭环控制，所以常常有PID外环+PID内环等等，这些细节就不说了。
PID算法对于过程控制的典型对象-“一阶滞后+纯滞后”与“二阶滞后+纯滞后”的控制对象，PID控制器是一种最佳控制。
PID调节规律是连续系统动态品质校正的一种有效方法。
PID调节器类型：1。
比例调节器 2.比例积分调节器 3.比例微分调节器 4.比例积分微分调节器a.比例调节器微分方程：Y=KpE(t) Y为调节器输出；Kp为比例系数；E（t)为调节器输入偏差。
调节器的输出与输出偏差成正比，偏差出现，就能即使产生与之成比例的调节作用，具有调节及时的特点。
b.比例积分调节器：积分作用是指调节器的输出与输入偏差的积分比例的作用。
无人机开源控制代码
无人机开源控制代码1. 引言无人机作为一种重要的航空器，具有广泛的应用前景。
然而，传统的商业无人机往往受限于封闭的硬件和软件平台，无法满足各种复杂应用场景的需求。
因此，开源控制代码的出现为无人机的发展带来了新的机遇。
2. 开源控制代码的意义2.1 提高可定制性开源控制代码允许用户自由修改和定制飞行控制算法，以适应各种特定的应用场景。
这种定制化的能力使得无人机可以根据实际需求进行功能扩展和优化，提高整体性能。
2.2 促进技术创新开源控制代码使得无人机开发者们可以共享经验和知识，相互学习和借鉴，从而促进技术创新的发展。
通过开源代码的共享，开发者们可以快速迭代和改进算法，推动整个无人机行业的进步。
2.3 降低研发成本商业无人机往往价格昂贵，不利于研究人员和爱好者的研发。
而开源控制代码的出现可以降低无人机的研发成本，使得更多的人能够参与到无人机的开发中来。
这将进一步推动无人机技术的普及和发展。
3. 开源控制代码的应用3.1 无人机竞赛开源飞控代码在无人机竞赛中得到了广泛应用。
竞赛选手可以根据比赛要求和自己的需求，对开源代码进行定制和优化，从而提高无人机的操作性能和竞争力。
3.2 农业领域无人机在农业领域的应用越来越受到关注。
通过使用开源控制代码，农民可以根据实际需求，自行定制植保无人机的控制系统，实现精准喷洒和精细管理，提高农作物的收成和品质。
3.3 搜索救援开源控制代码可以帮助开发者开发具有自主搜索和救援能力的无人机。
这些无人机可以通过搭载各种传感器和算法，快速定位和救援被困人员，提高救援效率和成功率。
3.4 学术研究开源控制代码提供了一个极好的平台，供学术研究人员进行无人机相关算法的研究和验证。
研究人员可以根据自己的需求，进行算法的优化和改进，从而推动无人机相关研究的进展。
4. 开源控制代码的选择与使用4.1 选择合适的飞控硬件开源控制代码通常与特定的飞控硬件相匹配。
在选择开源控制代码之前，需要先了解不同飞控硬件的特点和性能，选择适合自己需求的飞控硬件。
匿名四轴上位机使用手册
匿名四轴上位机使⽤⼿册匿名四轴上位机使⽤⼿册⽬录1、串⼝功能软件界⾯串⼝功能和串⼝助⼿等软件功能类似，设置也差不多。
1.设置接收格式、端⼝、波特率即可。
2.打开基本收码功能。
3.打开串⼝。
4.测试：2、⾼级收码⾼级收码设置选项卡：1.1-10对应0XA1-0XAA,我们只需要按定义帧设置即可，我这⾥使⽤0XA2，选择2，并打开开关。
2.因为我需要查看的有三个数据，pitch，roll，yaw，并为float类型，所以如此设置。
3.最后打开⾼级收码功能，如果你需要实时查看上传的数据可以打开收码显⽰。
4.打开串⼝。
5.测试：3、波形显⽰波形显⽰功能需要在⾼级收码的功能上做修改。
波形显⽰设置选项卡如下：1.1-20序号对应的是波形序号，也就是说最多能指定显⽰20个波形，我们这⾥有pitch,roll,yaw三个波形，所以设置1，2，3。
2.设置第⼀个波形1，它来源于数据帧2的第⼀位。
3.设置第⼀个波形2，它来源于数据帧2的第⼆位。
4.设置第⼀个波形3，它来源于数据帧2的第三位。
5.打开数据校验，数据显⽰。
6.打开波形显⽰。
7.在波形显⽰页⾯勾上前三个即可：8.打开串⼝。
9.测试：4、飞控状态使⽤到此功能再补充。
5、上传数据的单⽚机程序以下为上传三个⾓度的代码，串⼝初始化等略过：void usart1_send_char(u8 c){while((USART1->SR&0X40)==0);//等待上⼀次发送完毕USART1->DR=c;}//fun:功能字. 0XA0~0XAF//data:数据缓存区,最多28字节!!//len:data区有效数据个数void usart1_niming_report(u8 fun,u8*data,u8 len){u8 send_buf[32];u8 i;if(len>28)return; //最多28字节数据send_buf[len+3]=0; //校验数置零send_buf[0]=0X88; //帧头send_buf[1]=fun; //功能字send_buf[2]=len; //数据长度for(i=0;i<len;i++)send_buf[3+i]=data[i]; //复制数据for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //计算校验和for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //发送数据到串⼝1 }void mpu6050_send_data(float pitch,float roll,float yaw){u8 tbuf[16];unsigned char *p;p=(unsigned char *)&pitch;tbuf[0]=(unsigned char)(*(p+3));tbuf[1]=(unsigned char)(*(p+2));tbuf[2]=(unsigned char)(*(p+1));tbuf[3]=(unsigned char)(*(p+0));p=(unsigned char *)&roll;tbuf[4]=(unsigned char)(*(p+3));tbuf[5]=(unsigned char)(*(p+2));tbuf[6]=(unsigned char)(*(p+1));tbuf[7]=(unsigned char)(*(p+0));p=(unsigned char *)&yaw;tbuf[8]=(unsigned char)(*(p+3));tbuf[9]=(unsigned char)(*(p+2));tbuf[10]=(unsigned char)(*(p+1));tbuf[11]=(unsigned char)(*(p+0));usart1_niming_report(0XA2,tbuf,12);//⾃定义帧,0XA2}注意：最后⼀个函数把float拆成四个字节发送(由于串⼝只能⼀个字节⼀个字节的发送),⽤指针获得float型变量的⾸地址，然后强制转换为unsigned char型，地址逐渐加⼤把float 的四个字节分别发出即可。
无人机飞控技术最详细解读
无人机飞控技术最详细解读以前，搞无人机的十个人有八个是航空、气动、机械出身，更多考虑的是如何让飞机稳定飞起来、飞得更快、飞得更高。
如今，随着芯片、人工智能、大数据技术的发展，无人机开始了智能化、终端化、集群化的趋势，大批自动化、机械电子、信息工程、微电子的专业人才投入到了无人机研发大潮中，几年的时间让无人机从远离人们视野的军事应用飞入了寻常百姓家、让门外汉可以短暂的学习也能稳定可靠的飞行娱乐。
不可否认，飞控技术的发展是这十年无人机变化的最大推手。
无人机飞控是什么？飞行控制系统（Flight control system）简称飞控，可以看作飞行器的大脑。
多轴飞行器的飞行、悬停，姿态变化等等都是由多种传感器将飞行器本身的姿态数据传回飞控，再由飞控通过运算和判断下达指令，由执行机构完成动作和飞行姿态调整。
控可以理解成无人机的CPU系统，是无人机的核心部件，其功能主要是发送各种指令，并且处理各部件传回的数据。
类似于人体的大脑，对身体各个部位发送指令，并且接收各部件传回的信息，运算后发出新的指令。
例如，大脑指挥手去拿一杯水，手触碰到杯壁后，因为水太烫而缩回，并且将此信息传回给大脑，大脑会根据实际情况重新发送新的指令。
无人机的飞行原理及控制方法（以四旋翼无人机为例）四旋翼无人机一般是由检测模块，控制模块，执行模块以及供电模块组成。
检测模块实现对当前姿态进行量测；执行模块则是对当前姿态进行解算，优化控制，并对执行模块产生相对应的控制量；供电模块对整个系统进行供电。
悟四旋翼无人机机身是由对称的十字形刚体结构构成，材料多采用质量轻、强度高的碳素纤维；在十字形结构的四个端点分别安装一个由两片桨叶组成的旋翼为飞行器提供飞行动力，每个旋翼均安装在一个电机转子上，通过控制电机的转动状态控制每个旋翼的转速，来提供不同的升力以实现各种姿态；每个电机均又与电机驱动部件、中央控制单元相连接，通过中央控制单元提供的控制信号来调节转速大小；IMU惯性测量单元为中央控制单元提供姿态解算的数据，机身上的检测模块为无人机提供了解自身位姿情况最直接的数据，为四旋翼无人机最终实现复杂环境下的自主飞行提供了保障。
四轴飞控总结详尽的介绍各种飞控来源及硬件资源核心部分
四轴飞控总结详尽的介绍各种飞控来源及硬件资源核心部分四轴飞控总结详尽的介绍各种飞控来源及硬件资源核心部分从团队选择无人机项目开始，我的前期工作就是了解现在市场上所有的飞控以及功能，为接下来的无人机飞控打下基础。
现在市场上的飞控种类很多，常见的有MK、KK、KK flycam、EAGLE N6、玉兔飞控、FF、WKM、FC1212-S、MWC、FY等等，国内也是有越来越多的团队开始研究四轴飞控，其中很多属于山寨。
MK是德国的开源项目，但一般价格较贵，整个一套估计要1000多，且对模友的基础要求较高，玩的人不是很多。
KK是法国的开源项目，国内许多团队利用其开源的特点，将硬件电路和程序照搬过来然后在市场上卖，竞争比较激烈，因此价格很便宜，贵一点的也就100多便宜的只要60左右，目前最新版本是KK5.5，与其他飞控相比，KK飞控只有一个低端的陀螺仪而且不含加速度计，因此不能实现自稳，但价格低廉是其最大的优势，而且支持固定翼模式，很多模友在玩。
KK flycam是韩国的模友在KK的基础上开发的，添加了加速度计，用了更好一点的陀螺仪，因此能实现自稳，价格为145美元，目前国内卖得一般是其山寨版本，功能差不多，只有280左右。
EAGLE N6是国内一个团队刚刚研发的，使用的AVR单片机且效果很不错，支持8种飞行模式，每次启动只需要将拨码开关拨至指定模式就完成了模式的切换，且其288元的价格得到了很多模友的追捧，美中不足的是其没有加速度计，不能实现自稳，航拍性能不好。
/item.htm?id=12801941326玉兔飞控也是近期比较热门的一种飞控，由国外模友研发，采用ARM处理器，售价为288元，性价比较高。
功能特点：•主处理器，ARM32位，主频50MHZ•可以支持140g的mini小4轴，要知道小4轴比大4轴更“贼”哦。
•板载高精度数字3轴陀螺仪和3轴加速度计，实现自动稳定和自动平衡。
•8路接收通道，除了主要的4个摇杆通道外，还可以定义辅助开关通道或云台控制通道。