一、開篇
? ? ? ? 姿態(tài)控制篇終于來了、來了、來了~~~
? ? ? ? 心情爽不爽？愉悅不愉悅？開心不開心？
? ? ? ? 喜歡的話就請我吃頓飯吧，哈哈。
? ? ? ? 其實(shí)這篇blog一周前就應(yīng)該寫的，可惜被上一篇blog霸占了。但是也不算晚，整理了很多算法基礎(chǔ)知識(shí)，使得本篇blog更充實(shí)。一人之力總是有限的，難免有不足之處，大家見諒，有寫的不好的地方勞煩指正。看到標(biāo)題了吧，屬于連載篇，所以后續(xù)還會(huì)有相關(guān)問題的補(bǔ)充的。
三、實(shí)驗(yàn)平臺(tái)
Software?Version：PX4Firmware
Hardware Version：pixhawk
IDE：eclipse Juno （Windows）
四、基礎(chǔ)知識(shí)
1、寫在前面 ? ?
? ? ? ? 無人機(jī)控制部分主要分為兩個(gè)部分，姿態(tài)控制部分和位置控制部分；位置控制可用遠(yuǎn)程遙控控制，而姿態(tài)控制一般由無人機(jī)系統(tǒng)自動(dòng)完成。姿態(tài)控制是非常重要的，因?yàn)闊o人機(jī)的位置變化都是由姿態(tài)變化引起的。
? ? ? ? 下圖闡釋了PX4源碼中的兩個(gè)環(huán)路控制，分為姿態(tài)控制和位置控制。
? ? ? ? 補(bǔ)充：關(guān)于Pixhawk原生固件中姿態(tài)(估計(jì)/控制)和位置(估計(jì)/控制)源碼的應(yīng)用問題
PX4Fireware原生固件中的modules中姿態(tài)估計(jì)有多種：Attitude_estimator_ekf、Attitude_estimator_q、ekf_att_pos_estimator。
位置估計(jì)有：ekf_att_pos_estimator、local_position_estimator、position_estimator_inav
姿態(tài)控制有：fw_att_control、mc_att_control、mc_att_control_multiplatform、vtol_att_control
位置控制有：fw_pos_control_l1、fw_pos_control_l1、mc_pos_control_multiplatform
? ? ? ? 四旋翼用到以上哪些估計(jì)和控制算法呢？這部分在啟動(dòng)代碼rc.mc_app 里面有詳細(xì)的說明。
? ? 默認(rèn)的是： ? ? ? ? 姿態(tài)估計(jì) Attitude_estimator_q ? ? ? ? 位置估計(jì) position_estimator_inav ? ? ? ? 姿態(tài)控制 mc_att_control ? ? ? ? 位置控制 mc_pos_control
2、 飛行控制（該部分屬于理論概述）
? ? ? ? 飛行控制分為姿態(tài)控制和位置控制，該文章主講姿態(tài)控制。
? ? ? ? 所謂姿態(tài)控制，主要就是在前期姿態(tài)解算的基礎(chǔ)上對四旋翼飛行器進(jìn)行有效的飛行控制，以達(dá)到所需要的控制效果。在這種情況下，算法要學(xué)會(huì)如何連續(xù)地做決策，并且算法的評價(jià)應(yīng)該根據(jù)其所做選擇的長期質(zhì)量來進(jìn)行。舉一個(gè)具體的例子，想想無人機(jī)飛行所面臨的難題：每不到一秒，算法都必須反復(fù)地選擇最佳的行動(dòng)控制。控制過程還是以經(jīng)典的PID反饋控制器為主（在控制環(huán)路中可以添加smith預(yù)測器）。那么如何實(shí)現(xiàn)控制呢?
? ? ? ? 以四旋翼飛行器為例，主要就是通過改變旋翼的角速度來控制四旋翼無人機(jī)。每個(gè)旋翼產(chǎn)生一個(gè)推力（F1、F2、F3、F4）和一個(gè)力矩，其共同作用構(gòu)成四旋翼無人機(jī)的主推力、偏航力矩、俯仰力矩和滾轉(zhuǎn)力矩。在四旋翼無人機(jī)中，正對的一對旋翼旋轉(zhuǎn)方向一致，另外一對與之相反，來抵消靜態(tài)平穩(wěn)飛行時(shí)的回轉(zhuǎn)效應(yīng)和氣動(dòng)力矩。升降以及RPY的實(shí)現(xiàn)不在贅述。控制對象就是四旋翼無人機(jī)，其動(dòng)力學(xué)模型可以描述為：將其視為有一個(gè)力和三個(gè)力矩的三維剛體。如下給出了小角度變化條件下的四旋翼無人機(jī)的近似動(dòng)力學(xué)模型：
? ? ? ? PS：PX4的姿態(tài)控制部分使用的是roll-pitch和yaw分開控制的（是為了解耦控制行為），即tilt和torsion兩個(gè)環(huán)節(jié)。感性認(rèn)識(shí)一下，如下圖：
? ? ? ? 根據(jù)經(jīng)驗(yàn)所得，控制toll-pitch比控制yaw更容易實(shí)現(xiàn)。比如同樣是實(shí)現(xiàn)10°的變化，roll-pitch需要60ms左右；但是yaw控制器卻需要接近150ms。（上面兩幅圖是出自DJI某哥寫的論文里面，僅作參考，結(jié)合理解Pixhawk ）
? ? 控制流程：
? ? ? ? 1）、預(yù)處理：各參數(shù)的初始化。
? ? ? ? 2）、穩(wěn)定roll-pitch的角速度。
? ? ? ? 3）、穩(wěn)定roll-pitch的角度。
? ? ? ? 4）、穩(wěn)定yaw的角速度。
? ? ? ? 5）、穩(wěn)定yaw的角度。
? ? ? ? 其中在第五步中有一個(gè)yaw的前饋控制（MC_YAW_FF ）：There is MC_YAW_FF parameter that controls how much of userinput need to feed forward to yaw rate controller. 0 means very slow control,controller will start to move yaw only when sees yaw position error, 1 meansvery fast responsive control, but with some overshot, controller will move yawimmediately, always keeping yaw error near zero 。 This parameter is not critical and can be tuned in flight, inworst case yaw responce will be sluggish or too fast. Play with FF parameter toget comfortable responce. Valid range is 0…1. Typical value is 0.8…0.9. (Foraerial video optimal value may be much smaller to get smooth responce.) Yawovershot should be not more than 2-5% 。
? ? ? ? 摘自：https://pixhawk.org/users/multirotor_pid_tuning
3、 進(jìn)入姿態(tài)控制源碼的前期過程
? ? ? ? 首先感性認(rèn)識(shí)一下姿態(tài)控制部分的框架，控制部分分為內(nèi)外環(huán)控制，內(nèi)環(huán)控制角速度、外環(huán)控制角度。控制過程是先根據(jù)目標(biāo)姿態(tài)（target）和當(dāng)前姿態(tài)（current）求出偏差角，然后通過角速度來修正這個(gè)偏差角，最終到達(dá)目標(biāo)姿態(tài)。
? ? ? ? 和姿態(tài)解算算法的流程幾乎類似，主要的代碼流程首先就是按照C++語言的格式引用C語言的main函數(shù)，但是在該處變成了：
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])。
? ? ? ? 然后捏：跳轉(zhuǎn)到所謂的main函數(shù)，該部分有個(gè)要注意的點(diǎn)，如下代碼所示：即mc_att_control::g_control = new MulticopterAttitudeControl;//重點(diǎn)（934），new關(guān)鍵詞應(yīng)該不陌生吧，類似于C語言中的malloc，對變量進(jìn)行內(nèi)存分配的，即對姿態(tài)控制過程中使用到的變量賦初值。
[plain] view plain
copy int?mc_att_control_main(int?argc,?char?*argv[])?? {?? ????if?(argc?<?2)?{?? ????????warnx("usage:?mc_att_control?{start|stop|status}");?? ????????return?1;?? ????}?? ????if?(!strcmp(argv[1],?"start"))?{?? ????????if?(mc_att_control::g_control?!=?nullptr)?{?? ????????????warnx("already?running");?? ????????????return?1;?? ????????}?? ????????mc_att_control::g_control?=?new?MulticopterAttitudeControl;//重點(diǎn)?? ????????if?(mc_att_control::g_control?==?nullptr)?{?? ????????????warnx("alloc?failed");?? ????????????return?1;?? ????????}?? ????????if?(OK?!=?mc_att_control::g_control->start())?{//跳轉(zhuǎn)?? ????????????delete?mc_att_control::g_control;?? ????????????mc_att_control::g_control?=?nullptr;?? ????????????warnx("start?failed");?? ????????????return?1;?? ????????}?? ????????return?0;?? ????}?? ? ? ? ? 然后捏：start函數(shù)
[plain] view plain
copy Int?MulticopterAttitudeControl::start()?? {?? ????ASSERT(_control_task?==?-1);?? ????/*?start?the?task?*/?? ????_control_task?=?px4_task_spawn_cmd("mc_att_control",?? ???????????????????????SCHED_DEFAULT,?? ???????????????????????SCHED_PRIORITY_MAX?-?5,?? ???????????????????????1500,?? (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,?? ???????????????????????nullptr);?? ????if?(_control_task?<?0)?{?? ????????warn("task?start?failed");?? ????????return?-errno;?? ????}?? ????return?OK;?? }?? ? ? ? ? 其中上面有個(gè)封裝了nuttx自帶的生成task的任務(wù)創(chuàng)建函數(shù)（他把優(yōu)先級什么的做了重新的define，這么做是便于代碼閱讀）：px4_task_spawn_cmd()，注意它的用法。其函數(shù)原型是：
[plain] view plain
copy px4_task_t?px4_task_spawn_cmd(const?char?*name,?int?scheduler,?int?priority,?int?stack_size,?px4_main_t?entry,?? ??????????????????char?*const?argv[])?? ? ? ? ? 第一個(gè)參數(shù)是namespace，第二個(gè)參數(shù)是選擇調(diào)度策略，第三個(gè)是任務(wù)優(yōu)先級，第四個(gè)是任務(wù)的棧空間大小，第五個(gè)是任務(wù)的入口函數(shù)，最后一個(gè)一般是null。
? ? ? ? 然后捏：
[html] view plain
copy Void??MulticopterAttitudeControl::task_main_trampoline(int?argc,?char?*argv[])?? {?? ????mc_att_control::g_control-> task_main();?? }?? ? ? ? ? 再然后捏：終于到本體了。
[plain] view plain
copy Void?MulticopterAttitudeControl::task_main(){}?? ? ? ? ? 比較討厭的就是為什么要封裝那么多層，應(yīng)該是水平不夠，還沒有理解此處的用意。下面就是重點(diǎn)了。
五、重點(diǎn)
1、姿態(tài)控制源碼_訂閱 ? ? ? ? 姿態(tài)控制的代碼比姿態(tài)解算的代碼少了不少，所以接下來分析應(yīng)該會(huì)比較快。
? ? ? ? 首先還是需要通過IPC模型uORB進(jìn)行訂閱所需要的數(shù)據(jù)。需要注意的一個(gè)細(xì)節(jié)就是在該算法處理過程中的有效數(shù)據(jù)的用途問題，最后處理過的數(shù)據(jù)最后又被改進(jìn)程自己訂閱了，然后再處理，再訂閱，一直處于循環(huán)狀態(tài)，這就是所謂的PID反饋控制器吧，最終達(dá)到所需求的控制效果，達(dá)到控制效果以后就把一系列的控制量置0（類似于idle），該任務(wù)一直在運(yùn)行，隨啟動(dòng)腳本啟動(dòng)的。
[plain] view plain
copy /*?*?do?subscriptions?*/?? ????_v_att_sp_sub?=?orb_subscribe(ORB_ID(vehicle_attitude_setpoint));?? ????_v_rates_sp_sub?=?orb_subscribe(ORB_ID(vehicle_rates_setpoint));?? ????_ctrl_state_sub?=?orb_subscribe(ORB_ID(control_state));?? ????_v_control_mode_sub?=?orb_subscribe(ORB_ID(vehicle_control_mode));?? ????_params_sub?=?orb_subscribe(ORB_ID(parameter_update));?? ????_manual_control_sp_sub?=?orb_subscribe(ORB_ID(manual_control_setpoint));?? ????_armed_sub?=?orb_subscribe(ORB_ID(actuator_armed));?? ????_vehicle_status_sub?=?orb_subscribe(ORB_ID(vehicle_status));?? ????_motor_limits_sub?=?orb_subscribe(ORB_ID(multirotor_motor_limits));?? ? ? ? ? 上面這些訂閱到底訂閱了哪些東西呢，顧名思義，根據(jù)ORB()中的參數(shù)的名稱就是知道訂閱的到底用于做什么的了。這套開源代碼中最優(yōu)越的地方時(shí)變量的命名很好，通俗易懂。
2、 參數(shù)初始化
? ? ? ? 緊隨上面的代碼就是參數(shù)數(shù)據(jù)的獲取，parameters主要就是我們前期定義的感興趣的數(shù)據(jù)，在姿態(tài)控制中的這些數(shù)據(jù)都是私有數(shù)據(jù)（private），比如roll、pitch、yaw以及與它們對應(yīng)的PID參數(shù)。注意區(qū)分_params_handles和_params這兩種數(shù)據(jù)結(jié)構(gòu)（struct類型）。
[plain] view plain
copy ?/*?initialize?parameters?cache?*/?? ????parameters_update();?? 函數(shù)原型欣賞：?? int?MulticopterAttitudeControl::parameters_update()?? {?? ????float?v;?? ????/*?roll?gains?*/?? ????param_get(_params_handles.roll_p,?&v);?? ????_params.att_p(0)?=?v;?? ????param_get(_params_handles.roll_rate_p,?&v);?? ????_params.rate_p(0)?=?v;?? ????param_get(_params_handles.roll_rate_i,?&v);?? ????_params.rate_i(0)?=?v;?? ????param_get(_params_handles.roll_rate_d,?&v);?? ????_params.rate_d(0)?=?v;?? ????param_get(_params_handles.roll_rate_ff,?&v);?? ????_params.rate_ff(0)?=?v;?? ????/*?pitch?gains?*/?? ?????省略?? ????/*?yaw?gains?*/?? ?????省略?? ????/*?angular?rate?limits?*/?? ????param_get(_params_handles.roll_rate_max,?&_params.roll_rate_max);?? ????_params.mc_rate_max(0)?=?math::radians(_params.roll_rate_max);?? ????param_get(_params_handles.pitch_rate_max,?&_params.pitch_rate_max);?? ????_params.mc_rate_max(1)?=?math::radians(_params.pitch_rate_max);?? ????param_get(_params_handles.yaw_rate_max,?&_params.yaw_rate_max);?? ????_params.mc_rate_max(2)?=?math::radians(_params.yaw_rate_max);?? ????/*?manual?rate?control?scale?and?auto?mode?roll/pitch?rate?limits?*/?? ????param_get(_params_handles.acro_roll_max,?&v);?? ????_params.acro_rate_max(0)?=?math::radians(v);?? ????param_get(_params_handles.acro_pitch_max,?&v);?? ????_params.acro_rate_max(1)?=?math::radians(v);?? ????param_get(_params_handles.acro_yaw_max,?&v);?? ????_params.acro_rate_max(2)?=?math::radians(v);?? ????/*?stick?deflection?needed?in?rattitude?mode?to?control?rates?not?angles?*/?? ????param_get(_params_handles.rattitude_thres,?&_params.rattitude_thres);?? ????_actuators_0_circuit_breaker_enabled?=?circuit_breaker_enabled("CBRK_RATE_CTRL",?CBRK_RATE_CTRL_KEY);?? ????return?OK;?? }?? ? ? ? ? 重點(diǎn)分析一下上述代碼：其中param_get()函數(shù)比較重要，特別是內(nèi)部使用的lock和unlock的使用（主要就是通過sem信號(hào)量控制對某一數(shù)據(jù)的互斥訪問）。
[plain] view plain
copy Int?param_get(param_t?param,?void?*val)?? {?? ????int?result?=?-1;?? ????param_lock();?? ????const?void?*v?=?param_get_value_ptr(param);?? ????if?(val?!=?NULL)?{?? ????????memcpy(val,?v,?param_size(param));?? ????????result?=?0;?? ????}?? ????param_unlock();?? ????return?result;?? }?? ? ? ? ? 上述使用的*lock和*unlock通過sem實(shí)現(xiàn)互斥訪問（進(jìn)臨界區(qū)），源碼如下。
[plain] view plain
copy /**?lock?the?parameter?store?*/?? static?void?param_lock(void)?? {?? ????//do?{}?while?(px4_sem_wait(?m_sem)?!=?0);?? }?? /**?unlock?the?parameter?store?*/?? static?void?param_unlock(void)?? {?? ????//px4_sem_post(?m_sem);?? }?? ? ? ? ? 上面是開源代碼中的，代碼里面把lock和unlock函數(shù)都寫成空函數(shù)了，那還有屁用啊。應(yīng)該是由于程序開發(fā)和版本控制不是一個(gè)人，有的程序開發(fā)到一半人走了，搞版本控制的，又找不到新的人來進(jìn)行開發(fā)，擱置了忘記修改回來了吧；再或者別的什么意圖。
? ? ? ? 經(jīng)過上述分析，該parameters_update()函數(shù)主要就是獲取roll、pitch、yaw的PID參數(shù)的。并對三種飛行模式（stablize、auto、acro）下的最大姿態(tài)速度做了限制。
3、NuttX任務(wù)使能
[plain] view plain
copy /*?wakeup?source:?vehicle?attitude?*/?? px4_pollfd_struct_t?fds[1];?? fds[0].fd?=?_ctrl_state_sub;?? fds[0].events?=?POLLIN;?? ? ? ? ? 注意上面的fd的賦值。隨后進(jìn)入任務(wù)的循環(huán)函數(shù)：while
4、阻塞等待數(shù)據(jù) [plain] view plain
copy /*?wait?for?up?to?100ms?for?data?*/?? ????int?pret?=?px4_poll(&fds[0],?(sizeof(fds)?/?sizeof(fds[0])),?100);?? ????/*?timed?out?-?periodic?check?for?_task_should_exit?*/?? ????if?(pret?==?0)?{?? ????????continue;?? ????}?? ????/*?this?is?undesirable?but?not?much?we?can?do?-?might?want?to?flag?unhappy?status?*/?? ????if?(pret?<?0)?{?? ????????warn("mc?att?ctrl:?poll?error?%d,?%d",?pret,?errno);?? ????????/*?sleep?a?bit?before?next?try?*/?? ????????usleep(100000);?? ????????continue;?? ????}?? ????perf_begin(_loop_perf);?? ? ? ? ? 首先是px4_poll()配置阻塞時(shí)間100ms（uORB模型的函數(shù)API）。然后是打開MAVLINK協(xié)議，記錄數(shù)據(jù)。如果poll失敗，直接使用關(guān)鍵詞continue從頭開始運(yùn)行（注意while和continue的組合使用）。其中的usleep(10000)函數(shù)屬于線程級睡眠函數(shù)，使當(dāng)前線程掛起。原文解釋為：
? ? ? ? “ Theusleep() function will cause the calling thread to be suspended from executionuntil either the number of real-time microseconds specified by the argument'usec' has elapsed or a signal is delivered to the calling thread。 ”
??? 上面最后一個(gè)perf_begin(_loop_perf)，是一個(gè)空函數(shù)，帶perf開頭的都是空函數(shù)，它的作用主要是“Empty function calls forroscompatibility”。
5、重點(diǎn)來了（獲取當(dāng)前姿態(tài)Current） ? ? ? ? 終于到了姿態(tài)控制器了，興奮不？別只顧著興奮了，好好理解一下。尤其是下面的幾個(gè)*poll函數(shù)，特別重要，后期算法中的很多數(shù)據(jù)都是通過這個(gè)幾個(gè)*poll()函數(shù)獲取的，也是uORB模型，不理解這個(gè)后去會(huì)很暈的，別說沒提醒啊；代碼中沒有一點(diǎn)冗余的部分，每一個(gè)函數(shù)、每一行都是其意義所在。
[plain] view plain
copy /*?run?controller?on?attitude?changes?*/?? ????if?(fds[0].revents?&?POLLIN)?{?? ????????static?uint64_t?last_run?=?0;?? ????????float?dt?=?(hrt_absolute_time()?-?last_run)?/?1000000.0f;?? ????????last_run?=?hrt_absolute_time();?? ????????/*?guard?against?too?small?(<2ms)?and?too?large?(>20ms)?dt's?*/?? ????????if?(dt?<?0.002f)?{?? ????????????dt?=?0.002f;?? ????????}?else?if?(dt?>?0.02f)?{?? ????????????dt?=?0.02f;?? ????????}?? ????????/*?copy?attitude?and?control?state?topics?*///獲取當(dāng)前姿態(tài)數(shù)據(jù)?? ????????orb_copy(ORB_ID(control_state),?_ctrl_state_sub,?&_ctrl_state);?? ????????/*?check?for?updates?in?other?topics?*/?? ????????parameter_update_poll();?? ????????vehicle_control_mode_poll();?? ????????arming_status_poll();?? ????????vehicle_manual_poll();?? ????????vehicle_status_poll();?? ????????vehicle_motor_limits_poll();?? ? ? ? ? ?注意上面的revents，要與events區(qū)分開來，兩者的區(qū)別如下：
??? pollevent_t events;??/* The input event flags */
??? pollevent_t revents;?/* The output event flags */
? ? ? ? 首先就是判斷姿態(tài)控制器的控制任務(wù)是否已經(jīng)使能，然后就是檢測通過hrt獲取時(shí)間精度的所需時(shí)間，并且約束在2ms至20ms以內(nèi)。完了，orb_copy()函數(shù)怎么用的忘記了。。。。
[plain] view plain
copy /**?? ?*?Fetch?data?from?a?topic.?? *?This?is?the?only?operation?that?will?reset?the?internal?marker?that?? ?*?indicates?that?a?topic?has?been?updated?for?a?subscriber.?Once?poll?? ?*?or?check?return?indicating?that?an?updaet?is?available,?this?call?? ?*?must?be?used?to?update?the?subscription.?? *?@param?meta????The?uORB?metadata?(usually?from?the?ORB_ID()?macro)?? ?*??????for?the?topic.?? ?*?@param?handle??A?handle?returned?from?orb_subscribe.?? ?*?@param?buffer??Pointer?to?the?buffer?receiving?the?data,?or?NULL?? ?*??????if?the?caller?wants?to?clear?the?updated?flag?without?? ?*??????using?the?data.?? ?*?@return????OK?on?success,?ERROR?otherwise?with?errno?set?accordingly.?? ?*/?? int??orb_copy(const?struct?orb_metadata?*meta,?int?handle,?void?*buffer)?? {?? ????return?uORB::Manager::get_instance()->orb_copy(meta,?handle,?buffer);?? }?? ? ? ? ? 第三個(gè)參數(shù)就是為了保存通過orb_subscribe()函數(shù)訂閱獲得的有效數(shù)據(jù)，該部分獲取的是_ctrl_state，即控制姿態(tài)的數(shù)據(jù)，數(shù)據(jù)結(jié)構(gòu)如下：（包含三軸加速度、三軸速度、三軸位置、空速、四元數(shù)、roll/pitch/yaw的速率）。記住這個(gè) copy 的內(nèi)容，后面會(huì)用到多次。
? ? ? ? 然后就是檢測數(shù)據(jù)是否已經(jīng)更新，舉一例說明問題。
[plain] view plain
copy /*?check?for?updates?in?other?topics?*/?? parameter_update_poll();?? vehicle_status_poll();//注意這個(gè)，后面會(huì)用到內(nèi)部的數(shù)據(jù)處理結(jié)果，即發(fā)布和訂閱的ID問題。?? ? ? ? ? 函數(shù)原型：
[plain] view plain
copy Void?MulticopterAttitudeControl::parameter_update_poll()?? {?? ????bool?updated;?? ????/*?Check?if?parameters?have?changed?*/?? ????orb_check(_params_sub,?&updated);?? ????if?(updated)?{?? ????????struct?parameter_update_s?param_update;?? ????????orb_copy(ORB_ID(parameter_update),?_params_sub,??m_update);?? ????????parameters_update();?? ????}?? }?? ? ? ? ? 然后捏：飛行模式判斷是否是MAIN_STATE_RATTITUD模式，該模式是一種新的飛行模式，只控制角速度，不控制角度，俗稱半自穩(wěn)模式（ 小舵量自穩(wěn) 大舵量手動(dòng) ），主要用在setpoint中，航點(diǎn)飛行。根據(jù)介紹，這個(gè)模式只有在pitch和roll都設(shè)置為Rattitude模式時(shí)才有意義，如果yaw也設(shè)置了該模式，那么就會(huì)自動(dòng)被手動(dòng)模式替代了。所以代碼中只做了x、y閾值的檢測。官方介紹：
RATTITUDE?The pilot's inputs are passed as roll, pitch, and yaw?rate?commands to the autopilot if they are greater than the mode's threshold. If not the inputs are passed as roll and pitch?angle?commands and a yaw?rate?command. Throttle is passed directly to the output mixer. [plain] view plain
copy /*?Check?if?we?are?in?rattitude（新的飛行模式，角速度模式，沒有角度控制）?mode?and?the?pilot?is?above?the?threshold?on?pitch?or?roll?(yaw?can?rotate?360?in?normal?att?control).??If?both?are?true?don't??even?bother?running?the?attitude?controllers?*/?? if(_vehicle_status.main_state?==?vehicle_status_s::MAIN_STATE_RATTITUDE){?? ????????if?(fabsf(_manual_control_sp.y)?>?_params.rattitude_thres?||?? ????????fabsf(_manual_control_sp.x)?>?_params.rattitude_thres){?? ????????_v_control_mode.flag_control_attitude_enabled?=?false;?? ????????????}?? ????????}?? 6、姿態(tài)控制（這才是重點(diǎn)） [plain] view plain
copy if?(_v_control_mode.flag_control_attitude_enabled)?? {?? ????????????control_attitude(dt);?? ????????????????/*?publish?attitude?rates?setpoint?*/?? ????????????????_v_rates_sp.roll?=?_rates_sp(0);?? ????????????????_v_rates_sp.pitch?=?_rates_sp(1);?? ????????????????_v_rates_sp.yaw?=?_rates_sp(2);?? ????????????????_v_rates_sp.thrust?=?_thrust_sp;?? ????????????????_v_rates_sp.timestamp?=?hrt_absolute_time();?? ????????????????if?(_v_rates_sp_pub?!=?nullptr)?{?? ????????????????????orb_publish(_rates_sp_id,?_v_rates_sp_pub,?&_v_rates_sp);?? ?? ????????????????}?else?if?(_rates_sp_id)?{?? ????????????????????_v_rates_sp_pub?=?orb_advertise(_rates_sp_id,?&_v_rates_sp);?? ????????????????}?? ????????????//}?? ????????}?else?{?? ????????????/*?attitude?controller?disabled,?poll?rates?setpoint?topic?*/?? ????????????if?(_v_control_mode.flag_control_manual_enabled)?{?? ????????????????/*?manual?rates?control?-?ACRO?mode?*/?? ????????????????_rates_sp?=?math::Vector<3>(_manual_control_sp.y,?-_manual_control_sp.x,?_manual_control_sp.r).emult(_params.acro_rate_max);?? ????????????????_thrust_sp?=?math::min(_manual_control_sp.z,?MANUAL_THROTTLE_MAX_MULTICOPTER);?? ?? ????????????????/*?publish?attitude?rates?setpoint?*/?? ????????????????_v_rates_sp.roll?=?_rates_sp(0);?? ????????????????_v_rates_sp.pitch?=?_rates_sp(1);?? ????????????????_v_rates_sp.yaw?=?_rates_sp(2);?? ????????????????_v_rates_sp.thrust?=?_thrust_sp;?? ????????????????_v_rates_sp.timestamp?=?hrt_absolute_time();?? ????????????????if?(_v_rates_sp_pub?!=?nullptr)?{?? ????????????????????orb_publish(_rates_sp_id,?_v_rates_sp_pub,?&_v_rates_sp);?? ????????????????}?else?if?(_rates_sp_id)?{?? ????????????????????_v_rates_sp_pub?=?orb_advertise(_rates_sp_id,?&_v_rates_sp);?? ????????????????}?? ????????????}?else?{?? ????????????????/*?attitude?controller?disabled,?poll?rates?setpoint?topic?*/?? ????????????????vehicle_rates_setpoint_poll();?? ????????????????_rates_sp(0)?=?_v_rates_sp.roll;?? ????????????????_rates_sp(1)?=?_v_rates_sp.pitch;?? ????????????????_rates_sp(2)?=?_v_rates_sp.yaw;?? ????????????????_thrust_sp?=?_v_rates_sp.thrust;?? ????????????}?? ????????}?? ? ? ? ? 上面的代碼中，初始就是control_attitude(dt)，控制數(shù)據(jù)都是由它來獲取的。該函數(shù)內(nèi)部做了很多的處理，控制理論基本都是在這個(gè)里面體現(xiàn)的，所以需要深入研究理解它才可以進(jìn)一步的研究后續(xù)的算法。它的內(nèi)部會(huì)通過算法處理獲得控制量（目標(biāo)姿態(tài)Target），即_rates_sp，一個(gè)vector<3>變量，以便后續(xù)控制使用。好了，進(jìn)入正題。
? ? ? ? 首先是姿態(tài)控制(control_attitude)，然后是速度控制(control_attitude_rates)，一個(gè)個(gè)來。
6.1、control_attitude() 函數(shù) (角度控制環(huán) ) ? ? ? ? 獲取目標(biāo)姿態(tài) Target
[plain] view plain
copy /**?? ?*?Attitude?controller.?? ?*?Input:?'vehicle_attitude_setpoint'?topics?(depending?on?mode)?? ?*?Output:?'_rates_sp'?vector,?'_thrust_sp'?? ?*/?? Void?MulticopterAttitudeControl::control_attitude(float?dt)?? {?? ????vehicle_attitude_setpoint_poll();?? ????_thrust_sp?=?_v_att_sp.thrust;?? ????/*?construct?attitude?setpoint?rotation?matrix?*/?? ????math::Matrix<3,?3>?R_sp;?? ????R_sp.set(_v_att_sp.R_body);?? ????/*?get?current?rotation?matrix?from?control?state?quaternions?*/?? ????math::Quaternion?q_att(_ctrl_state.q[0],?_ctrl_state.q[1],?_ctrl_state.q[2],?_ctrl_state.q[3]);?? ????math::Matrix<3,?3>?R?=?q_att.to_dcm();?? ????/*?all?input?data?is?ready,?run?controller?itself?*/?? ????/*?try?to?move?thrust?vector?shortest?way,?because?yaw?response?is?slower?than?roll/pitch?約兩倍*/??? ????math::Vector<3>?R_z(R(0,?2),?R(1,?2),?R(2,?2));?? ????math::Vector<3>?R_sp_z(R_sp(0,?2),?R_sp(1,?2),?R_sp(2,?2));?? ????/*?axis?and?sin(angle)?of?desired?rotation?*/?? ????math::Vector<3>?e_R?=?R.transposed()?*?(R_z?%?R_sp_z);?? ????/*?calculate?angle?error?*/?? ????float?e_R_z_sin?=?e_R.length();?? ????float?e_R_z_cos?=?R_z?*?R_sp_z;?? ????/*?calculate?weight?for?yaw?control?*/?? ????float?yaw_w?=?R_sp(2,?2)?*?R_sp(2,?2);?? ????/*?calculate?rotation?matrix?after?roll/pitch?only?rotation?*/?? ????math::Matrix<3,?3>?R_rp;?? ????if?(e_R_z_sin?>?0.0f)?{?? ????????/*?get?axis-angle?representation?*/?? ????????float?e_R_z_angle?=?atan2f(e_R_z_sin,?e_R_z_cos);?? ????????math::Vector<3>?e_R_z_axis?=?e_R?/?e_R_z_sin;?? ????????e_R?=?e_R_z_axis?*?e_R_z_angle;?? ????????/*?cross?product?matrix?for?e_R_axis?*/?? ????????math::Matrix<3,?3>?e_R_cp;?? ????????e_R_cp.zero();?? ????????e_R_cp(0,?1)?=?-e_R_z_axis(2);?? ????????e_R_cp(0,?2)?=?e_R_z_axis(1);?? ????????e_R_cp(1,?0)?=?e_R_z_axis(2);?? ????????e_R_cp(1,?2)?=?-e_R_z_axis(0);?? ????????e_R_cp(2,?0)?=?-e_R_z_axis(1);?? ????????e_R_cp(2,?1)?=?e_R_z_axis(0);?? ????????/*?rotation?matrix?for?roll/pitch?only?rotation?*/?? ????????R_rp?=?R?*?(_I?+?e_R_cp?*?e_R_z_sin?+?e_R_cp?*?e_R_cp?*?(1.0f?-?e_R_z_cos));?? ????}?else?{?? ????????/*?zero?roll/pitch?rotation?*/?? ????????R_rp?=?R;?? ????}?? ????/*?R_rp?and?R_sp?has?the?same?Z?axis,?calculate?yaw?error?*/?? ????math::Vector<3>?R_sp_x(R_sp(0,?0),?R_sp(1,?0),?R_sp(2,?0));?? ????math::Vector<3>?R_rp_x(R_rp(0,?0),?R_rp(1,?0),?R_rp(2,?0));?? ????e_R(2)?=?atan2f((R_rp_x?%?R_sp_x)?*?R_sp_z,?R_rp_x?*?R_sp_x)?*?yaw_w;?? ????if?(e_R_z_cos?<?0.0f)?{?? ????????/*?for?large?thrust?vector?rotations?use?another?rotation?method:?? ?????????*?calculate?angle?and?axis?for?R?->?R_sp?rotation?directly?*/?? ????????math::Quaternion?q;?? ????????q.from_dcm(R.transposed()?*?R_sp);?? ????????math::Vector<3>?e_R_d?=?q.imag();?? ????????e_R_d.normalize();?? ????????e_R_d?*=?2.0f?*?atan2f(e_R_d.length(),?q(0));?? ????????/*?use?fusion?of?Z?axis?based?rotation?and?direct?rotation?*/?? ????????float?direct_w?=?e_R_z_cos?*?e_R_z_cos?*?yaw_w;?? ????????e_R?=?e_R?*?(1.0f?-?direct_w)?+?e_R_d?*?direct_w;?? ????}?? ????/*?calculate?angular?rates?setpoint?*/?? ????_rates_sp?=?_params.att_p.emult(e_R);?? ????/*?limit?rates?*/?? ????for?(int?i?=?0;?i?<?3;?i++)?{?? ????????_rates_sp(i)?=?math::constrain(_rates_sp(i),?-_params.mc_rate_max(i),?_params.mc_rate_max(i));?? ????}?? ????/*?feed?forward?yaw?setpoint?rate?*/?? ????_rates_sp(2)?+=?_v_att_sp.yaw_sp_move_rate?*?yaw_w?*?_params.yaw_ff;?? }?? ? ? ? ? ?詳細(xì)分析：首先就是通過uORB模型檢測姿態(tài)數(shù)據(jù)是否已經(jīng)更新。檢測到更新數(shù)據(jù)以后，把數(shù)據(jù)拷貝到當(dāng)前，并通過_thrust_sp = _v_att_sp.thrust把油門控制量賦值給控制變量。
? ? ? ? 然后捏：構(gòu)建姿態(tài)旋轉(zhuǎn)矩陣（目標(biāo)狀態(tài)，所謂的TargetRotation）。
[plain] view plain
copy ????/*?construct?attitude?setpoint?rotation?matrix?*/?? ????math::Matrix<3,3>?R_sp;?? R_sp.set(_v_att_sp.R_body);//不在贅述，在姿態(tài)解算時(shí)使用了同樣的方法?? ? ? ? ? 然后捏：通過控制四元數(shù)獲取當(dāng)前狀態(tài)的旋轉(zhuǎn)矩陣DCM，后面在計(jì)算誤差以后旋轉(zhuǎn)到b系時(shí)使用到了該處的DCM。即由姿態(tài)解算得到的有效姿態(tài)信息。
[plain] view plain
copy /*?get?current?rotation?matrix?from?control?state?quaternions?*/?? ????math::Quaternion?q_att(_ctrl_state.q[0],?_ctrl_state.q[1],?_ctrl_state.q[2],?_ctrl_state.q[3]);?? ????math::Matrix<3,?3>?R?=?q_att.to_dcm();?? ????通過math庫構(gòu)建四元數(shù)；獲取DCM的函數(shù)原型：無可厚非，都懂的?? ????/***?create?rotation?matrix?for?the?quaternion?*/?? ????Matrix<3,?3>?to_dcm(void)?const?{?? ????????Matrix<3,?3>?R;?? ????????float?aSq?=?data[0]?*?data[0];?? ????????float?bSq?=?data[1]?*?data[1];?? ????????float?cSq?=?data[2]?*?data[2];?? ????????float?dSq?=?data[3]?*?data[3];?? ????????R.data[0][0]?=?aSq?+?bSq?-?cSq?-?dSq;?? ????????R.data[0][1]?=?2.0f?*?(data[1]?*?data[2]?-?data[0]?*?data[3]);?? ????????R.data[0][2]?=?2.0f?*?(data[0]?*?data[2]?+?data[1]?*?data[3]);?? ????????R.data[1][0]?=?2.0f?*?(data[1]?*?data[2]?+?data[0]?*?data[3]);?? ????????R.data[1][1]?=?aSq?-?bSq?+?cSq?-?dSq;?? ????????R.data[1][2]?=?2.0f?*?(data[2]?*?data[3]?-?data[0]?*?data[1]);?? ????????R.data[2][0]?=?2.0f?*?(data[1]?*?data[3]?-?data[0]?*?data[2]);?? ????????R.data[2][1]?=?2.0f?*?(data[0]?*?data[1]?+?data[2]?*?data[3]);?? ????????R.data[2][2]?=?aSq?-?bSq?-?cSq?+?dSq;?? ????????return?R;?? ????}?? };?? ? ? ? ? 然后捏：取兩個(gè)矩陣中的Z軸向量，即YAW-axis。
[plain] view plain
copy /*?all?input?data?is?ready,?run?controller?itself?*/?? ????/*?try?to?move?thrust?vector?shortest?way,?because?yaw?response?is?slower?than?roll/pitch?這個(gè)地方應(yīng)該知道旋轉(zhuǎn)按照ZYX來進(jìn)行的*/?? ????math::Vector<3>?R_z(R(0,?2),?R(1,?2),?R(2,?2));?? ????math::Vector<3>?R_sp_z(R_sp(0,?2),?R_sp(1,?2),?R_sp(2,?2));?? ? ? ? ? 然后捏：當(dāng)前姿態(tài)的z軸和目標(biāo)姿態(tài)的z軸的誤差大小（即需要旋轉(zhuǎn)的角度）并旋轉(zhuǎn)到b系（即先對齊Z軸）。
[plain] view plain
copy /*?axis?and?sin(angle)?of?desired?rotation?*/?? ????math::Vector<3>?e_R?=?R.transposed()?*?(R_z?%?R_sp_z);?? ? ? ? ? R_z%R_sp_z叉積，還記得這個(gè)么?在mahony算法中已經(jīng)出現(xiàn)過一次了，就是求取誤差的，本來應(yīng)該z軸相互重合的，如果不是0就作為誤差項(xiàng)。然后再左乘旋轉(zhuǎn)矩陣旋轉(zhuǎn)到b系。
? ? ? ? 轉(zhuǎn)置源碼：
[plain] view plain
copy Matrix3<T>?Matrix3<T>::transposed(void)?const?? {?? ????return?Matrix3<T>(Vector3<T>(a.x,?b.x,?c.x),?? ??????????????????????Vector3<T>(a.y,?b.y,?c.y),?? ??????????????????????Vector3<T>(a.z,?b.z,?c.z));?? }?? ? ? ? ? 然后捏：計(jì)算姿態(tài)角度誤差
[plain] view plain
copy /*?calculate?angle?error?*/?? ????float?e_R_z_sin?=?e_R.length();?? float?e_R_z_cos?=?R_z?*?R_sp_z;?? ? ? ? ? 然后捏：計(jì)算yaw的權(quán)重（不懂，誰幫忙解釋一下原因。。跪謝 ）
[plain] view plain
copy ????/*?calculate?weight?for?yaw?control?*/?? ????float?yaw_w?=?R_sp(2,?2)?*?R_sp(2,?2);//不懂?? 第一行的這個(gè)權(quán)重純粹是因?yàn)槿绻晦D(zhuǎn)動(dòng)roll-pitch的話那應(yīng)該是1，而如果轉(zhuǎn)動(dòng)的話，那個(gè)權(quán)重會(huì)平方倍衰減?（來自MR的解釋）。?? ? ? ? ? 然后捏：因?yàn)槎噍S的yaw響應(yīng)一般比roll/pitch慢了接近一倍，因此將兩者解耦(需要理解解耦的目的)，先補(bǔ)償roll-pitch的變化，計(jì)算R_rp。
[plain] view plain
copy /*?calculate?rotation?matrix?after?roll/pitch?only?rotation?*/?? ????math::Matrix<3,?3>?R_rp;?? ????if?(e_R_z_sin?>?0.0f)?{?? ????????/*?get?axis-angle?representation?*/?? ????????float?e_R_z_angle?=?atan2f(e_R_z_sin,?e_R_z_cos);?? ????????math::Vector<3>?e_R_z_axis?=?e_R?/?e_R_z_sin;?? ????????e_R?=?e_R_z_axis?*?e_R_z_angle;//很大的用途，下面的R_rp求取公式就是利用歐拉角計(jì)算的。?? ????????/*?cross?product?matrix?for?e_R_axis?*/?? ????????math::Matrix<3,?3>?e_R_cp;?? ????????e_R_cp.zero();?? ????????e_R_cp(0,?1)?=?-e_R_z_axis(2);?? ????????e_R_cp(0,?2)?=?e_R_z_axis(1);?? ????????e_R_cp(1,?0)?=?e_R_z_axis(2);?? ????????e_R_cp(1,?2)?=?-e_R_z_axis(0);?? ????????e_R_cp(2,?0)?=?-e_R_z_axis(1);?? ????????e_R_cp(2,?1)?=?e_R_z_axis(0);?? ????????/*?rotation?matrix?for?roll/pitch?only?rotation?*/?? ????????R_rp?=?R?*?(_I?+?e_R_cp?*?e_R_z_sin?+?e_R_cp?*?e_R_cp?*?(1.0f?-?e_R_z_cos));//羅德里格旋轉(zhuǎn)公式：Rodrigues?rotation?formula?? ????}?else?{?? ????????/*?zero?roll/pitch?rotation?*/?? ????????R_rp?=?R;?? ????}?? ? ? ? ? 首先需要明確的就是上述處理過程中的DCM量都是通過歐拉角來表示的，這個(gè)主要就是考慮在控制時(shí)需要明確具體的歐拉角的大小，還有就是算法的解算過程是通過矩陣微分方程推導(dǎo)得到的（參考《慣性技術(shù)_鄧正隆》_P148-P152以及《慣性導(dǎo)航_秦永元》_P342），并且在《慣性技術(shù)_鄧正隆》_P154頁介紹了姿態(tài)矩陣的實(shí)時(shí)解算方法 。再判斷兩個(gè)z軸是否存在誤差（e_R_z_sin> 0.0f），若存在誤差則通過反正切求出該誤差角度值（atan2f(e_R_z_sin,e_R_z_cos)）；然后歸一化e_R_z_axis(e_R /e_R_z_sin該步計(jì)算主要就是利用e_R_z_sin=e_R.length()，往上看就是了，不會(huì)這么快就忘記了吧？！)。然后就是e_R =e_R_z_axis* e_R_z_angle了（主要就是為了誤差向量用角度量表示）。
? ? ? ? 然后捏：計(jì)算yaw的誤差，該誤差是roll_pitch獲取的z軸和目標(biāo)姿態(tài)的z軸的誤差。
[plain] view plain
copy /*?R_rp?and?R_sp?has?the?same?Z?axis,?calculate?yaw?error?*/?? ????math::Vector<3>?R_sp_x(R_sp(0,?0),?R_sp(1,?0),?R_sp(2,?0));?? ????math::Vector<3>?R_rp_x(R_rp(0,?0),?R_rp(1,?0),?R_rp(2,?0));?? ????e_R(2)?=?atan2f((R_rp_x?%?R_sp_x)?*?R_sp_z,?R_rp_x?*?R_sp_x)?*?yaw_w;?? ? ? ? ? 該部分同樣是根據(jù)向量的叉積和點(diǎn)積求出誤差角度的正弦和余弦，再反正切求出角度（又忘記了？回頭看吧）。
? ? ? ? 上面介紹的是在小角度變化時(shí)，如果是大角度變化時(shí)（大于90°，可能性比較小，還是集中在上面的算法吧）使用如何方法處理。
[plain] view plain
copy if?(e_R_z_cos?<?0.0f)?{?? ????????/*?for?large?thrust?vector?rotations?use?another?rotation?method:?? ?????????*?calculate?angle?and?axis?for?R->R_sp?rotation?directly?*/?? ????????math::Quaternion?q;?? ????????q.from_dcm(R.transposed()?*?R_sp);?? ????????math::Vector<3>?e_R_d?=?q.imag();?? ????????e_R_d.normalize();?? ????????e_R_d?*=?2.0f?*?atan2f(e_R_d.length(),?q(0));//不懂?? ????????/*?use?fusion?of?Z?axis?based?rotation?and?direct?rotation?*/?? ????????float?direct_w?=?e_R_z_cos?*?e_R_z_cos?*?yaw_w;?? ????????e_R?=?e_R?*?(1.0f?-?direct_w)?+?e_R_d?*?direct_w;?? ????}?? ? ? ? ? 上面這段代碼比較好理解，主要就是由DCM獲取四元數(shù)；然后把四元數(shù)的虛部取出賦值給e_R_d(e_R_d = q.imag());然后對其進(jìn)行歸一化處理；最后2行是先求出互補(bǔ)系數(shù)，再通過互補(bǔ)方式求取e_R。
? ? ? ? 然后捏：計(jì)算角速度變化的大小，并對其進(jìn)行約束（constrain）。
[plain] view plain
copy /*?calculate?angular?rates?setpoint?*/?? ????_rates_sp?=?_params.att_p.emult(e_R);?? ????/*?limit?rates?*/?? ????for?(int?i?=?0;?i?<?3;?i++)?{?? ????????_rates_sp(i)?=?math::constrain(_rates_sp(i),?-_params.mc_rate_max(i),?_params.mc_rate_max(i));?? ????}?? ????/*?feed?forward?yaw?setpoint?rate?因?yàn)閥aw響應(yīng)較慢，再加入一個(gè)前饋控制*/?? _rates_sp(2)?+=?_v_att_sp.yaw_sp_move_rate?*?yaw_w?*?_params.yaw_ff;?? 上述代碼中的一個(gè)emult(e_R)的函數(shù)原型：?? ????Matrix<Type,?M,?N>?emult(const?Matrix<Type,?M,?N>?&other)?const?? ????{?? ????????Matrix<Type,?M,?N>?res;?? ????????const?Matrix<Type,?M,?N>?&self?=?*this;?? ????????for?(size_t?i?=?0;?i?<?M;?i++)?{?? ????????????for?(size_t?j?=?0;?j?<?N;?j++)?{?? ????????????????res(i?,?j)?=?self(i,?j)*other(i,?j);?? ????????????}?? ????????}?? ????????return?res;?? }?? ? ? ? ? 所以_rates_sp = _params.att_p.emult(e_R)這句話的意思就是用att_p的每一個(gè)元素和e_R中對應(yīng)位置的每一個(gè)元素相乘，結(jié)果賦值給_rates_sp角速度變量（該死的C++）。
6.2、control_attitude(dt)返回以后 [plain] view plain
copy /*?publish?attitude?rates?setpoint?*/?? ????_v_rates_sp.roll?=?_rates_sp(0);?? ????_v_rates_sp.pitch?=?_rates_sp(1);?? ????_v_rates_sp.yaw?=?_rates_sp(2);?? ????_v_rates_sp.thrust?=?_thrust_sp;?? ????_v_rates_sp.timestamp?=?hrt_absolute_time();?? ????if?(_v_rates_sp_pub?!=?nullptr)?{?? ????????orb_publish(_rates_sp_id,?_v_rates_sp_pub,?&_v_rates_sp);?? ????????}?else?if?(_rates_sp_id)?{?? ???????????_v_rates_sp_pub?=?orb_advertise(_rates_sp_id,?&_v_rates_sp);?? ????????????????????}?? ? ? ? ? 上面這部分代碼就通過control_attitude(dt)經(jīng)過一系列的算法處理過以后獲取的目標(biāo)內(nèi)環(huán)角速度值，并通過uORB模型發(fā)布出去，包含roll-pitch-yaw、油門量和時(shí)間戳。
? ? ? ? 該處正好可以再次深入理解一下uORB模型的一些理論。上述代碼涉及了orb_publish()和orb_advertise()兩個(gè)函數(shù)接口，通常第一次發(fā)布有效數(shù)據(jù)之前需要使用orb_advertise()函數(shù)進(jìn)行廣播（類似topic register），它發(fā)布成功以后會(huì)返回一個(gè)handle供orb_publish()發(fā)布時(shí)使用，即廣播之后可以使用orb_publish()進(jìn)行發(fā)布新的數(shù)據(jù)。orb_advertise()發(fā)布函數(shù)有第一個(gè)參數(shù)類似ID，返回值作為handle以便區(qū)分再次使用orb_publish()時(shí)發(fā)布的是何種消息數(shù)據(jù)，即再次說明orb_publish()需要在orb_advertise()函數(shù)接口之后使用。通過查看orb_advertise()函數(shù)的代碼原型可以了解到，該函數(shù)的作用就類似于把需要后續(xù)發(fā)布的主題（topic）注冊一下，然后才可以進(jìn)行orb_publish()。
? ? ? ? 現(xiàn)在最不明了的就是這個(gè)數(shù)據(jù)發(fā)布出去以后在哪訂閱了該數(shù)據(jù)呢或者說給誰用呢？？？自己發(fā)布，自己訂閱，生生不息息，PX4里面有很多都是自己發(fā)布然后再自己訂閱的，感謝群友我是肉包子的幫助。細(xì)節(jié)說明：在task_main()的開頭處就是訂閱各種topics，其中就有一個(gè)_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint))訂閱過程（735_linenumber），它就是在該算法執(zhí)行到最后時(shí)發(fā)布的控制量數(shù)據(jù)“_v_rates_sp” （822），也就是按照前講述的理論，自己訂閱自己發(fā)布的topic，以實(shí)現(xiàn)循環(huán)控制。其中需要注意的就是發(fā)布時(shí)用的ID和訂閱時(shí)用的不一致所迷惑了，其實(shí)它倆是一樣的;因?yàn)樵谏鲜鎏幚磉^程中是把ORB_ID(vehicle_rates_setpoint)賦值給_rates_sp_id 它的（567），賦值過程在發(fā)布topic之前，即在vehicle_status_poll()函數(shù)內(nèi)部（794）。
? ? ? ? 前面的算法都是在flag_control_attitude_enabled非零（姿態(tài)控制）的情況下實(shí)現(xiàn)的。緊接著，是在flag_control_attitude_enabled為零時(shí)，即轉(zhuǎn)變?yōu)閒lag_control_manual_enabled：手動(dòng)控制，方法類似，不在贅述。再接著，連手動(dòng)控制都為使能時(shí)，只能poll了，并把控制量都置0。
? ? ? ? 姿態(tài)控制結(jié)束。
? ? ? ? 姿態(tài)速度控制開始。
7、姿態(tài)速度控制（角速度環(huán)） ? ? ? ? 代碼來也，先感性認(rèn)識(shí)~~~~~~
[plain] view plain
copy if?(_v_control_mode.flag_control_rates_enabled)?{?? ????control_attitude_rates(dt);?? ????/*?publish?actuator?controls?*/?? ????_actuators.control[0]?=?(PX4_ISFINITE(_att_control(0)))???_att_control(0)?:?0.0f;?? ????_actuators.control[1]?=?(PX4_ISFINITE(_att_control(1)))???_att_control(1)?:?0.0f;?? ????_actuators.control[2]?=?(PX4_ISFINITE(_att_control(2)))???_att_control(2)?:?0.0f;?? ????_actuators.control[3]?=?(PX4_ISFINITE(_thrust_sp))???_thrust_sp?:?0.0f;?? ????_actuators.timestamp?=?hrt_absolute_time();?? ????_actuators.timestamp_sample?=?_ctrl_state.timestamp;?? ????_controller_status.roll_rate_integ?=?_rates_int(0);?? ????_controller_status.pitch_rate_integ?=?_rates_int(1);?? ????_controller_status.yaw_rate_integ?=?_rates_int(2);?? ????_controller_status.timestamp?=?hrt_absolute_time();?? ????if?(!_actuators_0_circuit_breaker_enabled)?{?? ????????if?(_actuators_0_pub?!=?nullptr)?{?? ????????????orb_publish(_actuators_id,?_actuators_0_pub,?&_actuators);?? ????????????perf_end(_controller_latency_perf);?? ????????}?else?if?(_actuators_id)?{?? ??????????_actuators_0_pub?=?orb_advertise(_actuators_id,?&_actuators);?? ????????????????????}?? ????????????????}?? ????/*?publish?controller?status?*/?? ????if(_controller_status_pub?!=?nullptr)?{?? ????orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub,?&_controller_status);?? ????}?else?{?? ????_controller_status_pub?=?orb_advertise(ORB_ID(mc_att_ctrl_status),?&_controller_status);?? ????????????????}?? ????????????}?? ? ? ? ? 進(jìn)入上述代碼首先就是control_attitude_rates(dt)，該函數(shù)的輸入是前面算法處理得到的_rates_sp控制量（目標(biāo)姿態(tài)），輸出是_att_control控制量。其函數(shù)原型是：
[plain] view plain
copy Void?MulticopterAttitudeControl::control_attitude_rates(float?dt)?? {?? ????/*?reset?integral?if?disarmed?*/?? ????if?(!_armed.armed?||?!_vehicle_status.is_rotary_wing)?{?? ????????_rates_int.zero();?? ????}?? ????/*?current?body?angular?rates?*/?? ????math::Vector<3>?rates;?? ????rates(0)?=?_ctrl_state.roll_rate;?? ????rates(1)?=?_ctrl_state.pitch_rate;?? ????rates(2)?=?_ctrl_state.yaw_rate;?? ????/*?angular?rates?error?*/?? ????math::Vector<3>?rates_err?=?_rates_sp?-?rates;//目標(biāo)姿態(tài)-當(dāng)前姿態(tài)?? ????_att_control?=?_params.rate_p.emult(rates_err)?+?_params.rate_d.emult(_rates_prev?-?rates)?/?dt?+?_rates_int?+?_params.rate_ff.emult(_rates_sp?-?_rates_sp_prev)?/?dt;?? ????_rates_sp_prev?=?_rates_sp;?? ????_rates_prev?=?rates;?? ????/*?update?integral?only?if?not?saturated?on?low?limit?and?if?motor?commands?are?not?saturated?*/?? ????if?(_thrust_sp?>?MIN_TAKEOFF_THRUST?&&?!_motor_limits.lower_limit?&&?!_motor_limits.upper_limit?)?{?? ????????for?(int?i?=?0;?i?<?3;?i++)?{?? ????????????if?(fabsf(_att_control(i))?<?_thrust_sp)?{?? ????????????????float?rate_i?=?_rates_int(i)?+?_params.rate_i(i)?*?rates_err(i)?*?dt;?? ????????????????if?(PX4_ISFINITE(rate_i)?&&?rate_i?>?-RATES_I_LIMIT?&&?rate_i?<?RATES_I_LIMIT?&&?? ????????????????????_att_control(i)?>?-RATES_I_LIMIT?&&?_att_control(i)?<?RATES_I_LIMIT)?{?? ????????????????????_rates_int(i)?=?rate_i;?? ????????????????}?? ????????????}?? ????????}?? ????}?? }?? ? ? ? ? 主要就是通過_ctrl_state數(shù)據(jù)結(jié)構(gòu)（前面說過要記住它的吧，當(dāng)前姿態(tài)信息）把需要的有效數(shù)據(jù)賦值給rates，然后通過rates進(jìn)行一系列的算法處理。該過程中最最最需要注意的就是這個(gè)_ctrl_state變量的獲取過程，其實(shí)還是通過uORB。前面也涉及過多次，比如control_attitude()函數(shù)內(nèi)部使用它構(gòu)造狀態(tài)四元數(shù)。
? ? ? ? 如下非常重要。。。。。。打通姿態(tài)解算和姿態(tài)控制部分。
? ? ? ? 數(shù)據(jù)獲取過程：
? ? ? ? Quaterion_CF姿態(tài)解算算法：（需要對代碼有個(gè)整體把握，不然會(huì)很暈啊，還有就是關(guān)于姿態(tài)解算部分使用的CF時(shí)，在PX4Firmware/src/module/attitude_estimator_q中）。首先是通過姿態(tài)解算部分獲取當(dāng)前的姿態(tài)信息（Quaterion_CF），獲取之后通過uORB模型發(fā)布：
[plain] view plain
copy /*?publish?to?control?state?topic?*/(646)?? orb_publish_auto(ORB_ID(control_state),?&_ctrl_state_pub,?&ctrl_state,?&ctrl_inst,?ORB_PRIO_HIGH);?? ? ? ? ? Ekf2姿態(tài)解算算法：（還是需要對代碼有個(gè)整體把握，不然還是會(huì)很暈啊，還有就是關(guān)于姿態(tài)解算部分使用的ekf2時(shí)，在PX4Firmware/src/module/ekf2中）。首先是通過姿態(tài)解算部分獲取當(dāng)前的姿態(tài)信息（ekf2），獲取之后通過uORB模型發(fā)布：
[plain] view plain
copy //?publish?control?state?data(475)?? if?(_control_state_pub?==?nullptr)?{?? ????_control_state_pub?=?orb_advertise(ORB_ID(control_state),?&ctrl_state);?? }?else?{?? ????orb_publish(ORB_ID(control_state),?_control_state_pub,?&ctrl_state);?? ????}?? ? ? ? ? 關(guān)于到底使用哪種解算算法在啟動(dòng)腳本rc_mc_app里面涉及了關(guān)于姿態(tài)解算用什么算法的問題，里面給了一個(gè)宏，通過宏定義選取的。而且在使用四元數(shù)的互補(bǔ)算法和ekf2的算法里面都對結(jié)算到的姿態(tài)信息進(jìn)行了發(fā)布處理，以便供姿態(tài)控制時(shí)訂閱使用。
? ? ? ? 然后再姿態(tài)控制中通過uORB模型訂閱：
[plain] view plain
copy _ctrl_state_sub?=?orb_subscribe(ORB_ID(control_state));(736)?? orb_copy(ORB_ID(control_state),?_ctrl_state_sub,?&_ctrl_state);(787)?? ? ? ? ? 再然后就是姿態(tài)控制量（_att_control）的獲取：獲取原則是由預(yù)期姿態(tài)控制獲取的角速度值與通過uORB獲得的角速度值做差（該部分差值代表error=target-current，_ctrl_state應(yīng)該是要控制的控制量）。rates_err的獲取就是通過經(jīng)典的PD控制器了，然后再加個(gè)前饋。還未使用I控制器；在后面會(huì)單獨(dú)使用。
[plain] view plain
copy /*?angular?rates?error?*/?? math::Vector<3>?rates_err?=?_rates_sp?-?rates;?? _att_control?=?_params.rate_p.emult(rates_err)?+?_params.rate_d.emult(_rates_prev?-?rates)?/?dt?+?_rates_int?+?_params.rate_ff.emult(_rates_sp?-?_rates_sp_prev)?/?dt;?? _rates_sp_prev?=?_rates_sp;?? _rates_prev?=?rates;?? I控制器的使用（注意使用條件）。?? ????/*?update?integral?only?if?not?saturated?on?low?limit?and?if?motor?commands?are?not?saturated?*/?? ????if?(_thrust_sp?>?MIN_TAKEOFF_THRUST?&&?!_motor_limits.lower_limit?&&?!_motor_limits.upper_limit?)?{?? ????????for?(int?i?=?0;?i?<?3;?i++)?{?? ????????????if?(fabsf(_att_control(i))?<?_thrust_sp)?{?? ????????????????float?rate_i?=?_rates_int(i)?+?_params.rate_i(i)?*?rates_err(i)?*?dt;?? ????????????????if?(PX4_ISFINITE(rate_i)?&&?rate_i?>?-RATES_I_LIMIT?&&?rate_i?<?RATES_I_LIMIT?&&?? ????????????????????_att_control(i)?>?-RATES_I_LIMIT?&&?_att_control(i)?<?RATES_I_LIMIT)?{?? ????????????????????_rates_int(i)?=?rate_i;?? ????????????????}?? ????????????}?? ????????}?? ????}?? ? ? ? ? 其中fabsf()的函數(shù)原型是（取絕對值）：
[plain] view plain
copy float?fabsf(float?x)?? {?? ??return?((x?<?0)???-x?:?x);?? }?? ? ? 常用的幾種取絕對值的函數(shù)：
? ? ? ? int abs(int i);????? ???//處理int類型的取絕對值
? ? ? ? double fabs(double i); //處理double類型的取絕對值
? ? ? ? float fabsf(float i);? //處理float類型的取絕對值
? ? ? ? 注意上面的fabsf(_att_control (i)) <_thrust_sp )這個(gè)判斷項(xiàng)，符合就執(zhí)行積分。這個(gè)做主要是為了安全考慮，當(dāng)roll的變化值需要很大時(shí)，就停止積分項(xiàng)的累加以便防止積分項(xiàng)產(chǎn)生較大的誤差。
? ? ? ? 別看這個(gè)_thrust_sp單單的一個(gè)控制量，其實(shí)它可麻煩了，不對整體核心的解算和控制（姿態(tài)解算姿態(tài)控制、位置解算位置控制）有個(gè)深入理解的話，很難看懂這部分。下面詳細(xì)介紹一下這個(gè)控制量的獲取過程，耐心看，別暈了。介紹還是需要正向介紹，在看的時(shí)候可以反向看，比較容易理解。
? ? ? ? 首先是_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));（813），訂閱所需的控制量。
? ? ? ? 然后再attitude control里面處理：_thrust_sp =_v_att_sp .thrust （653）
? ? 上面是訂閱拷貝和使用部分，下面就是發(fā)布部分。
? ? ? ? 發(fā)布分為兩個(gè)地方，一個(gè)是mc_pos_control和mavlink_receiver.cpp。主要考慮前者。
? ? ? ? ID重定義：_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);（595）
? ? ? ? 正式發(fā)布給mc_att_control: orb_publish(_attitude_setpoint_id ,_att_sp_pub ,&_att_sp );(1932)
? ? ? ? 為何稱為正式發(fā)布呢？主要是因?yàn)樵趍c_pos_control里面根據(jù)不懂的模式進(jìn)行了多次發(fā)布處理，比如idle狀態(tài)下這個(gè)_thrust_sp 就賦值為0發(fā)布出去。這個(gè)正式發(fā)布出來的才是我們飛行控制過程中需要考慮的控制量。
? ? ? ? 補(bǔ)充mavlink_receiver.cpp
? ? ? ? orb_publish(ORB_ID(vehicle_attitude_setpoint),_att_sp_pub ,&_att_sp );（951）
? ? 現(xiàn)在發(fā)現(xiàn)這個(gè)規(guī)律了吧，任務(wù)間通信（IPC）都是靠的uORB，找不到來源就查ID吧。
8、發(fā)布控制量 [plain] view plain
copy /*?publish?actuator?controls?*/?? ????????????????_actuators.control[0]?=?(PX4_ISFINITE(_att_control(0)))???_att_control(0)?:?0.0f;?? ????????????????_actuators.control[1]?=?(PX4_ISFINITE(_att_control(1)))???_att_control(1)?:?0.0f;?? ????????????????_actuators.control[2]?=?(PX4_ISFINITE(_att_control(2)))???_att_control(2)?:?0.0f;?? ????????????????_actuators.control[3]?=?(PX4_ISFINITE(_thrust_sp))???_thrust_sp?:?0.0f;?? ????????????????_actuators.timestamp?=?hrt_absolute_time();?? ????????????????_actuators.timestamp_sample?=?_ctrl_state.timestamp;?? ????????????????_controller_status.roll_rate_integ?=?_rates_int(0);?? ????????????????_controller_status.pitch_rate_integ?=?_rates_int(1);?? ????????????????_controller_status.yaw_rate_integ?=?_rates_int(2);?? ????????????????_controller_status.timestamp?=?hrt_absolute_time();?? ????????????????if?(!_actuators_0_circuit_breaker_enabled)?{?? ????????????????????if?(_actuators_0_pub?!=?nullptr)?{?? ????????????????????????orb_publish(_actuators_id,?_actuators_0_pub,?&_actuators);?? ????????????????????????perf_end(_controller_latency_perf);?? ????????????????????}?else?if?(_actuators_id)?{?? ????????????????????????_actuators_0_pub?=?orb_advertise(_actuators_id,?&_actuators);?? ????????????????????}?? ????????????????}?? ????????????????/*?publish?controller?status?*/?? ????????????????if(_controller_status_pub?!=?nullptr)?{?? ????????????????????orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub,?&_controller_status);?? ????????????????}?else?{?? ????????????????????_controller_status_pub?=?orb_advertise(ORB_ID(mc_att_ctrl_status),?&_controller_status);?? ????????????????}?? ????????????}?? ????????}?? ????????perf_end(_loop_perf);?? ????}?? ????_control_task?=?-1;?? ????return;?? ? ? ? ? PS：一個(gè)比較有趣的東西task handle：“_control_task”
? ? ? ? 了解姿態(tài)控制任務(wù)的執(zhí)行流么？可以參考這個(gè)task handle思考思考。
六、結(jié)論 ? ? ? ? 其實(shí)在mc_att_control里面就完全涵蓋了姿態(tài)控制的內(nèi)環(huán)和外環(huán)（即角速度控制、角度控制）。主要就是attitude control和attitude rate control兩個(gè)部分，前者是控制角度后者是控制角速度并把控制量輸入給mixer。在控制過程中是通過控制電機(jī)的速度以實(shí)現(xiàn)多旋翼的整體的rpy的速度，通過這個(gè)速度隨時(shí)間的累加實(shí)現(xiàn)角度控制。
? ? ? ? attitude_control 輸入是體軸矩陣R和期望的體軸矩陣Rsp，角度環(huán)只是一個(gè)P控制，算出來之后輸出的是期望的角速度值rate_sp（這一段已經(jīng)完成了所需要的角度變化，并將角度的變化值轉(zhuǎn)換到了需要的角速度值）。并且把加速度值直接輸出給attitude rate control，再經(jīng)過角速度環(huán)的pid控制，輸出值直接就給mixer，然后控制電機(jī)輸出了。
? ? ? ? 關(guān)于這些，主要還是需要理解這個(gè)控制過程：一方面是通過姿態(tài)解算部分獲取的實(shí)時(shí)的姿態(tài)信息，并通過uORB模型把姿態(tài)信息發(fā)布出去；姿態(tài)控制部分訂閱姿態(tài)解算得到的姿態(tài)信息。然后通過attitude control獲取目標(biāo)姿態(tài)和當(dāng)前姿態(tài)的角度差值并經(jīng)過算法處理得到對應(yīng)的角速度值，并把這個(gè)角速度值輸出給attitude rate control 最終獲取到需求的控制量。輸出給mixer。但是關(guān)于上述還是有一個(gè)迷惑的地方，就是在attitude control這個(gè)里面輸出的是根據(jù)目標(biāo)姿態(tài)計(jì)算的角速度值，然后再和attitude rate control 里面通過uORB獲取的當(dāng)前的角速度值做差得出角速度差值。。。。本身對這個(gè)比較懵逼。其實(shí)attitude control輸出是需要達(dá)到這個(gè)誤差角度時(shí)所需要的角速度值，用這個(gè)值與當(dāng)前的角速度值做差，求出現(xiàn)在需要的角速度值而已。這個(gè)就是為什么控制角速度的原因，進(jìn)而達(dá)到控制角度的效果。
? ? ? ? 本篇blog寫了很久了也寫了很久，收獲甚多，感觸甚多，愿本篇blog能給正在迷茫的你一點(diǎn)幫助~~~
? ? ? ? 祝愿祖國繁榮昌盛，也希望雷某案早日結(jié)束。有能力的還是移民吧~~~~
? ? ? ? 下一篇預(yù)告：歐拉旋轉(zhuǎn)和羅德里格旋轉(zhuǎn)公式~~~~
版權(quán)聲明：. https://blog.csdn.net/qq_21842557/article/details/51439171
總結(jié)
以上是生活随笔 為你收集整理的Pixhawk之姿态控制篇（1）_源码算法分析（超级有料） 的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔 網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔 推薦給好友。
