├─.vscode
├─Application
│  ├─Inc
│  └─Src
├─Debug
│  ├─Exe
│  ├─List
│  └─Obj
├─Drive
│  ├─Inc
│  ├─Src
│  └─Vl53l0x
│      ├─Inc
│      └─Src
├─figure
├─FlyControl_Algorithm
│  ├─Inc
│  └─src
├─FlyControl_Calculate
│  ├─Inc
│  └─Src
├─Library
├─Note
└─settings

Application
├─Inc  
└─Src
        BSP_Init.c        // 初始化所有（底层驱动、中断配置）
        DY_DT.c           // 数据传输
        DY_Flight_Log.c
        DY_OF.c          // 匿名optical flow 光流的驱动(使用uart4中断接收数据) 本项目未用到
        DY_Parameter.c
        DY_power.c
        DY_RC.c          // 遥控器通道数据处理
        DY_Scheduler.c   // 任务调度（所有的任务及其调度方式）
        DY_Tracking.c
        main.c
        OpticalFlow.c    // DY or ATK-PMW3901 光流驱动，使用SPI接收光流信息

此文件夹存放了main.c文件，整个系统的启动、初始化、电源管理都在此处。Inc中存放的是对应的.h文件，这里不再打印。

FlyControl_Calculate
├─Inc    
└─Src
        DY_AltCtrl.c
        DY_AttCtrl.c          // 角度控制
        DY_FlightCtrl.c
        DY_FlightDataCal.c    // 读取加速度计、陀螺仪的数据
        DY_LocCtrl.c
        DY_MagProcess.c
        DY_MotorCtrl.c        // 电机控制

FlyControl_Algorithm
├─Inc  
└─Src
        DY_FcData.c
        DY_Filter.c
        DY_Imu.c
        DY_Math.c
        DY_MotionCal.c
        DY_Navigate.c
        DY_Pid.c          // PID计算（控制器）

Drive
├─Inc     
├─Src
│      Drv_adc.c
│      Drv_ak8975.c       // 电子罗盘驱动
│      Drv_icm20602.c     // 姿态传感器的驱动
│      Drv_led.c
│      Drv_pwm_in.c       // 遥控器输入信号接收
│      Drv_pwm_out.c
│      Drv_soft_i2c.c
│      Drv_spi.c
│      Drv_spl06.c
│      Drv_time.c
│      Drv_usart.c
│      Drv_vl53l0x.c      // TOF激光测距模块驱动
│      Drv_w25qxx.c
│      uartstdio.c
│      
└─Vl53l0x                 // TOF激光测距模块库函数
    ├─Inc     
    └─Src
            vl53l0x_api.c
            vl53l0x_api_calibration.c
            vl53l0x_api_core.c
            vl53l0x_api_ranging.c
            vl53l0x_api_strings.c
            vl53l0x_i2c.c
            vl53l0x_platform.c

如图，摘自网络大佬博客，更详细、高清的框图请见大佬公开 [思维导图]: https://www.processon.com/view/link/5d374332e4b0b3e4dcd01d3a

姿态传感器，其中有1个3轴陀螺仪和1个3轴加速度计。使用I2C或SPI通讯，工程中使用的是SPI通讯，其接线图如下：

源代码中提供的api文件名为Drv_icm20602.c，其中提供的函数如下：

void Drv_Icm20602CSPin_Init(void); // 初始化 Icm20602 的CS引脚，输出1
u8   Drv_Icm20602Reg_Init(void);   //  初始化icm进入可用状态。
void Drv_Icm20602_Read(void);      // 把数据读入mpu_buffer数组中
void Sensor_Data_Prepare(u8 dT_ms);
void Center_Pos_Set(void);

此飞控的遥控器接收器只能使用PWM模式，6通道信号；数据通过Drv_pwm_in.c文件，接收到Rc_Pwm_In数组中，然后在DY_RC.c的RC_duty_task函数中变为+-500摇杆量存储在CH_N数组中。

此函数接收摇杆量CH_N，转换为速度量fs.speed_set_h。作为后面环的控制。但如果启用OpenMV控制，则变为：

OpenMV模式下，fs.speed_set_h[Z]将在DY_AltCtrl.c中被赋值。

