版權聲明：本文爲博主原創博文，未經允許不得轉載，若要轉載，請說明出處並給出博文鏈接
本文針對AP_TECS的框架給出對應代碼及個人理解體會。
首先進行update_50hz（）這個函數，50hz更新一次，該函數會更新飛機的高度h、向上爬升速率climb_rate。
接下來，就是TECS的核心工作流程，即下面這段代碼（10hz更新一次）。其中會有一些控制邏輯諸如限幅、濾波等不做詳解。
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
enum AP_Vehicle::FixedWing::FlightStage flight_stage,
float distance_beyond_land_wp,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor,
bool soaring_active)
{
// Calculate time in seconds since last update
uint64_t now = AP_HAL::micros64();
_DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
_update_pitch_throttle_last_usec = now;
_flags.is_doing_auto_land = (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
_distance_beyond_land_wp = distance_beyond_land_wp;
_flight_stage = flight_stage;
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;
_EAS_dem = EAS_dem_cm * 0.01f;
// 二階互補濾波器估計出速度
// （參見博客https://blog.csdn.net/u012814946/article/details/81290163）
_update_speed(load_factor);
if (aparm.takeoff_throttle_max != 0 &&
(_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
_THRmaxf = aparm.takeoff_throttle_max * 0.01f;
} else {
_THRmaxf = aparm.throttle_max * 0.01f;
}
_THRminf = aparm.throttle_min * 0.01f;
// work out the maximum and minimum pitch
// if TECS_PITCH_{MAX,MIN} isn't set then use
// LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
// larger than LIM_PITCH_{MAX,MIN}
if (_pitch_max <= 0) {
_PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f;
} else {
_PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
}
if (_pitch_min >= 0) {
_PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
} else {
_PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
}
// apply temporary pitch limit and clear
if (_pitch_max_limit < 90) {
_PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit);
_PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf);
_pitch_max_limit = 90;
}
if (_landing.is_flaring()) {
// in flare use min pitch from LAND_PITCH_CD
_PITCHminf = MAX(_PITCHminf, _landing.get_pitch_cd() * 0.01f);
// and use max pitch from TECS_LAND_PMAX
if (_land_pitch_max != 0) {
_PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max);
}
// and allow zero throttle
_THRminf = 0;
} else if (_landing.is_on_approach() && (-_climb_rate) > _land_sink) {
// constrain the pitch in landing as we get close to the flare
// point. Use a simple linear limit from 15 meters after the
// landing point
float time_to_flare = (- hgt_afe / _climb_rate) - _landing.get_flare_sec();
if (time_to_flare < 0) {
// we should be flaring already
_PITCHminf = MAX(_PITCHminf, _landing.get_pitch_cd() * 0.01f);
} else if (time_to_flare < timeConstant()*2) {
// smoothly move the min pitch to the flare min pitch over
// twice the time constant
float p = time_to_flare/(2*timeConstant());
float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*_landing.get_pitch_cd();
#if 0
::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif
_PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f);
}
}
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
// we have reached our target speed in takeoff, allow for
// normal throttle control
_flags.reached_speed_takeoff = true;
}
}
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd, hgt_afe);
// 更新總體能量速率限制
_update_STE_rate_lim();
// 更新出速度的目標值
_update_speed_demand();
// 更新出高度的目標值
_update_height_demand();
// 檢測是否欠速
_detect_underspeed();
// 更新勢能、動能的目標與估計值
_update_energies();
// 更新油門的目標值 - 如果沒有空速計，就用簡單的pitch到油門
// Note that caller can demand the use of
// synthetic airspeed for one loop if needed. This is required
// during QuadPlane transition when pitch is constrained
if (_ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed || _use_synthetic_airspeed_once) {
_update_throttle_with_airspeed();
_use_synthetic_airspeed_once = false;
} else {
_update_throttle_without_airspeed(throttle_nudge);
}
// Detect bad descent due to demanded airspeed being too high
_detect_bad_descent();
// 一飛沖天時，不會觸發差的下降
if (soaring_active) {
_flags.badDescent = false;
}
// 更新出俯仰角的目標值
_update_pitch();
// 記下LOG
DataFlash_Class::instance()->Log_Write(
"TECS",
"TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f",
"smnmnnnn----o--",
"F0000000----0--",
"QfffffffffffffB",
now,
(double)_height,
(double)_climb_rate,
(double)_hgt_dem_adj,
(double)_hgt_rate_dem,
(double)_TAS_dem_adj,
(double)_TAS_state,
(double)_vel_dot,
(double)_integTHR_state,
(double)_integSEB_state,
(double)_throttle_dem,
(double)_pitch_dem,
(double)_TAS_rate_dem,
(double)logging.SKE_weighting,
_flags_byte);
DataFlash_Class::instance()->Log_Write("TEC2", "TimeUS,KErr,PErr,EDelta,LF", "Qffff",
now,
(double)logging.SKE_error,
(double)logging.SPE_error,
(double)logging.SEB_delta,
(double)load_factor);
}
看到這裏，我們可以回顧一下《TECS的基礎與理論》。想進一步學習的可以去看《基於飛機總能量控制系統(TECS)的飛行航跡、速度解耦控制方法研究》。
結合理論知識，我們可以對應代碼找到幾個關鍵的函數：
①_update_speed(load_factor)
此函數主要目的是計算出真實的空速數值，詳細可以參見ArduPilot—AP_TECS—更新速度
②_update_speed_demand();
此函數主要目的是計算出下一步需要用到的空速目標值以及空速速率（微分項）目標值，這裏用到了真實空速和總能量速率的最大最小值來對空速目標值進行一些速率限制，以防超過物理性能上的限制。
③_update_height_demand();
此函數主要目的是計算出下一步需要用到的飛機高度目標值以及高度速率（微分項）目標值
④_update_energies();
此函數主要目的是計算出下一步需要用到的總能量，更新勢能、動能以及勢能微分和動能微分的目標值和實際估計值
⑤_update_throttle_with_airspeed();
此函數主要目的是計算出下一步需要用到的油門目標值（使用空速計的情況，這裏暫且不討論不使用空速計的情況）
採用總能量的誤差作爲控制器的輸入，通過PID+前饋控制器，得到油門的目標值。這裏與理論中講到的也不太一致，理論中用總能量的誤差作爲積分器的輸入，用總能量的反饋作爲P的輸入，是爲了防止較大的超調，但是這裏因爲加入了D阻尼，所以對超調有一點的抑制作用。
油門的目標值將直接作爲飛機油門的通道的輸出，提高飛機螺旋槳旋轉，進一步提供飛機的推力。
⑥_update_pitch();
此函數主要目的是計算出下一步需要用到的pitch角的目標值，理論上應該求到的是航跡角，但是一般情況認爲飛機的攻角比較小，從而通過將升降舵的偏角作爲pitch角的目標值，以此來更好地控制航跡角。
這裏計算了勢能和動能的均衡值，即對動能和勢能均進行加權，兩個權重之和爲2，如果動能的權重爲2，那麼勢能的權重爲0，此時飛機更加偏向動能，也就更加逼近速度目標。反之，飛機更加傾向勢能，也就更加逼近飛機的高度目標，如果權重爲1，說明雙方兼顧，能量傾向更均衡。
// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
進一步通過均衡能量的目標值與估計值做差，計算均衡能量誤差及其微分值。
最終的pitch的目標值也是利用均衡能量誤差作爲控制輸入，經過PID控制器輸出的。
更新得到的pitch目標值就會進入pitch controller裏面，與導航觀測到的pitch估計值做差，進一步做姿態角的控制，進而改變升降舵的偏移量，從而控制飛機的勢能與動能的均衡。
經過上面的一些TECS的基礎理論的理解，以及相關控制流程的分析，相信大家多少會對TECS有個整體的認識了。已上全部是個人的一些學習體會，如有錯誤不足，還請指出，必定改正，感謝。
相關ArduPilot的學習博客鏈接：
④ L1 control——ArduPilot——更新航點update_waypoint（一）