無人機定高算法

高度控制:高於目標值時應該減速，要輸出負值，所以高度差=目標高度-當前高度

速度控制：向上時(速度爲正)應該減速，要輸出負值，所以速度差=高度差的速度-當前速度

如果只有高度控制時：出現高度變化時，高度差要麼是正數要麼是負數。即使高度差會慢慢變小，但是高度差在沒有有到達目標高度時，高度差的符號是不會變的，符號不變就一直加速，儘管加速度在變小，有加速就一定會越過目標值。越過目標值纔會開始減速。在減速開始的瞬間，就意味着已經過越過目標點。高度差又出現了，再次重複上面的過程。所以不管怎麼調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;
}