无人机定高算法

高度控制:高于目标值时应该减速，要输出负值，所以高度差=目标高度-当前高度

速度控制：向上时(速度为正)应该减速，要输出负值，所以速度差=高度差的速度-当前速度

如果只有高度控制时：出现高度变化时，高度差要么是正数要么是负数。即使高度差会慢慢变小，但是高度差在没有有到达目标高度时，高度差的符号是不会变的，符号不变就一直加速，尽管加速度在变小，有加速就一定会越过目标值。越过目标值才会开始减速。在减速开始的瞬间，就意味着已经过越过目标点。高度差又出现了，再次重复上面的过程。所以不管怎么调PID参数，都不可能稳定高度。

那么要稳定高度，就必须要到达目标高度之前，输出反向的力刹住。

重新分析高度差控制的环节会发现，到回到目标高的时候，输出减少到最小值了，但是因为是一直加速度回来，所以速度也达到到了最大值。那么就可以利用此时的速度来做刹车了。所以有了下面的速度控制。

如果只有速度控制器时：出现向上的速度时，输出负值，抵制加速，出现向下的速度时，输出正值，上升回来。那么高度控制实际上就是对速度的控制.

然而只有速度控制的话，由于高度计的精度就在那，微小的速度变化是检测不出来的。那么在缓慢下降和上升时就没办法调节了。

所以前面的高度差就有作用了，高度差作为微调添加到速度那边去计算。就算速度为0时，由于高度差的存在，输出也不会为0。直到高度差和速度都为0时才不输出。

现在思路就很清晰了，速度作为动态快速响应，高度作为微调校正。所以速度为主，高度为辅助，作为微调的高度差的PID的输出绝对不能越过速度。这么看的话高度环只有比例一个调节也可以了。

算法有了，剩下的问题是速度和高度怎么测量，用什么传感器了。

typedef struct {
	float kp;
	float ki;
	float kd;
	float max;
	float min;
	float error;
	float integral;
	float differential;
	float maxIntegral;
	float minIntegral;
	float lastError;
	float lastInput;
}PID_InitTypedef;
long CONSTRAIN(long input, long min, long max)
{
	if (input < min)
		input = min;
	if (input > max)
		input = max;
	return input;
}
float PID_ComputeInt(float error, PID_InitTypedefInt16* pid)
{

	pid->error = error;
	pid->integral += pid->error;
	pid->integral = CONSTRAIN(pid->integral, pid->minIntegral, pid->maxIntegral);
	pid->differential = pid->error - pid->lastError;
	//pid->lastInput = input;
	pid->lastError = pid->error;
	float result = pid->kp * pid->error + pid->ki * pid->integral/1000 + pid->kd * pid->differential;
	result = CONSTRAIN(result, pid->min, pid->max);
	return result;
}

/*定高串级PID计算*/
/*mesurement:测量到的高度数据*/
/*targetPoint：目标高度*/
/*_pidH：高度环PID*/
/*_pidV：速度环PID*/
/*Output: 速度*/
/*timestamp: 时间戳（us）*/
float KeepHeight(float mesurementHeight, float targetHeight, PID_InitTypedef* _pidH, PID_InitTypedef *_pidV, uint32_t timestamp)
{
	static float velocity;//高度的变化速度，
	static float velocityPre;//上次高度的变化速度，
	static float mesurementPre;//上次的高度
	static float result;
	static float resultPre;
	static float timestampPre;//上次时间戳
	float outH, outV;//高度度环输出，速度环输出
	float dt;//时间差,us
	dt = timestamp - timestampPre;//时间差,us
	timestampPre = timestamp;
	velocity = mesurementHeight - mesurementPre;//计算测量值的变化量，即速度
	velocity = velocity * 1000000.0f/ dt;//实际速度cm/s
	velocity = constrain(velocity, -1500, 1500);// 限制  velocity ±1500cm/s
	velocity = velocity * 0.2 + velocityPre * 0.8;//滤波一下
	velocityPre = velocity;
	mesurementPre = mesurementHeight;
	outH = PID_Compute(targetHeight - mesurementHeight, _pidH);//如果存在高度差，则会输出速度
	outV = PID_Compute(outH - velocity, _pidH);//达到上面的速度
	result += outV;//输出是调节值，要稳定输出则要累计。位置型PID
	result = constrainf(result, 1000, 2000);//限制输出范围
	result = result * 0.1 + resultPre * 0.9;//输出滤波，限制突变
	resultPre = result;
	return result;
}

高度计用是的歌尔的SPL06,加速度计是MPU6050,看看定高效果