开关状态任务

匿名飞控的开关状态任务为函数Swtich_State_Task(u8 dT_ms)，定义在Ano_FlightCtrl.c中，在任务调度中引用，1ms执行一次。主要负责确定一些传感器是否能用，如果正常就将相应的标志位置1（打开）。包括光流、激光、GPS(暂时没有)、UWB、OPENMV。

void Swtich_State_Task(u8 dT_ms) {  switchs.baro_on = 1; //光流模块 if(sens_hd_check.of_ok || sens_hd_check.of_df_ok) { // if(sens_hd_check.of_ok) {    jsdata.of_qua = OF_QUALITY;    jsdata.of_alt = (u16)OF_ALT; } else if(sens_hd_check.of_df_ok) {    jsdata.of_qua = of_rdf.quality;    jsdata.of_alt = Laser_height_cm; } // if(jsdata.of_qua>50 )//|| flag.flying == 0) //光流质量大于50 /*或者在飞行之前*/，认为光流可用，判定可用延迟时间为1秒 { if(of_quality_delay<500) {     of_quality_delay += dT_ms; } else {     of_quality_ok = 1; } } else {    of_quality_delay =0;    of_quality_ok = 0; } //光流高度600cm内有效 if(jsdata.of_alt<600) { //    of_tof_on_tmp = 1;    jsdata.valid_of_alt_cm = jsdata.of_alt; //延时1.5秒判断激光高度是否有效 if(of_alt_delay<1500) {     of_alt_delay += dT_ms; } else { //判定高度有效     of_alt_ok = 1; } } else { //    of_tof_on_tmp = 0; // if(of_alt_delay>0) {     of_alt_delay -= dT_ms; } else { //判定高度无效     of_alt_ok = 0; } } // // if(flag.flight_mode == LOC_HOLD) { if(of_alt_ok && of_quality_ok) {     switchs.of_flow_on = 1; } else {     switchs.of_flow_on = 0; } } else {    of_tof_on_tmp = 0;    switchs.of_flow_on = 0; } //   switchs.of_tof_on = of_tof_on_tmp; } else {   switchs.of_flow_on = switchs.of_tof_on = 0; } //激光模块 if(sens_hd_check.tof_ok) { if(0)//(Laser_height_mm<1900) {    switchs.tof_on = 1; } else {    switchs.tof_on = 0; } } else {   switchs.tof_on = 0; } //GPS  //UWB if(uwb_data.online && flag.flight_mode == LOC_HOLD) {   switchs.uwb_on = 1; } else {   switchs.uwb_on = 0; } //OPMV if(opmv.offline==0 && flag.flight_mode == LOC_HOLD) {   switchs.opmv_on = 1; } else {   switchs.opmv_on = 0; } } 

程序中的光流代码中对光流的有无、是否发生错误进行检查的，如果没有问题，就将sens_hd_check.of_ok || sens_hd_check.of_df_ok置为1，我觉得应该一个表示匿名光流，一个是优像光流。当得知光流可以使用后，在Swtich_State_Task中就可以将它打开了（同样是置标志位）。这个过程中加了一些延时，应该是出于安全考虑，也可能是光流初始化需要时间？然后需要对光流高度进行判断，如果小于6m。认为有效。只有以上工作全部成功，才能启用光流。否则，即使光流存在也只是个摆设。如果工作在定位模式，就将switchs.of_flow_on置为1。
激光模块是否正常通过标志sens_hd_check.tof_ok（在激光的初始化过程定义的）判断。下面的if(0)我感觉是没有用的，也就是说激光只要初始化成功就能用了。
UWB和openmv是只要判断了工作正常，模式正确就能打开。总体来说这个函数除了标志位还是标志位，没有什么特别的。应该算是一种保险措施。防止某一个模块突然不工作对无人机造成影响。

遥控器数据处理任务

遥控器数据处理任务函数RC_duty_task位于Ano_RC.c中，11ms执行一次。使用PPM协议或者SBUS协议，程序默认使用PPM协议。有关几种协议的介绍见链接: link

void RC_duty_task(u8 dT_ms) //建议2ms调用一次 { if(flag.start_ok) //在主函数中被置为1 { /////////////获得通道数据//////////////////////// if(RC_IN_MODE == PPM || RC_IN_MODE == PWM) { for(u8 i=0;i<CH_NUM;i++) //CH_NUM默认为8 { if(chn_en_bit & (1<<i))//(Rc_Ppm_In[i]!=0)//该通道有值 { //CH_N[]+1500为上位机显示通道值      CH_N[i] = ((s16)RC_PPM.Captures[i] - 1500); //1000 -- 2000us,处理成大约+-500摇杆量 } else {      CH_N[i] = 0; }     CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500 } } else//sbus { for(u8 i=0;i<CH_NUM;i++) { if(chn_en_bit & (1<<i))//该通道有值 { //CH_N[]+1500为上位机显示通道值      CH_N[i] = 0.65f *((s16)Rc_Sbus_In[i] - 1024); //248 --1024 --1800,处理成大约+-500摇杆量 } else {      CH_N[i] = 0; }     CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500 } } /////////////////////////////////////////////// //解锁监测  unlock(dT_ms); //摇杆触发功能监测 stick_function(dT_ms); //通道看门狗 ch_watch_dog(dT_ms); //失控保护检查 fail_safe_check(dT_ms);//3ms } } 

RC_PPM.Captures[]和Rc_Sbus_In[i]都是对信号进行结算得到的值，相关函数位于Drv_RcIn.c中，计算过程是通用的，一般不需要修改，在调用
的时候可能需要将初始化部分修改一下（根据原理图修改相应的GPIO即可）。
整整一大段过程，PPM模式对我们工程有影响的数据只有RC_PPM.Captures[]和CH_N[i]，我们需要将数据发送到上位机显示以及通过接收的数据控制无人机。
unlock(dT_ms)为无人机上锁解锁函数，在电池电压、imu、气压计、惯性传感器工作正常的情况下才允许解锁，并且此时不能操作flash(自我感觉操作flash会降低运行速度，且一些参数正在被修改，如果此时解锁无法保证无人机正常飞行)。解锁方式是遥控器（美国手）遥杆内八或者外八，如果原本为上锁则解锁，原本为解锁状态则上锁（在操作飞控过程中切记不要把遥杆打到内八或外八）。建议这部分程序不要更改！
stick_function(dT_ms)为遥杆触发功能检测，也就是根据相应的通道值判断无人机俯仰、滚转或者偏航，然后校准相应的传感器（陀螺仪，加速度计，磁力计），注意这个函数不是用来控制无人机状态的，只是根据无人机状态校准传感器的。
fail_safe_check(dT_ms)为失控保护检查，当通道没有信号或者接收到失控保护信号时就启动失控保护，如果有GPS就返航，无GPS直接降落。当然并不是一次异常就判断为失控，需要多次不能接受信号或收到失控保护信号在进行失控保护。

数传数据交换

数传数据交换任务ANO_DT_Data_Exchange()定义在Ano_RC.c中，在我看来就是加了协议的串口通信。数据传出协议与匿名上位机相对应，可以查看匿名上位机的手册编写这部分函数。不过有些函数是固定的，比如pid参数，飞控状态等，这些直接和匿名上位机相对应，建议不要更改，如果要传输自己添加的某些数据，可选用其他通道，避免和默认的数据传输冲突，如何编写数据传输程序可以查看匿名的上位机教程 link.

void ANO_DT_Data_Exchange(void) { static u16 cnt = 0; static u16 senser_cnt  = 10; static u16 senser2_cnt  = 50; static u16 user_cnt  = 10; static u16 status_cnt  = 15; static u16 rcdata_cnt  = 20; static u16 motopwm_cnt = 20; static u16 power_cnt = 50; static u16 speed_cnt    = 50; static u16 sensorsta_cnt = 500; static u16 omv_cnt = 100; static u16 location_cnt = 500; static u8 flag_send_omv = 0; if((cnt % senser_cnt) == (senser_cnt-1))   f.send_senser = 1; if((cnt % senser2_cnt) == (senser2_cnt-1))   f.send_senser2 = 1; if((cnt % user_cnt) == (user_cnt-2))   f.send_user = 1; if((cnt % status_cnt) == (status_cnt-1))   f.send_status = 1; if((cnt % rcdata_cnt) == (rcdata_cnt-1))   f.send_rcdata = 1; if((cnt % motopwm_cnt) == (motopwm_cnt-2))   f.send_motopwm = 1; if((cnt % power_cnt) == (power_cnt-2))   f.send_power = 1; if((cnt % speed_cnt) == (speed_cnt-3))   f.send_speed = 1; if((cnt % sensorsta_cnt) == (sensorsta_cnt-2)) {   f.send_sensorsta = 1; } if((cnt % omv_cnt) == (omv_cnt-2)) {   flag_send_omv = 1; } if((cnt % location_cnt) == (location_cnt-3)) {   f.send_location = 1; } if(++cnt>1000) cnt = 0; ///////////////////////////////////////////////////////////////////////////////////// if(f.send_version) {   f.send_version = 0; ANO_DT_Send_Version(4,300,100,400,0); //发送版本 } else if(f.paraToSend < 0xffff) { ANO_DT_SendParame(f.paraToSend);    f.paraToSend = 0xffff; } /////////////////////////////////////////////////////////////////////////////////////// else if(f.send_status) {   f.send_status = 0; ANO_DT_Send_Status(imu_data.rol,imu_data.pit,imu_data.yaw,wcz_hei_fus.out,(flag.flight_mode+1),flag.unlock_sta); } /////////////////////////////////////////////////////////////////////////////////////// else if(f.send_speed) {   f.send_speed = 0; ANO_DT_Send_Speed(loc_ctrl_1.fb[Y],loc_ctrl_1.fb[X],loc_ctrl_1.fb[Z]); } /////////////////////////////////////////////////////////////////////////////////////// else if(f.send_user) {   f.send_user = 0; ANO_DT_Send_User(); } /////////////////////////////////////////////////////////////////////////////////////// else if(f.send_senser) {   f.send_senser = 0; ANO_DT_Send_Senser(sensor.Acc[X],sensor.Acc[Y],sensor.Acc[Z],sensor.Gyro[X],sensor.Gyro[Y],sensor.Gyro[Z],mag.val[X],mag.val[Y],mag.val[Z]); } ///////////////////////////////////////////////////////////////////////////////////// else if(f.send_senser2) {   f.send_senser2 = 0; ANO_DT_Send_Senser2(baro_height,ref_tof_height,sensor.Tempreature_C*10);//原始数据 } ///////////////////////////////////////////////////////////////////////////////////// else if(flag_send_omv) {   flag_send_omv = 0; if(f.send_omv_ct) {    f.send_omv_ct = 0; ANO_DT_SendOmvCt(opmv.cb.color_flag,opmv.cb.sta,opmv.cb.pos_x,opmv.cb.pos_y,opmv.cb.dT_ms); } else if(f.send_omv_lt) {    f.send_omv_lt = 0; ANO_DT_SendOmvLt(opmv.lt.sta, opmv.lt.angle, opmv.lt.deviation, opmv.lt.p_flag, opmv.lt.pos_x, opmv.lt.pos_y, opmv.lt.dT_ms); } } ///////////////////////////////////////////////////////////////////////////////////// else if(f.send_rcdata) {   f.send_rcdata = 0;   s16 CH_GCS[CH_NUM]; for(u8 i=0;i<CH_NUM;i++) { if((chn_en_bit & (1<<i)))//(Rc_Pwm_In[i]!=0 || Rc_Ppm_In[i] !=0  )//该通道有值 {     CH_GCS[i] = CH_N[i] + 1500; } else {     CH_GCS[i] = 0; } } ANO_DT_Send_RCData(CH_GCS[2],CH_GCS[3],CH_GCS[0],CH_GCS[1],CH_GCS[4],CH_GCS[5],CH_GCS[6],CH_GCS[7],0,0); } /////////////////////////////////////////////////////////////////////////////////////  else if(f.send_motopwm) {   f.send_motopwm = 0; #if MOTORSNUM == 8 ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],motor[6],motor[7]); #elif MOTORSNUM == 6 ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],0,0); #elif MOTORSNUM == 4 ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],0,0,0,0); #else #endif } ///////////////////////////////////////////////////////////////////////////////////// else if(f.send_power) {   f.send_power = 0; ANO_DT_Send_Power(Plane_Votage*100,0); } else if(f.send_sensorsta) {   f.send_sensorsta = 0; ANO_DT_SendSensorSta(switchs.of_flow_on ,switchs.gps_on,switchs.opmv_on,switchs.uwb_on,switchs.of_tof_on); } else if(f.send_location) {   f.send_location = 0; ANO_DT_Send_Location(switchs.gps_on,Gps_information.satellite_num,(s32)Gps_information.longitude,(s32)Gps_information.latitude,123,456); } else if(f.send_vef) { ANO_DT_Send_VER();   f.send_vef = 0; } ///////////////// ANO_DT_Data_Receive_Anl_Task(); //////////////////////////////////////////////////////////////////// } 

前半部分主要是用来对任务进行分配，这么多组数据一起发送肯定乱套了。然后就是根据分配的任务依次执行了。
ANO_DT_Send_Version应该是用来发送版本信息的
ANO_DT_SendParame用来发送参数信息（pid等）
ANO_DT_Send_Status用来发送飞控状态（俯仰、偏航角等）
ANO_DT_Send_Speed用来发送x、y、z方向的速度
ANO_DT_Send_User这个就是我刚才说的自定义数据发送，如果用到的话可以查看上位机视频或手册。
ANO_DT_Send_Senser用来发送陀螺仪、加速度计、磁力计的数据
ANO_DT_Send_Senser2用来发送气压计、激光数据以及传感器温度
ANO_DT_SendOmvCt、ANO_DT_SendOmvCt用来发送openmv数据
ANO_DT_Send_RCData用来发送遥控器数据信息（处理之后）
ANO_DT_Send_MotoPWM用来发送控制电机的pwm信息
ANO_DT_Send_Power用来发送电源电压
ANO_DT_SendSensorSta用来发送传感器状态（有无或是否正常工作）
ANO_DT_Send_Location用来发送位置信息，需要GPS
ANO_DT_Data_Receive_Anl_Task是数据接收任务，首先在串口中断中ANO_DT_Data_Receive_Prepare(com_data)接收数据并存放在DT_RxBuffer中，由ANO_DT_Data_Receive_Anl根据得到的命令来控制飞控（恢复默认参数、改写参数、控制飞行等）

延时存储任务

延时存储任务Ano_Parame_Write_task()50ms执行一次.需要注意的是参数改变是不会立刻存入flash，因为为写入flash耗时较长，所以当无人机工作时如果需要存储数据，会置一个标志位，等到无人机降落锁定后在存入flash，保证飞行的安全性。代码如下：

void Ano_Parame_Write_task(u16 dT_ms) { //因为写入flash耗时较长，我们飞控做了一个特殊逻辑，在解锁后，是不进行参数写入的，此时会置一个需要写入标志位，等飞机降落锁定后，再写入参数，提升飞行安全性 //为了避免连续更新两个参数，造成flash写入两次，我们飞控加入一个延时逻辑，参数改变后三秒，才进行写入操作，可以一次写入多项参数，降低flash擦写次数 if(para_sta.save_en ) //允许存储 { if(para_sta.save_trig == 1) //如果触发存储标记1 {    LED_STA.saving = 1;        para_sta.time_delay = 0; //计时复位    para_sta.save_trig = 2; //触发存储标记2 } if(para_sta.save_trig == 2) //如果触发存储标记2 { if(para_sta.time_delay<3000) //计时小于3000ms {     para_sta.time_delay += dT_ms; //计时 } else {          para_sta.save_trig = 0; //存储标记复位 Ano_Parame_Write(); //执行存储 ANO_DT_SendString("Set save OK!");     LED_STA.saving = 0; } } else {    para_sta.time_delay = 0; } } else {   para_sta.time_delay = 0;   para_sta.save_trig = 0; } } 

但这样就会出现一个问题，在我们调飞控的过程中改变参数，无人机不会立刻做出响应？其实不会的。我们更改参数有两种途径，一种是通过上位机修改，当我们修改完之后会调用ANO_DT_ParListToParUsed()函数，直接将参数改过来。另一种是在软件上修改PID_Rest()和Parame_Reset()中的参数，在我们上电后程序会先进一个初始化//数据初始化 Dvr_ParamterInit(); //读取初始数据 Para_Data_Init();
如果我们是第一次烧写程序，那么读取数据过程就会将PID_Rest()和Parame_Reset()的数据直接存入内存。但如果我们之前已经下载过程序的话，那么Ano_Parame.set.frist_init = SOFT_VER就会被存入内存中，这个时候读数据过程就不会重新写入数据了。代码如下：

static void Ano_Parame_Write(void) { All_PID_Init(); //////存储PID参数后，重新初始化PID   Ano_Parame.set.frist_init = SOFT_VER; Parame_Copy_Fc2para(); Dvr_ParamterSave(); } void Ano_Parame_Read(void) { Dvr_ParamterRead(); if(Ano_Parame.set.frist_init != SOFT_VER) //内容没有被初始化，则进行参数初始化工作 { Parame_Reset(); PID_Rest(); Ano_Parame_Write(); } Parame_Copy_Para2fc(); } 

这个时候我们可以通过上位机恢复默认参数将PID_Rest()和Parame_Reset()的数据存入内存。

默认保存的数据如下

__packed struct Parameter_s {  u16 frist_init; //飞控第一次初始化，需要做一些特殊工作，比如清空flash      u8  pwmInMode; //接收机模式，分别为PWM型PPM型  u8  heatSwitch; // float  acc_offset[VEC_XYZ]; //加速度计零偏 float  gyro_offset[VEC_XYZ]; //陀螺仪零偏 float  surface_vec[VEC_XYZ]; //水平面向量 float  center_pos_cm[VEC_XYZ]; //重心相对传感器位置偏移量 float  mag_offset[VEC_XYZ]; //磁力计零偏 float  mag_gain[VEC_XYZ]; //磁力计校正比例 float  pid_att_1level[VEC_RPY][PID]; //姿态控制角速度环PID参数 float  pid_att_2level[VEC_RPY][PID]; //姿态控制角度环PID参数 float  pid_alt_1level[PID]; //高度控制高度速度环PID参数 float  pid_alt_2level[PID]; //高度控制高度环PID参数 float  pid_loc_1level[PID]; //位置控制位置速度环PID参数 float  pid_loc_2level[PID]; //位置控制位置环PID参数 float  pid_gps_loc_1level[PID]; //位置控制位置速度环PID参数 float  pid_gps_loc_2level[PID]; //位置控制位置环PID参数 float   warn_power_voltage; float return_home_power_voltage; float   lowest_power_voltage; float auto_take_off_height; float auto_take_off_speed; float auto_landing_speed; float idle_speed_pwm; }; 

直接存入内存的参数名称如下，上位机得到的参数数据也会赋值给下列参数控制系统。

//--- 姿态控制角速度环PID参数  Ano_Parame.set.pid_att_1level[ROL][KP] = 4.0f; //姿态控制角速度环PID参数  Ano_Parame.set.pid_att_1level[ROL][KI] = 3.0f; //姿态控制角速度环PID参数  Ano_Parame.set.pid_att_1level[ROL][KD] = 0.15f; //姿态控制角速度环PID参数    Ano_Parame.set.pid_att_1level[PIT][KP] = 4.0f; //姿态控制角速度环PID参数  Ano_Parame.set.pid_att_1level[PIT][KI] = 3.0f; //姿态控制角速度环PID参数  Ano_Parame.set.pid_att_1level[PIT][KD] = 0.15f; //姿态控制角速度环PID参数    Ano_Parame.set.pid_att_1level[YAW][KP] = 6.0f; //姿态控制角速度环PID参数  Ano_Parame.set.pid_att_1level[YAW][KI] = 0.5f; //姿态控制角速度环PID参数  Ano_Parame.set.pid_att_1level[YAW][KD] = 0.0f; //姿态控制角速度环PID参数 //--- 姿态控制角度环PID参数  Ano_Parame.set.pid_att_2level[ROL][KP] = 7.0f; //姿态控制角度环PID参数  Ano_Parame.set.pid_att_2level[ROL][KI] = 0.0f; //姿态控制角度环PID参数  Ano_Parame.set.pid_att_2level[ROL][KD] = 0.00f; //姿态控制角度环PID参数    Ano_Parame.set.pid_att_2level[PIT][KP] = 7.0f; //姿态控制角度环PID参数  Ano_Parame.set.pid_att_2level[PIT][KI] = 0.0f; //姿态控制角度环PID参数  Ano_Parame.set.pid_att_2level[PIT][KD] = 0.00f; //姿态控制角度环PID参数    Ano_Parame.set.pid_att_2level[YAW][KP] = 5.0f; //姿态控制角度环PID参数  Ano_Parame.set.pid_att_2level[YAW][KI] = 0.0f; //姿态控制角度环PID参数  Ano_Parame.set.pid_att_2level[YAW][KD] = 0.5; //姿态控制角度环PID参数  //--- 高度控制高度速度环PID参数   Ano_Parame.set.pid_alt_1level[KP] = 2.0f; //高度控制高度速度环PID参数  Ano_Parame.set.pid_alt_1level[KI] = 1.0f; //高度控制高度速度环PID参数  Ano_Parame.set.pid_alt_1level[KD] = 0.05f; //高度控制高度速度环PID参数 //--- 高度控制高度环PID参数  Ano_Parame.set.pid_alt_2level[KP] = 1.0f; //高度控制高度环PID参数  Ano_Parame.set.pid_alt_2level[KI] = 0; //高度控制高度环PID参数(NULL)  Ano_Parame.set.pid_alt_2level[KD] = 0; //高度控制高度环PID参数(NULL) //--- 位置控制位置速度环PID参数   Ano_Parame.set.pid_loc_1level[KP] = 0.15f; //位置控制位置速度环PID参数  Ano_Parame.set.pid_loc_1level[KI] = 0.10f; //位置控制位置速度环PID参数  Ano_Parame.set.pid_loc_1level[KD] = 0.00f; //位置控制位置速度环PID参数 //--- 位置控制位置环PID参数  Ano_Parame.set.pid_loc_2level[KP] = 0; //位置控制位置环PID参数(NULL)  Ano_Parame.set.pid_loc_2level[KI] = 0; //位置控制位置环PID参数(NULL)  Ano_Parame.set.pid_loc_2level[KD] = 0; //位置控制位置环PID参数(NULL) //--- GPS位置控制位置速度环PID参数   Ano_Parame.set.pid_gps_loc_1level[KP] = 0.15f; //位置控制位置速度环PID参数  Ano_Parame.set.pid_gps_loc_1level[KI] = 0.10f; //位置控制位置速度环PID参数  Ano_Parame.set.pid_gps_loc_1level[KD] = 0.00f; //位置控制位置速度环PID参数 //--- GPS位置控制位置环PID参数  Ano_Parame.set.pid_gps_loc_2level[KP] = 0.3f; //位置控制位置环PID参数  Ano_Parame.set.pid_gps_loc_2level[KI] = 0; //位置控制位置环PID参数(NULL)  Ano_Parame.set.pid_gps_loc_2level[KD] = 0; //位置控制位置环PID参数(NULL) 

 Ano_Parame.set.pwmInMode = PPM;  Ano_Parame.set.heatSwitch = 0;  Ano_Parame.set.warn_power_voltage = 3.50f *3;  Ano_Parame.set.return_home_power_voltage = 3.7f *3;  Ano_Parame.set.lowest_power_voltage = 3.4f *3;    Ano_Parame.set.auto_take_off_height = 0;//cm  Ano_Parame.set.auto_take_off_speed = 150;  Ano_Parame.set.auto_landing_speed = 60;    Ano_Parame.set.idle_speed_pwm = 20;//20% for(u8 i = 0;i<3;i++) {   Ano_Parame.set.acc_offset[i] = 0;   Ano_Parame.set.gyro_offset[i] = 0;   Ano_Parame.set.mag_offset[i] = 0;     Ano_Parame.set.mag_gain[i] = 1;          Ano_Parame.set.center_pos_cm[i] = 0; } 

 //等号前面为存入内存的数据，等号后面为飞控使用的参数名称   Ano_Parame.set.acc_offset[i] = save.acc_offset[i];   Ano_Parame.set.gyro_offset[i] = save.gyro_offset[i];   Ano_Parame.set.mag_offset[i] = save.mag_offset[i];     Ano_Parame.set.mag_gain[i] = save.mag_gain[i];