fw_pos_control模組

class landingslope

為固定翼著陸的角度變化模組

• calulateSlopeValues() void private
  更新H1,H0,d1,根據log（H0/H1）的比例調整 d1 / d1+ delta d的比例，更新其他引數
• getLandingSlopeRelativeAltitude(wp_landing_distance) float
  返回在距離落航點的著陸坡上點的相對高度，呼叫多引數的同名函式
• getLandingSlopeRelativeAltitudeSave(wp_landing_distance,bearing_lastwp_currwp,bearing_airplane_currwp) float public
  返回在距離落航點的著陸坡上點的相對高度，檢查飛行器是否在航點上來避免爬升
• getLandingSlopeAbsoluteAltitude(wp_landing_distance,wp_altitude) float
  返回在距離落航點的著陸坡上點的絕對高度

基本上示例都是這麼一個結構

• getLandingSlopeAbsoluteAltitudeSave(wp_landing_distance,bearing_last_wp_currwp,bearing_airplane_currwp) float
• 檢查飛行器是否在航點上來避免爬升
• getLandingSlopeRelativeAltitude(wp_landing_distance,horizontal_slope_displacement,landing_slope_angle_rad) float static
  返回h_flare.rel的高度，返回在距離落航點的著陸坡上點的相對高度
• getLandingSlopeAbsoluteAltitude(wp_landing_distance,wp_landing_altitude,horizontal_slope_displacement,landing_slope_angle_rad) float static
  返回h_flare.rel + H1的高度，返回在距離落航點的著陸坡上點的絕對高度
• getLandigSlopeWPDistance(slope_altitude,wp_landing_altitude,horizontal_slope_displacement,landing_slope_angle_rad) float static
  給定降落高度，返回距離預定降落點的距離
• getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp) float
• getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, wp_altitude) float
  獲取Flare曲線的相對高度
• updata(landing_slope_angle_Rad_new, flare_relative_alt_new, mmotor_lim_relative_alt_new, H1_virt_new) void
  將值重新賦值，並且重新呼叫calcuateSlopeValues()

class lib/extern_lgpl/tecs

這個類負責控制俯仰和油門來控制速度和高度，在後面會用到，不做深入講究，只是為了簡紹這個類的作用

/*
 *  Written by Paul Riseborough 2013 to provide:
 *  - Combined control of speed and height using throttle to control
 *    total energy and pitch angle to control exchange of energy between
 *    potential and kinetic.
 *    通過控制油門來控制能量，控制俯仰控制勢能，使得速度高度結合控制，在勢能和運動結合
 *    Selectable speed or height priority modes when calculating pitch angle
 *    當計算俯仰角度時優先選擇速度/高度模式
 *  - Fallback mode when no airspeed measurement is available that
 *    sets throttle based on height rate demand and switches pitch angle control to
 *    height priority
 *    當空速測量不管用時，切換俯仰角度優先控制到高度優先控制，並設定油門基於高度速率需求
 *  - Underspeed protection that demands maximum throttle switches pitch angle control
 *    to speed priority mode
 *    需求最大油門的低速保護下，切換俯仰控制優先到速度優先模式
 *  - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use
 *    of easy to measure aircraft performance data
 *    通過使用直觀的時間常數，微調速率和阻尼引數以及使用易於測量飛機效能資料的相對易於調諧
 *   
 */

class FixedwingPositionControl

FixedwingPositionControl:

使用param_find()函式定位所有的引數,賦初值

start() int static public

開始控制

task_running() bool

任務的狀態

更新控制函式：

• parameters_update() int private
  更新所有的類內參數，使用param_get()方法
• control_update() void
  更新控制輸出
• vehicle_control_mode_poll()
  飛行器模式更新，通過orb_check來檢測是否更新
• vehicle_status_poll()
  飛行器狀態更新，通過orb_check
• vehicle_manual_control_setpoint_poll()
  飛行器手動控制設定點更新，同上
• control_state_poll()
  控制狀態更新
  如果空速1s內沒有更新，認定是非法的。
  將四元組的控制資訊轉化為尤拉角度
  更新TECS的狀態
• vehicle_sensor_combined_poll()
  飛行器感測器狀態更新