所以说，这个函数的主要功能就是把摇杆量转换为后续控制环所需设定值，这里所有的设定值皆为速度量。此外函数中还有对标志位的检测

此程序中所有的PID计算都由DY_Pid.c 中的 PID_calculate 函数完成，其参数列表如下：

float PID_calculate(float dT_s,            //周期（单位：秒）
					float in_ff,		   //前馈值
					float expect,		   //期望值（设定值）
					float feedback,		   //反馈值（）
					_PID_arg_st *pid_arg,  //PID参数结构体
					_PID_val_st *pid_val,  //PID数据结构体
					float inte_d_lim,      //积分误差限幅
					float inte_lim		   //integration limit，积分限幅)

其中pid_arg中储存的是如 P,I,D之类的参数；pid_val中储存的是如上次的误差，上次的反馈值等在位置式PID中需要用到的储存量。也就是说，给定特定的 arg和val就可组成特定的PID控制器。

其可以实现的是一个反馈-前馈的控制，结构上采用了微分先行的方式，实现上使用的是位置式PID。

PID的连续型公式为： $$ u(t) = K_p [e(t) + \frac{1}{T_i} \int_{t}^{0}e(t)dt + T_d \frac{de(t)}{dt}] $$ 直接对其离散化，即可得到位置式PID： $$ u(k) = k_p e(k) + K_i \sum_{i=0}e(i) + K_D[e(k) - e(k-1)] $$ 这里有对误差的求和项$\sum_{i=0} e(i)$，此项容易造成存储空间的占据，于是增量式PID诞生： $$ \begin{align*} \Delta u(k) &= u(k) - u(k-1)\ &= K_p[e(k) - e(k-1)] + K_I e(k) +K_D[e(k) - 2e(k-1) + e(k-2)] \end{align*} $$

所谓微分先行，就是对反馈量直接微分，作为控制器输出的一部分：

结合微分先行和位置式PID，代码中展现如下：

pid_val->out = pid_arg->k_ff *in_ff      // 前馈系数 * 前馈
	         + pid_arg->kp *pid_val->err // Kp      * 误差 
			 + differential              // 微分先行项
    	     + pid_val->err_i;		     //误差积分项（已乘Ki）

PS：

按理说，differential 应当为 $K_d × \frac{d(feedback)}{dt}$，但在代码中却展示为：$K_{d(sp)} \frac{d(setpoint)}{dt}-K_{d(fb)} \frac{d(feedback)}{dt}$.不知所谓：

//如何正确理解微分定义？  dx/dt = lim  [x(t+dt) - x(t)]/dt = [x(t+dt) - x(t)]/T
//期望值的微分 = （期望 - 上次期望） * 频率
pid_val->exp_d = (expect - pid_val->exp_old) *hz;

//反馈值的微分 = （反馈 - 上次反馈） * 频率		 
pid_val->fb_d = (feedback - pid_val->feedback_old) *hz;
// 微分先行算法
//微分 = 期望微分系数 * 期望微分值 - 反馈微分系数 * 反馈微分值	
differential = (pid_arg->kd_ex *pid_val->exp_d - pid_arg->kd_fb *pid_val->fb_d);

$$ \frac{dx}{dt} = \lim\limits_{t\to 0} \frac{x(t+dt)-x(t)}{dt} = \frac{x(t_2)-x(t_1)}{T} = [x(t_2)-x(t_1)]f $$

位置控制环函数为DY_LocCtrl.c，主要控制函数如下：

void Loc_1level_Ctrl(u16 dT_ms,s16 *CH_N)

其中参数CH_N，并未被用到，输入输出结果如下：

这里的反馈值是光流传输回来的，暂且不清楚是什么。其会输出一个数组，这个数组将作为角度环的设定值。

角度控制是通过串级控制实现的，实现的文件为DY_AttCtrl.c，主要函数为：Att_2level_Ctrl角度控制器和Att_1level_Ctrl角速度控制器，方框图如下：

联系函数内容可以做出下图：

设定值CH_N[YAM]来自遥控器的输入，是偏航角的设定值，但其余两个设定值暂不清楚。

高度环控制由DY_AltCtrl.c中的，Alt_2level_Ctrl和Alt_1level_Ctrl两个函数完成。也是串级控制，内环为速度环，外环为高度环。

但是关于设定值loc_ctrl_2.exp[Z]却十分奇怪，在flag.taking_off != 1时，他被赋值为反馈值loc_ctrl_2.fb[Z]；

如果flag.taking_off = 1，且flag.ct_alt_hold != 1，那么loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z] + alt_val_2.err。但此时因为没有进入PID环节，alt_val_2.err就是为0，所以设定值仍然等于反馈值。

