代码解读七 文件名“Ano_MotorCtrl.c

本文件比较简单，代码比较少，主要涉及解锁后四个电机依次1/2/3/4转动，然后四轴也不会飞，只是在原地轻微转动，随后需要逐渐加油门至50%到达临界点，稍微往上推一点，电机声音就会发生变化，所以可以听声音来判断临界点，很好判断，同时也是执行最后一公里任务的重要组成部分，代码里面都有详细注释

#include "Ano_MotorCtrl.h"
#include "Ano_Math.h"
#include "ANO_RC.h"
#include "Drv_icm20602.h"
#include "Drv_spl06.h"
#include "Ano_Imu.h"
#include "Drv_pwm_out.h"
#include "Ano_MotionCal.h"
#include "Ano_Filter.h"
#include "Ano_Navigate.h"


/*
四轴：
      机头
(顺)m2     m1(逆)
     \   /
      \ /
      / \
     /   \
(逆)m3     m4(顺)
      机尾
*/


//重要数组
//第一个参数是最终需求，第二个参数起到临时变量的作用，最后把值传给第一个
s16 motor[MOTORSNUM];
s16 motor_step[MOTORSNUM];
//float motor_lpf[MOTORSNUM];

//计数器
static u16 motor_prepara_cnt;
_mc_st mc;
//见名知意，处于空闲状态.
//如果没猜错，处于这个状态的话只会让螺旋桨轻微转动
#define IDLING 200



//电机控制任务
//此文件仅此一个函数
void Motor_Ctrl_Task(u8 dT_ms)
{
	u8 i;
	
//	if(flag.taking_off)
//	{
//		flag.motor_preparation = 1;
//		motor_prepara_cnt = 0;			
//	}
	//如果准备好飞行了
	if(flag.fly_ready)
	{		
		//电机尚未准备好
		if(flag.motor_preparation == 0)
		{
			//开始计数
			motor_prepara_cnt += dT_ms;
			
			//再次判断，电机尚未准备好
			if(flag.motor_preparation == 0)
			{			
				//计数值0-300
				if(motor_prepara_cnt<300)
				{
					//第一个电机稍微转
					//简单说下
					//电机稍微转呈现的状态是解锁后四个电机依次启动然后轻微转动
					//随后抬油门至50%即临界点，过了50%即控制四轴起飞然后累加50%以上
					//的增量，因此飞机会越飞越高，同时向下打油门至50%以下，保持不变，
					//速度也会越来越低然后油门拉到底，无人机就降落了
					motor_step[m1] = IDLING;
				}
				//计数值300-600
				else if(motor_prepara_cnt<600)
				{
					//第二个电机稍微转
					motor_step[m2] = IDLING;
				}
				//计数值600-900
				else if(motor_prepara_cnt<900)
				{
					//第三个电机稍微转
					motor_step[m3] = IDLING;
				}	
				//计数值900-1200
				else if(motor_prepara_cnt<1200)
				{	
					//第四个电机稍微转
					motor_step[m4] = IDLING;
				}
				else
				{
					//电机准备好了同时计数清0
					flag.motor_preparation = 1;
					motor_prepara_cnt = 0;
				}
			}
			
		}	
	}
	else
	{
		//未准备好飞行，电机不启动
		flag.motor_preparation = 0;
	}
	

	//电机准备好了可以进行下一步
	//不难发现mc.ct_val_thr/mc.ct_val_rol/mc.ct_val_pit/mc.ct_val_yaw这四个变量
	//分别来自文件Ano_AltCtrl.c和Ano_AttCtrl.c，直接滑到文件最下面即可分别找到这四个最终输出变量
	//这四个值即最终作用到电机上的值，起到执行作用，作为负反馈控制中的一部分，得改变执行机构即电机
	//才能通过测量反馈至输入端与期望不断比较再次输出，起到控制作用。很重要！！！
	if(flag.motor_preparation == 1)
	{		
		motor_step[m1] = mc.ct_val_thr -mc.ct_val_rol +mc.ct_val_pit +mc.ct_val_yaw;
		motor_step[m2] = mc.ct_val_thr +mc.ct_val_rol +mc.ct_val_pit -mc.ct_val_yaw;
		motor_step[m3] = mc.ct_val_thr +mc.ct_val_rol -mc.ct_val_pit +mc.ct_val_yaw;
		motor_step[m4] = mc.ct_val_thr -mc.ct_val_rol -mc.ct_val_pit -mc.ct_val_yaw;
	}	
		
	//限幅200-1000
	for(i=0;i<MOTORSNUM;i++)
	{
		if(flag.fly_ready)
		{
			if(flag.motor_preparation == 1)
			{
				motor_step[i] = LIMIT(motor_step[i],IDLING,1000);
			}
	
		}
		else
		{		
			motor_step[i] = 0;
		}	
		//motor_step最终把值给了motor，起到临时变量的作用
		motor[i] = motor_step[i];		
	}


	
	//设置PWM控制电机旋转
	SetPwm(motor);
}