• vehicle_setpoint_poll()
  設定點更新
• navigation_capabilities_publish()
  導航能力輸出

get_demanded_airspeed() float

返回需求的空速

實現：

根據油門輸入(_manual.z)大於0.5與否，決定速度應該變大還是變小（按照比例）

// 如果油門輸入<0.5，輸出空速應該是 最小速度+(可縮減速度)*(2×輸入值)  

    // 可縮減速度為當前速度到最小速度的差，以0.5*2為1，中值，不會變化

calculate_target_airspeed(airspeed_demand) float

返回目標速度，輸出需求速度

實現：

一般只對需求速度限制在最大和最小的範圍內

在目的空速加上最小下衝速度(_groundspeed_undershoot)，只有在有強風時候這個最小下衝速度不為0

calculate_gnd_speed_undershoot(&current_position,&ground_speed_2d,&pos_sp_triplet)

計算對地下衝速度，輸出到_groundspeed_undershoot屬性中

說明:

對地下衝速度（固定翼速度不夠，升力<重力 產生的飛行高度下降速度） 是飛機沒有到達一定地速的結果。因此，當空速大於最小地速時候，對地下衝速度為0，否則為正當小於最小地速時候。

get_waypoint_heading_distance(heading, distance, &wp_prev, &wp_next) void

基於方向和當前的距離獲取下一個waypoint，輸出到wp_prev,wp_next

實現：

如果是flag_init，根據角度和距離獲取前後航點

否則，根據前一幀的前後航點說確定的直線，根據預先定義的距離，獲取新的前後航點。

get_terrain_altitude_landing(land_setpoint_alt, &global_pos) float

返回降落點的地形高度

實現：

如果地形評估無效，直接使用設定點的高度

如果使用地形評估，並切換到地形評估，一直使用到降落為止

get_terrain_altitude_takeoff(takeoff_alt, &global_pos) float

返回起飛時候的地形高度

實現：

如果global_pos中地形高度有效返回，否則返回起飛高度takeoff_alt

in_takeoff_situation() bool

檢查是否正在起飛中

實現：

判斷是否剛進入空中，油門輸入是否到門限，高度是否符合最小的起飛的條件

do_takeoff_help(*hold_altitude,*pitch_limit_min)

定高模式下的起飛引數資訊的獲取函式，獲得起飛的高度和最小俯仰，傳入引數指標，直接對地址修改

實現：

如果正在起飛，更新定高高度為起飛最小高度和最小俯仰；否則，根據引數pitch_limit_min更新最小俯仰

update_desired_altitude(dt) bool

根據俯仰輸入更新期望高度,輸出到屬性_alt_hold,引數為Time step，單位時間,返回的bool型別是是否是爬升模式。

實現：

0.06 ~ -0.06 視為0， 其餘1 ~ 0.06 | -0.06 ~  -1 部分按比例，更新高度 = 原高度 + 最大變化速率*dt * pitch （俯仰指數）

如果是垂直起降飛機，在旋翼狀態/轉化狀態，高度是當前高度。

control_position(global_pos, ground_speed, pos_sp_triplet) bool

控制位置，根據輸入的位置，速度，和航點，（存在全域性變數的飛行器狀態），輸出對應的控制姿態_att_sp（全域性變數）

get_tecs_pitch() float

如果mTecs可行，返回getPitchSetpoint()否則返回_tecs.get_pitch_demand()

get_tecs_thrust() float

同上

get_demanded_airspeed() float

獲取需要的空速大小

實現演算法如下：

判斷輸入油門的大小，設定可變化速度為當前速度距最大/最小速度的差，airspeed_trim字面上是修剪的空速速度（即當前速度）

    如果油門輸入小於0.5，輸出速度=最小速度+可變化速度×油門輸入值×2；（如果油門輸入值0.5，則速度不變化，在輸入值小於0.5的情況下，油門輸入×2 < 1）

    如果油門大於0.5，輸出速度 = 當前速度 + 可變化速度 × （油門輸入值-0.5）*2（減去0.5的基礎油門）

calculate_target_airspeed(airspeed_demand) float

計算目標空速，沒看懂想幹嘛