这里表达的意思是，若飞机未进入定高悬停状态flag.ct_alt_hold=0，则高度环的主控制器不起作用，高度环控制器输出为0。当进入定高悬停状态时，则loc_ctrl_2.exp[Z]等于当前的高度值，高度环控制器开始工作，使得高度得到控制。

这里，进入定高的条件是高度速度环的反馈值和期望值接近。也就是说，飞机在最开始起飞时，是没有高度控制器的，只有速度控制器；当速度接近设定值时，高度控制器才投入运行，保持当前的飞行高度。

因此，想要无人机定高飞行，只需要修改loc_ctrl_2.exp[Z]即可达到目的。

其实本身没有什么好说的，但其设定值还是有点东西。下面这个框图可以说明。

如果flag.fly_ready==1，那么飞控就会依次使4个电机达到怠速状态，然后置位flag.motor_preparation，表示电机已经准备完成。

如果flag.fly_ready==0，那么flag.motor_preparation只会为0，且motor_step一直为0，这样，电机就会停止转动。所以通过改变flag.fly_ready就可以控制电机立即刹车。

电机控制模块为：DY_MotorCtrl.c文件，其核心代码如下：

if(flag.motor_preparation == 1)
{		
    motor_step[m1] = mc.ct_val_thr  +mc.ct_val_yaw -mc.ct_val_rol +mc.ct_val_pit;
    motor_step[m2] = mc.ct_val_thr  -mc.ct_val_yaw +mc.ct_val_rol +mc.ct_val_pit;
    motor_step[m3] = mc.ct_val_thr  +mc.ct_val_yaw +mc.ct_val_rol -mc.ct_val_pit;
    motor_step[m4] = mc.ct_val_thr  -mc.ct_val_yaw -mc.ct_val_rol -mc.ct_val_pit;
}

motor_step数组为4个电机转速对应的值，而mc.ct_val_thr和等式右的值来自高度环和角度环的输出，这些输出需要按一定的原则分配到4个电机上，才能实现控制任务，这里就是在完成控制任务的分配。

在完成控制任务分配后，对分配值进行限幅操作，赋值给数组motor，最后使用函数SetPwm把数值转化为占空比，输出到4个电机。

首先，原版的一键起飞和降落是通过摇杆的旋钮实现的，旋钮通道CH5和CH6的值就会改变。数据通过Drv_pwm_in.c文件，接收到Rc_Pwm_In数组中，然后在DY_RC.c的RC_duty_task函数中变为+-500摇杆量存储在CH_N数组中。

在文件DY_FlightCtrl.c的函数Flight_Mode_Set中，会通过判断CH_N[AUX1]来判断是否要一键降落，通过判断CH_N[AUX2]来判断是否要一键起飞。

	if(CH_N[AUX2]<-200)
	{
	  // 若启用一键起飞后，OpemMV控制高度模式没有启动，则启动；
	  // 同时启用one_key_take_off，置位标志位one_key_taof_start和flag.fly_ready
      if(DY_Debug_Height_Mode==0)
      {
        DY_Debug_Height_Mode = 1;
        one_key_take_off();
        dy_height = 30;
      }
      // 若启用一键起飞后，OpemMV控制高度模式已经启动
      else
      {
		// 如果当前高度高于1.2m且没有开始计数，就使得高度设定值为0，开始计数
        if(tof_height_mm>=1200 && DY_CountTime_Flag==0)
        {
          dy_height = 0;
          DY_CountTime_Flag = 1;
        }
        if(DY_CountTime_Flag)
        {
          DY_Task_ExeTime++;
		  // 如果计数15s后，就启用一键降落(DY_Land_Flag为防止one_key_land被执行多次)
          if(DY_Task_ExeTime>=1500 && DY_Land_Flag==0) // 10ms*1500 = 15s
          {
            DY_Land_Flag = 1;
            one_key_land();     //一键降落
          }
//          if(DY_Task_ExeTime>=1000 && DY_Debug_Mode==0)
//          {
//            DY_Debug_Mode = 1;
//            MAP_UARTCharPut(UART5_BASE, 'H');     //OpenMv开始工作
//          }
        }
      }
	} 

