四旋翼无人机从0到1的实现（二十一）无人机工程中数学计算

Author：家有仙妻谢掌柜
Date：2021/2/18

今年会更新一个系列，小四轴无人机从功能设计→思维导图→原理图设计→PCBLayout→焊接PCB→程序代码的编写→整机调试一系列，以此记录自己的成长历程！
这个小四轴无人机是大学时期学习制作的，加上现在工作学习对嵌入式的理解更加深入，因此想要重新梳理一下小四轴，之后在此基础上实现大四轴的飞控设计，这些都将在工作之余完成！

//小四轴无人机设计，死区函数，曲线拟合等

 #include "maths.h"
/*******************************************************************************
 * fuction	applyDeadband
 * brief	加死区函数
 * param	当前输入值  死区大小
 * return	处理后的值
 *******************************************************************************/  
float applyDeadband(float value,float deadband)
{
    
    
	if(fabs(value)<=deadband)  value = 0;
	if(value>deadband)    value -= deadband;
	if(value<-deadband)   value += deadband;
	return value ;
}
/*******************************************************************************
 * fuction	constrain_float
 * brief	限定最大最小值输出
 * param	当前值  最小值  最大值
 * return	处理后的值
 *******************************************************************************/ 
 float constrain_float(float amt,float low,float high )
 {
    
    
	if(isnan(amt))
	{
    
    
		return (low+high)*0.5f;
	}
    // 条件判断? 结果: 当前值
	/* 先比小再比大 */
	return ((amt)< (low)? (low):((amt)> (high)? (high): (amt)));      //   条件判断? 结果: 当前值
	//(amt)> (high)? (high): (amt)  
}
/*******************************************************************************
 * fuction	Get_K
 * brief	获取 Y = KX+B中的K
 * param	两坐标点的坐标Ax Ay  Bx By 
 * return	比例系数K
 *******************************************************************************/ 
float Get_K(float Ax, float Ay , float Bx, float By)
{
    
    
	float Tmp1 ,Tmp2;
	Tmp1 = By - Ay;
	Tmp2 = Bx - Ax;
	return  Tmp1/Tmp2;
}
/*******************************************************************************
 * fuction	Get_B
 * brief	获取 Y = KX+B中的B
 * param	两坐标点的坐标Ax Ay  Bx By 
 * return	常数B
 *******************************************************************************/ 
float Get_B(float Ax, float Ay , float Bx, float By)
{
    
    
	float Tmp1 ,Tmp2,TmpK;
	Tmp1 = By - Ay;
	Tmp2 = Bx - Ax;
	TmpK = Tmp1/Tmp2;
	return (Ay-Ax*TmpK);
}
/*******************************************************************************
 * fuction	Calc_Curve
 * brief	处理曲线方程参数
 * param	无 
 * return	无
 *******************************************************************************/ 
void Calc_Curve(void)
{
    
    
	uint8_t i;
	for(i=0;i<8;i++)
	{
    
    
	//	Curve_K[i]= Get_K(float Ax, float Ay , float Bx, float By);
		Curve_K[i]= Get_K(PointX[i], PointY[i] , PointX[i+1], PointY[i+1]);
		Curve_B[i]= Get_B(PointX[i], PointY[i] , PointX[i+1], PointY[i+1]);
	}
}
/*******************************************************************************
 * fuction	Curve_Ctrl
 * brief	油门曲线拟合处理函数
 * param	Curve_X 处理前的数据
 * return	Curve_Y 拟合后的数据
 *******************************************************************************/ 
float Curve_Ctrl(float Curve_X)
{
    
    
	uint8_t Curve_index;
	Curve_index = Curve_X/118.0f;
	if(Curve_index>7) Curve_index=7;
	//  Y = K X + B   Curve_K[i]  Curve_B[i]
	Curve_Y =  Curve_X*Curve_K[Curve_index]+Curve_B[Curve_index];
	return Curve_Y;
}

#ifndef _MATHS_H__
#define _MATHS_H__

#include "board_define.h"
#include "var_global.h"

float applyDeadband(float value,float deadband);
float constrain_float(float amt,float low,float high );
float Get_K(float Ax, float Ay , float Bx, float By);
float Get_B(float Ax, float Ay , float Bx, float By);
void Calc_Curve(void);
float Curve_Ctrl(float Curve_X);

#endif