實現：

當前空速根據全域性變數_airspeed_valid是否合法，合法取當前控制狀態的空速，否則取中值。

目標空速= airspeed_demand+_groundsped_undershoot(期望速度+地速)，再對目標空速進行限制，使其在最大最小之間。

_airspeed_error這個全域性變數是目標速度和當前空速之差，應該是用來調整速度的。

返回目標速度

calualte_gndspeed_undershoot(current_pos, ground_speed_2d, pos_sp_triplet) void

計算對地下降速度_groundspeed_undershoot，對地下衝速度（固定翼速度不夠，升力<重力 產生的飛行高度下降速度） 是飛機沒有到達一定地速的結果。因此，當空速大於最小地速時候，對地下衝速度為0，否則為正當小於最小地速時候。

根據distance和delta_altitude計算期望的速度，與當前速度之差就是應該對地下降的速度，

task_main_trampoline()

構建FixedwingPositionControl類，賦給l1_control名稱空間的g_control指標，開始執行task_main()函式，然後釋放。

task_main() void

先訂閱需要的資訊，設定更新速率等。

fds[2]，fds[0]負責檢查引數更新，fds[1]負責位置更新

while

px4_poll檢查是否有更新，更新control_mode,status

如果引數更新，獲取更新引數

如果位置更新，control_statue,setpoint,sensor_combined,manual_control_setpoint更新，執行control_position()函式，如果控制成功，通過uorb機制輸出控制結果資訊，最後如果導航資訊很久沒有更新，輸出導航資訊沒看懂

reset_takeoff_state() void

reset_landing_state() void

tecs_update_pitch_trottle() void

一個呼叫實現TECS的包裝器函式(mTECS是隻通過引數啟用)

類的實現流程：

從fw_pos_control_l1_main函式開始執行，這個main方法有命令列引數，根據引數，輸出是否fw_pos_control_l1這個模組的執行狀態，實際執行呼叫l1_control::g_control指標所建立的物件,呼叫FixedwingPositionControl::start方法的px4_task_spawn_cmd來初始化建立物件。再通過task_main_trampoline真正建立類的物件。

FixedwingPositionControl::task_main()正式開始：

訂閱全域性位置，三個一組的航點位置，控制狀態，感測器狀態，控制模式，飛行器狀態，引數，手動輸入等通過uORB訂閱。設定更新速率

• 設定px4_pollfd_struct_t fds[2]這個結構體，fds[0]作為_params_sub，fds[1]作為_global_pos_sub的監視，開始迴圈。
• 更新飛行器的控制模式，狀態資訊。如果引數更新了(fds[0]),呼叫paramters_update()更新所有的引數。
• 如果位置發生變化(fds[1]),
  呼叫perf_begin(_loop_perf),將最新的位置更新出來。
  更新控制狀態資訊，設定點資訊，感測器的資訊，手動控制的資訊。
  構造ground_speed(ned3個方向的速度),current_position(經緯座標)，呼叫control_position(),所有的控制內容都在這個函式中實現
  控制成功，通過uORB輸出_attitude_sp_pub,
  呼叫perf_end(_loop_perf)結束

注：_global_pos是當前飛行器的位置

_paramters是全域性引數

pos_sp_triplet是航點三兄弟的結構體

_att_sp設定的姿態控制

_vehicle_status 飛行器的狀態

_manual 手動控制輸入的結構體

control_position()的函式執行流程：

bool control_position(
    const math::Vector<2> &current_position,  //當前位置，經緯座標
    const math::Vector<3> &ground_speed,      //三軸加速度
    const struct position_setpoint_triplet_s &pos_sp_triple) //航點集合