因为我们的遥控器没有CH5和CH6通道，所以我们必须手动进入这个判断。我们的期望是，当飞机启动一段时间后，在20s内完成一键起飞和降落的功能。

使用的起飞函数如下：

void our_take_off()
{   
    if(flag.auto_take_off_land != AUTO_TAKE_OFF_FINISH && our_delay_times[0] <200)
    {
        // DY_Debug_Height_Mode = 1;
        one_key_take_off();
        dy_height = 30;
    }
}

our_delay_times[0]是一个计时器

C语言中，统一文件夹下的全局变量可以在不同的C文件中调用。

1. CH_N

初次定义于DY_RC.c中，为遥控器的遥感量，在RC_duty_task函数中被赋值。

被作为参数传入函数Flight_State_Task(u8 dT_ms,s16 *CH_N)中，或许作为设定值？暂时不太清楚。

typedef struct
{
      //基本状态/传感器
      u8 start_ok;	//系统初始化OK
      u8 sensor_ok;
      u8 motionless;
      u8 power_state;
      u8 wifi_ch_en;
      u8 rc_loss;	
      u8 gps_ok;	
      u8 gps_signal_bad;


      //控制状态
      u8 manual_locked;
      u8 unlock_en;
      u8 fly_ready;  //unlocked 准备起飞，非常重要
      u8 thr_low;
      u8 locking;
      u8 taking_off; //起飞
      u8 set_yaw;
      u8 ct_loc_hold;
      u8 ct_alt_hold;


      //飞行状态
      u8 flying;             // 正在飞行
      u8 auto_take_off_land;
      u8 home_location_ok;	
      u8 speed_mode;
      u8 thr_mode;	
      u8 flight_mode;
      u8 gps_mode_en;
      u8 motor_preparation;
      u8 locked_rotor;
}_flag;

fly_ready=1表示已经准备好起飞，此时flag.taking_off才能被置位。此外更重要的是：只有当fly_ready==1时，标志位motor_preparation才可能置位，才会开始对电机的控制，否则电机的输入PWM占空比为0，电机停转。下表为fly_ready赋值处及其意义。

只有flag.taking_off被置位时，才会设置垂直方向速度，否则就设置垂直方向速度为0；

当flag.taking_off=1被维持1s后，flag.flying被置位，表示飞机已经起飞。

7.6 今日在看DY_FlightCtrl.c中的Flight_State_Task函数，明天需要看DY_RC.c来进一步了解。

7.7 今天搞明白了遥控器是如何控制的，PID的如何运算的，其输入输出和配置如何，下一步就是要开始看每一个具体的环路控制了。

7.12 今天搞明白了飞行器角度的控制方法DY_AttCtrl.c，但角度控制中的设定值来源loc_ctrl_1.out[Y]，变量暂不清楚是何作用，其来自于DY_LocCtrl.c位置环控制，此外电机控制DY_MotorCtrl.c也初露端倪，下一步就是搞懂这两块的代码。

7.18 今日任务，读懂DY_LocCtrl.c，DY_MotorCtrl.c；搞明白光流，高度环和位置环，另外知道设定值到底是哪个变量，进行飞行实验。

今日完成DY_MotorCtrl.c，想要加入一个远程zigbee紧急停车模块，下一步要测试Uart4_Init等的波特率，看波特率到底如何设置，如何通信和如何解析接收数据。

7.19 1：完成紧急刹车的布置，当按下stm32板子上的按钮（现在为重启）后，无人机的电机会自动停止旋转。

实现原理是使得fly_ready=0，无人机上的zigbee和串口4连接，接收并解析stm32上zigbee传来的信息，当数据的数据包部分第一个数据为0x66，直接使得fly_ready=0，完成刹车。

2:想要完成一键起飞，但失败了，不知道是什么原因，下一步查看MV和控制器的互动。