1. _control_position_last_called記錄上一次位置控制的時間
2. 判斷是否是固定翼模式/VTOL的變形模式，否則return false
3. 預設不使用方向舵，不使用襟翼,eas2tas(基於當前的測量來計算實際值),accel_body是飛行器的加速度，accel_earth是飛行器對地的加速度，得到這個引數用於tecs.update_state()
4. 構造ground_speed_2d，(2個維度的速度)，作為calualte_gndspeed_undershoot()函式的引數
5. in_air_alt_control是判斷飛行器是否滯空的標識，飛行器的控制模式在auto|velocity|altitude下 & 飛行器沒有降落(!_vehicle_status.condition_landed),是可以控制的，同樣，這個引數用於tecs.update_state()
6. 如果mTecs不可用，就使用tecs.update_state()更新引數
7. 計算下衝速度_gndspeed_undershoot
8. 計算高度差altitude_error，目標高度-當前高度
9. 設定油門上限
10. _was_in_air置為true，記錄在空中的時間_time_went_in_air和高度_takeoff_ground_alt
11. 如果飛行器已經降落(_vehicle_status.condition_landed)，_was_in_air置為false

12. 根據飛航模式控制(每一個模式下的控制都分為水平控制和垂直控制，水平控制交給l1_control類，更新yaw和roll，垂直控制交給tecs_update更新油門俯仰)

    • 自動模式下當前航點有效 flag_control_auto_enabled & pos_sp_triplet.current.valid

      1. 如果從FW_POSCTRL_MODE_OTHER(其他模式)切換過來，重置積分器。_control_mode_current全域性變數記錄當前模式，如果mTecs可用，重置mTecs,否則tecs.reset_state(),將當前模式置為FW_POSCTRL_MODE_AUTO
      2. 更新定高高度_hold_alt為當前高度，定航航向_hdg_hold_yaw為當前航向
      3. 獲取控制物件_l1_control的circle_mode()狀態,後面在控制完成後決定是否重置積分器；
      4. 儲存speed weight到tecs中，防止速度值立馬改變
      5. 構造next_wp,curr_wp,prev_wp(作為l1_control導航控制函式的引數)
         前倆個使用pos_sp_triplet物件的current賦值，prev_wp根據pos_sp_triplet的previous是否合法：
         若是，使用previous的值;
         否則，使用current的值。
      6. 目標速度(mission_airspeed)預設是需要修改的速度(_parameters.airspeed_trim),
         如果當前的巡航速度符合範圍&巡航速度>0.1，改為當前巡航速度 (_pos_sp_triplet.current.cruising_speed)
         (目標速度作為calculate_target_airspeed()的引數用於計算最終輸出的速度)

      7. 根據當前航點的狀態控制

         • 怠機狀態(SETPOINT_TYPE_IDLE),可認為在地上中
           (_att_sp應該就是控制輸出)
           油門，俯仰，滾轉為0
         • 普通航點(SETPOINT_TYPE_POSITION)
           
           1. 使用_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d)做普通的導航，獲得_att_sp.roll_body,yaw_body的值(通過呼叫_l1_control的方法)
              (這個類只根據航點的資訊，當前的狀態去區分控制，然後分別把具體的控制類的對應函式去更新引數，比如水平控制交給l1_control,垂直控制交給mtecs/tecs;屬於比較高階呼叫類)
           2. 呼叫tecs_update_pitch_throttle()更新俯仰和油門
         • 徘徊航點(SETPOINT_TYPE_LOITER)
           
           1. 使用_l1_control.navigate_loiter()方法，做徘徊的導航，獲得_att_sp.roll_body,yaw_body的值(通過呼叫_l1_control的方法)
           2. 根據條件設定高度：
              如果是因為中止著陸而進入徘徊模式，需要將高度設定高於降落模式
              否則，設定為當前航點的高度
           3. 判斷是否是起飛模式 | 因為中止降落而沒有低於安全高度
              是，限制滾轉在15°內
           4. tecs_update_pitch_throttle()更新俯仰和油門

         • 降落航點(SETPOINT_TYPE_LAND)(這個航點的內容 比較多，水平控制垂直控制分別註釋)

           1. 降落模式開啟襟翼，記錄開始著陸的時間_time_started_landing
              // Horizontal landing control
           2. 計算上一個航點到當前航點的方向bearing_lastwp_currwp,飛行器到當前航點的方向bearing_airplane_currwp，以及當前位置到當前目標航點的距離wp_distance
           3. 如果飛行器到航點的方位-上一個航點到當前航點的方位差>90°
              設定wp_distance_save=0，偏差太大
              （建立虛擬航點在期望的路徑上，但是一些距離落後於航點（沒看懂這句）。這樣可以確保飛機即使在關閉廢除降落航點時也能在期望飛行路徑上）
           4. 根據方位和距離產生虛擬航點curr_wp_shifted,但是感覺後面沒用啊
           5. 如果當前距離據航點距離小於著陸的指向距離|| 著陸進入無可返回的狀態
              （可以認為是準備水平方向準備就緒或者為了進入安全距離的保護，鎖住方向，沿著當前路徑一直走）(程式裡大多判斷是 條件判斷 || 屬性判斷 ，條件判斷符合條件後，設定對應屬性為true，下次可以直接進入)
              是：
              如果land_noreturn_horizontal是false，第一次進入,設定target_bearing
              如果前一個航點合法，target_bearing=bearing_lastwp_Currwp;
              否則，為當前航線_yaw
              通過_l1_control.navigate_heading()做定航向的導航
              否：
              _l1_control.navigate_waypoint() 只做普通的航點控制
           6. 獲得_att_sp.roll_body,yaw_body的值(通過呼叫_l1_control的方法)
           7. 如果land_noreturn_horizontal為true，水平方向無法回退了，
              限制滾轉角度-10~10°
              //Vertical landing control
              (使用tecs姿態控制來降落,如果近地，使用最小的俯仰，限制滾轉來，而且高度誤差為負，為了降落)
           8. 計算得到降落油門(throttle_land)，降落空速(airspeed_land)，進場空速(airspeed_aproach)
           9. 判斷是否有地形評估(_paramters.land_use_terrain_estimate)：
              是：
              判斷地形評估高度是否有效(_global_pos.terrain_alt_valid):
              設定地形評估高度，上一個地形評估的值，上一個更新的值
              如果是第一次進入，之前沒有設定過(_time_last_t_alt==0):
              如果距開始降落10s內
              是：地形評估設定為當前航點的高度
              否：地形評估高度設定為當前航點的高度，但是中止導航。(abort_landing = true)
              如果地形評估無效&距上一次更新的時間沒有超過規定 || 垂直控制no way back
              設定地形評估為上一次有效值
              否則：
              地形評估高度設定為當前航點的高度，但是中止導航。(abort_landing = true)
              否：
              地形評估高度設為當前航點的高度(terrain_alt = pos_sp_triplet.current.lat)
           10. 計算上一個有效的航點的相對高度L_altitude_rel,並呼叫landslope類獲得距離目標點wp_distance的距離時候的期望高度landing_slope_alt_rel_desired
           11. （如果當前飛行器的高度<地面高度+降落曲線的高度 && 距離<降落距離+5 ） || 垂直控制no way back
               是：//可以帶油門降落，因為高度比預計高度低
               設定油門最大值(這一項先做)，設定設定點的航向，執行使用垂尾
               如果高度低於發動機限制高度 | 降落髮動機限制開啟
               設定油門的最大值(在當前油門和引數中的油門中選小的)
               將land_motor_lim設為true
               獲取flare_curve_alt_Rel
               如果當前的垂直相對高度》上一個垂直相對高度 & no way back 認為在地上停留
               使用tecs更新油門和俯仰
               如果目前還沒有到垂直控制no way back 的地步
               是：設定目標俯仰為0，計算height_flare，設定land_noreturn_vertical=true
               否：當前姿態中vel_d是否>0.1
               是：設定picth_body?
               否：不改變pitch_body
               更新flare_curve_alt_rel_last為當前的值
               否：//這裡通過滑翔曲線來使高度過高的點下降,直到到與期待的航線的交叉點
               如果當前高度>地形高度+降落期望高度 || land_onslope
               是：期待高度alttitude_desired_rel設為landing_slope_alt_rel_desired
               否：altitude_desired_rel設為L_altitude_rel?1615
               使用tecs更新油門和俯仰

         • 起飛航點(SETPOINT_TYPE_TAKEOFF)
           如果runway_takoff可用
           是：runway_takeoff的起飛模組的具體控制，猜的

           1. 如果runway_takeoff沒有初始化，初始化它
           2. 獲取地形高度terrain_alt
           3. 使用_runway_takeoff模組更新
           4. 使用_l1_control的航點導航模式(為什麼要使用航點導航，難道起飛與普通導航只有爬升模式一個區別嗎？不過這麼想貌似也沒問題)
           5. 獲取_runway_takeoff的最大俯仰角takeoff_pitch_max_deg
           6. 使用tecs更新油門俯仰
           7. 從_runway_takeoff模組獲取roll_bady,yaw_body,fw_control_yaw,pitch_body,roll_reset_integral,pitch_reset_integral的值
              否：//如果runway_takeoff模組不可用，使用起飛檢測模組
              如果起飛檢測模組可用&&launch_detection_state 不是LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS
              是：
              如果距上一次更新時間>4e6，給控制檯訊息
              起飛檢測更新(launchDetector.update())
              起飛檢測狀態更新（launch_detection_state）
              否：
              起飛檢測不可用，置狀態為需要控制
              //起飛控制依賴於起飛檢測狀態
              如果起飛檢測狀態 != LAUNCHDETECTION_RES_NONE
              是：飛行器起飛
              使用_l1_control進行航點控制
              獲取_l1_control的roll_body,yaw_body
              選擇油門：只有在LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS模式下，我們使用全油門，否則其他我們使用先前起飛的油門
              選擇最大的俯仰角度：起飛檢測模組可能會根據起飛狀態給俯仰強加一個限制
              使用最小的俯仰和最小的滾轉如果目標高度不在爬升誤差之內
              否：
              認為在地上，重置積分器，設定預設的roll,pitch
      8. 如果是降落航點，重置降落狀態；
         如果是起飛航點，重置起飛狀態
         如果航點前是迴圈模式，航點後不是迴圈模式，重置積分器
    • 在定速模式下高度控制模式 flag_control_velocity_enabled & flag_control_altitude_enabled
      俯仰設定高度，油門設定速度，航向設定航點
      
      1. 如果當前模式不是FW_POSCTRL_MODE_POSITION,需要重置一下，_hold_alt,_hdg_hold_yaw,
      2. 如果當前模式是FW_POSCTRL_MODE_OTHER,重置積分器（mTecs可用重置mTecs，否則重置tecs）
      3. 將控制模式改到FW_POSCTRL_MODE_POSITION
      4. 獲取期望速度altctrl_airspeed和爬升climbout_requested,?do_takeoff_help
      5. 油門最大值設定為引數的油門最大值，如果飛行器有條件降落且油門值小於油門門限，油門最大值為0
      6. 使用tecs更新油門和俯仰
      7. （航向控制）如果航向的輸入值小於偏航的門限
         是：
         如果控制狀態(_ctrl_state)的偏航速率小於設定的門限且偏航沒有被鎖定，鎖定偏航（_yaw_lock_engaged設為true）
         //如果使用者試圖在定航模式下起飛，重置航向確保起飛不滾轉
         如果是在起飛情況：_hdg_hold_enable -> false，_yaw_lock_engaged -> true(啟用航線鎖定)
         如果是_yaw_lock_engaged(設定對應屬性鎖定航向)
         將_hdg_hold_enabled設為true，設定_hdg_hold_yaw為當前航線
         前面獲取到prev_wp,curr_wp,給_l1_control做引數，使用其做航點控制，獲取roll,yaw的輸出值
         如果是起飛情形，限制roll在15°內
         否：
         _hdg_hold_enable -> false，_yaw_lock_engaged -> false
    • 在高度控制模式下 flag_control_altitude_enabled
      
      1. 如果控制模式不是位置控制模式/高度控制模式(FW_POSCTRK_MODE_POSITION/ALITUDE),設定定高(_hold_alt)為當前高度
      2. 如果當前模式是FW_POSCTRL_MODE_OTHER,重置積分器（mTecs可用重置mTecs，否則重置tecs）
      3. 設定控制模式為FW_POSCTRL_MODE_ALTITUDE
      4. 獲取期望速度altctrl_airspeed和爬升climbout_requested,?do_takeoff_help
      5. 油門最大值設定為引數的油門最大值，如果飛行器有條件降落且油門值小於油門門限，油門最大值為0
      6. 使用tecs更新油門和俯仰
    • 其他情況下
      
      1. 將控制模式設為FW_POSCTRL_MODE_OTHER，設定定高(_hold_alt)為當前高度
      2. 設定_time_started_landing 和_time_last_t_alt為0
      3. abort_landing 為false,重置起飛降落狀態
13. copy thrust output for publication，根據具體條件設定油門
    
    1. 如果飛行器引擎出了問題，
       輸出 0
    2. 在自動模式下 & 當前點是起飛點 & 降落檢測狀態是ENABLEMOTORS & _runway_takeoff不可用
       根據launchDetectionEnabled的狀態，可用就設定油門為起飛的油門，否則為怠機油門
    3. 如果是自動模式 & 當前航點是起飛航點 & _runway_takeoff可用
       油門設為runway_takeoff的有油門
    4. 如果自動模式 & 航點為怠機航點
       油門為0
    5. 其他
       如果飛行器有降落條件：
       是：油門在最大和怠機油門中取最小
       否：控制油門(get_tecs_thrust())|油門最大值中取最小
14. 如果不滿足一下條件：
    (是自動模式 &&（（航點為起飛航點 &&（起飛檢測為 RES_NONE || runway_takeoff 可用）） || （航點為降落航點 && 垂直方向降落nowayback））)
    輸出控制的俯仰為get_tecs_pitch()
15. 根據控制模式的flag_control_position_enabled(是否是手動控制)
    是：last_manual 為false（用這個屬性覺得下次是否重置積分器）
    否：last_manual為 true

tecs_update_pitch_throttle()函式的執行流程：

void tecs_update_pitch_throttle(
    float alt_sp,                           //目標高度
    float v_sp,                             //目標速度
    float eas2tas,                          //註釋給出的基於當前的測量來計算實際值，給的預設值為1.0
    float throttle_min, float throttle_max, //油門範圍
    float throttle_cruise,                  //巡航油門
    bool  climboot_mode,                    //是否是爬升模式
    float climbout_pitch_min_rad,           //爬升最小仰角
    float altitude,                         //當前高度
    const math::Vector<3> &ground_speed,    //當前速度
    unsigned mode = tecs_status_s::TECS_MODE_NORMAL,
    bool pitch_max_special=false
    );