7.20 1：完成了一键起飞的任务，并且添加了zigbee回传标志位的功能。新建飞行日志文件夹，存放每次飞行时的记录。

2：phs的高度飞行设计完成，添加了飞行高度tof_height_mm的回传，添加了飞行日志，随着数据可以回传，我们的进度有所加快。下一步预计进行前后作用的控制。

7.22：今天完成了定高任务，完成区域定高，取消遥控器控制。下一步测试X、Y方向的运动，回传高度融合数据。

7.26：Accident but fix it 今天出师不利，遇到了各种各样的问题，但还好phs解决了问题；是通过调参解决的，原始的位置环的PID参数都是0，皓崧加入了调的参数，使得位置环投入工作；此外我们还更换了飞控；下一步测试XY方向运动，正方形轨迹飞行。

7.27-1: 完成了pit方向上的往复运动，但现在面临一个问题，就是起飞后数据不在回传，尝试把发送数据移动到200Hz线程，但还是无法回传。但终于可以和OpenMV联动了，下午希望和OpenMV联动实现定点一键起飞、悬停和降落。

7.28-1：Connect to OpenMV。完成了和伟哥OpenMV的联动，实现了定点悬停和降落；但目前无人机摆动较为严重，不稳，需要解决这个问题。

7.28-2：Connect to OpenMV2-add our heigh control。加入了我们自己的高度控制，openMV只发送S，即悬停信号。

7.28-3：Connect to OpenMV3-Can't fix the problem of shake注释了OpenMV控制中有关dy_height的内容，使得高度完全由飞控控制。把发送H开始OpenMV控制从延时发送，修改为达到指定高度才发送，即到达指定高度，OpenMV开始控制。

现在情况是：如果不加光流，只接OpenMV，会乱飞；如果加入光流，又加入OpenMV，不会乱飞，但会在上空乱抖。（但有已经证明OpenMV是在控制飞行器的）

可以看出，光流的加入可以实现定高，但会使得物体抖动。明天需要测试只用光流，拖动垫子，飞机会不会跟随，以测试光流的控制效果。

明天看一下原厂程序的一键起飞定高是否乱晃，如果乱晃，则说明是我们飞机的问题（包括电机、飞控等等）。

7.29-1: Stable with help of OP 做出了以下调整使得仅仅光流作用下无人机可以较为平稳的悬停。首先，只是用了一个高度环，当系统的高度环和phs的共同作用时，就会出现抖动的情况。更换了地贴，现在使用地贴是之前小车留下的纯白布。此外这套代码也实现了和OpenMV的联动。现在高度环是phs写的。

此外，现在的问题有：

1. 高度环的控制不够稳定。当只使用OP，进行顶高定点时，无人机会上下飘动，幅度肉眼可见，大概为10-15cm，虽然频率不高，但仍需要解决，已经尝试调整了几次高度环的PID，但没有什么效果。

2. 和OpenMV联动后，抖动较严重，使用在OpenMV中使用大小环的方式，使得情况有一定的好转，但还是会有问题。

7.29-2: Stable with help of OP2 看来大部分原因是换了地毯，当地毯上没有胶带时，飞机定点较为平稳，但如果有胶带，飞机就会定点时摇摆，并且上下位移，思考到一个解决方案是放弃光流，纯粹用OpenMV，但现在这个解决方案会导致OpenMV无法有效控制飞机，飞机会乱飘。

7.30-1: Finish height and place fixed with OpenMV 通过调参完成了在圆上起飞，悬停15s再降落，也可偏离圆起飞，飞到圆上悬停，现在飞行、悬停都较为稳定。目前的代码取消了原厂的高度环，只用了PHS的高度环，在稳定后加强高度环的PID参数，但高度上仍然有一定的波动。下一步开始巡线，为了保证安全，要重新加回zigbee模块。

7.30-2：Zigbee DT Start again. 完成通过UART3的Zigbee通信，现在又可以传输数据并且使用32进行紧急停车了。

7.31-1: Zigbee Plus 完成了stm32的k0为急停，k1为一键降落。

7-31-2: Zigbee Plus2 更换了Zigbee，之前数据传输断连似乎是Zigbee模块连接有误的原因。现在进行的是识别火源。

8-2-1：Fix Height PID， 取消了切换PID，修改了高度PID。下一步测试偏航。