1. 計算距離上次執行tecs（_last_tecs_update）的時間dt，dt最小0.01，更新_last_tecs_update
2. 只在滯空時候執行tecs模組，當飛行器為VTOL時，不在旋翼模式/變形模式下執行，（空速太小）通過更新run_tecs這個函式內變數
3. 如果是VTOL模式的變形模式：
   是：設定屬性（_was_in_transition）為true，設定變形後的速度屬性（_asp_after_transition）為控制的空速
   否：
   如果_was_in_transition為true，
   是：說明已經從VTOL模式改變，期望速度(_asp_after_transition)增加（2m/s），設定v_sp，如果速度大於v_sp時候，認為變形完成重置_was_in_transition和_asp_after_transition
4. 更新屬性_is_tecs_runing=run_tecs，
   如果run_tecs為false，設定屬性_reinitialize_tecs為true，表示下次執行TECS需要初始化。
   根據_reinitialize_tecs的值決定是否初始化_tecs
5. 判斷_mTecs是否可用
   是：
   設定flightPathAngle角度
   建立limitOverride物件，
   如果飛行器引擎故障：限制俯仰-1~5
   如果是爬升模式：限制最小的爬升角度
   否則：不使用
   如果pitch_max_special，設定最大的俯仰角度
   否則：不設定
   呼叫_mTecs.updateAltitudeSpeed(),傳入flightPAthAngle,高度，設定高度，當前空速，目標空速，模式，限制
   否：
   如果飛行器引擎故障：限制俯仰-1~5
   除了著陸模式外，其他需要設定低速保護_tecs
   如果 是尾起的飛行器，我們需要在旋翼和固定翼模式中切換
   使用_tecs.update_pitch_throttle()更新油門俯仰，使用get_tecs_state取出得到的控制輸出，再把對應資訊賦給tecs_status_s結構體的例項，通過uORB以ORB_ID(tecs_status)輸出