PX4——mc_att_control(姿態控制)

 

       本文轉載自  博主： summer   博客：《Pixhawk之姿態控制篇（1）_源碼算法分析（超級有料）》

        原博客地址：http://blog.csdn.net/qq_21842557/article/details/51439171

 

1、寫在前面    

        無人機控制部分主要分爲兩個部分，姿態控制部分和位置控制部分；位置控制可用遠程遙控控制，而姿態控制一般由無人機系統自動完成。姿態控制是非常重要的，因爲無人機的位置變化都是由姿態變化引起的。

        下圖闡釋了PX4源碼中的兩個環路控制，分爲姿態控制和位置控制。

 

        補充：關於Pixhawk原生固件中姿態(估計/控制)和位置(估計/控制)源碼的應用問題

PX4Fireware原生固件中的modules中姿態估計有多種：Attitude_estimator_ekf、Attitude_estimator_q、ekf_att_pos_estimator。

位置估計有：ekf_att_pos_estimator、local_position_estimator、position_estimator_inav

姿態控制有：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

        四旋翼用到以上哪些估計和控制算法呢？這部分在啓動代碼rc.mc_app裏面有詳細的說明。

 

    默認的是：
        姿態估計 Attitude_estimator_q
        位置估計 position_estimator_inav
        姿態控制 mc_att_control
        位置控制 mc_pos_control

2、飛行控制（該部分屬於理論概述）

        飛行控制分爲姿態控制和位置控制，該文章主講姿態控制。

        所謂姿態控制，主要就是在前期姿態解算的基礎上對四旋翼飛行器進行有效的飛行控制，以達到所需要的控制效果。在這種情況下，算法要學會如何連續地做決策，並且算法的評價應該根據其所做選擇的長期質量來進行。舉一個具體的例子，想想無人機飛行所面臨的難題：每不到一秒，算法都必須反覆地選擇最佳的行動控制。控制過程還是以經典的PID反饋控制器爲主（在控制環路中可以添加smith預測器）。那麼如何實現控制呢?

        以四旋翼飛行器爲例，主要就是通過改變旋翼的角速度來控制四旋翼無人機。每個旋翼產生一個推力（F1、F2、F3、F4）和一個力矩，其共同作用構成四旋翼無人機的主推力、偏航力矩、俯仰力矩和滾轉力矩。在四旋翼無人機中，正對的一對旋翼旋轉方向一致，另外一對與之相反，來抵消靜態平穩飛行時的迴轉效應和氣動力矩。升降以及RPY的實現不在贅述。控制對象就是四旋翼無人機，其動力學模型可以描述爲：將其視爲有一個力和三個力矩的三維剛體。如下給出了小角度變化條件下的四旋翼無人機的近似動力學模型：

        PS：PX4的姿態控制部分使用的是roll-pitch和yaw分開控制的（是爲了解耦控制行爲），即tilt和torsion兩個環節。感性認識一下，如下圖：

 

        根據經驗所得，控制toll-pitch比控制yaw更容易實現。比如同樣是實現10°的變化，roll-pitch需要60ms左右；但是yaw控制器卻需要接近150ms。（上面兩幅圖是出自DJI某哥寫的論文裏面，僅作參考，結合理解Pixhawk）

    控制流程：

        1）、預處理：各參數的初始化。

        2）、穩定roll-pitch的角速度。

        3）、穩定roll-pitch的角度。

        4）、穩定yaw的角速度。

        5）、穩定yaw的角度。

        其中在第五步中有一個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、 進入姿態控制源碼的前期過程

        首先感性認識一下姿態控制部分的框架，控制部分分爲內外環控制，內環控制角速度、外環控制角度。控制過程是先根據目標姿態（target）和當前姿態（current）求出偏差角，然後通過角速度來修正這個偏差角，最終到達目標姿態。

 

        和姿態解算算法的流程幾乎類似，主要的代碼流程首先就是按照C++語言的格式引用C語言的main函數，但是在該處變成了：

extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])。

        然後捏：跳轉到所謂的main函數，該部分有個要注意的點，如下代碼所示：即mc_att_control::g_control = new MulticopterAttitudeControl;//重點（934），new關鍵詞應該不陌生吧，類似於C語言中的malloc，對變量進行內存分配的，即對姿態控制過程中使用到的變量賦初值。

 

[plain] view plain copy

1. int mc_att_control_main(int argc, char *argv[])  
2. {  
3.     if (argc < 2) {  
4.         warnx("usage: mc_att_control {start|stop|status}");  
5.         return 1;  
6.     }  
7.     if (!strcmp(argv[1], "start")) {  
8.         if (mc_att_control::g_control != nullptr) {  
9.             warnx("already running");  
10.             return 1;  
11.         }  
12.         mc_att_control::g_control = new MulticopterAttitudeControl;//重點  
13.         if (mc_att_control::g_control == nullptr) {  
14.             warnx("alloc failed");  
15.             return 1;  
16.         }  
17.         if (OK != mc_att_control::g_control->start()) {//跳轉  
18.             delete mc_att_control::g_control;  
19.             mc_att_control::g_control = nullptr;  
20.             warnx("start failed");  
21.             return 1;  
22.         }  
23.         return 0;  
24.     }  

 

        然後捏：start函數

[plain] view plain copy

1. Int MulticopterAttitudeControl::start()  
2. {  
3.     ASSERT(_control_task == -1);  
4.     /* start the task */  
5.     _control_task = px4_task_spawn_cmd("mc_att_control",  
6.                        SCHED_DEFAULT,  
7.                        SCHED_PRIORITY_MAX - 5,  
8.                        1500,  
9. (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,  
10.                        nullptr);  
11.     if (_control_task < 0) {  
12.         warn("task start failed");  
13.         return -errno;  
14.     }  
15.     return OK;  
16. }  

        其中上面有個封裝了nuttx自帶的生成task的任務創建函數（他把優先級什麼的做了重新的define，這麼做是便於代碼閱讀）：px4_task_spawn_cmd()，注意它的用法。其函數原型是：

[plain] view plain copy

1. px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,  
2.                   char *const argv[])  

 

        第一個參數是namespace，第二個參數是選擇調度策略，第三個是任務優先級，第四個是任務的棧空間大小，第五個是任務的入口函數，最後一個一般是null。

 

        然後捏：

[html] view plain copy

1. Void  MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])  
2. {  
3.     mc_att_control::g_control->task_main();  
4. }  

        再然後捏：終於到本體了。

[plain] view plain copy

1. Void MulticopterAttitudeControl::task_main(){}  

        比較討厭的就是爲什麼要封裝那麼多層，應該是水平不夠，還沒有理解此處的用意。下面就是重點了。

五、重點

 

1、姿態控制源碼_訂閱

 

        姿態控制的代碼比姿態解算的代碼少了不少，所以接下來分析應該會比較快。

        首先還是需要通過IPC模型uORB進行訂閱所需要的數據。需要注意的一個細節就是在該算法處理過程中的有效數據的用途問題，最後處理過的數據最後又被改進程自己訂閱了，然後再處理，再訂閱，一直處於循環狀態，這就是所謂的PID反饋控制器吧，最終達到所需求的控制效果，達到控制效果以後就把一系列的控制量置0（類似於idle），該任務一直在運行，隨啓動腳本啓動的。

[plain] view plain copy

1. /* * do subscriptions */  
2.     _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));  
3.     _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));  
4.     _ctrl_state_sub = orb_subscribe(ORB_ID(control_state));  
5.     _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));  
6.     _params_sub = orb_subscribe(ORB_ID(parameter_update));  
7.     _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));  
8.     _armed_sub = orb_subscribe(ORB_ID(actuator_armed));  
9.     _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));  
10.     _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));  

        上面這些訂閱到底訂閱了哪些東西呢，顧名思義，根據ORB()中的參數的名稱就是知道訂閱的到底用於做什麼的了。這套開源代碼中最優越的地方時變量的命名很好，通俗易懂。

2、 參數初始化

        緊隨上面的代碼就是參數數據的獲取，parameters主要就是我們前期定義的感興趣的數據，在姿態控制中的這些數據都是私有數據（private），比如roll、pitch、yaw以及與它們對應的PID參數。注意區分_params_handles和_params這兩種數據結構（struct類型）。

[plain] view plain copy

1.  /* initialize parameters cache */  
2.     parameters_update();  
3. 函數原型欣賞：  
4. int MulticopterAttitudeControl::parameters_update()  
5. {  
6.     float v;  
7.     /* roll gains */  
8.     param_get(_params_handles.roll_p, &v);  
9.     _params.att_p(0) = v;  
10.     param_get(_params_handles.roll_rate_p, &v);  
11.     _params.rate_p(0) = v;  
12.     param_get(_params_handles.roll_rate_i, &v);  
13.     _params.rate_i(0) = v;  
14.     param_get(_params_handles.roll_rate_d, &v);  
15.     _params.rate_d(0) = v;  
16.     param_get(_params_handles.roll_rate_ff, &v);  
17.     _params.rate_ff(0) = v;  
18.     /* pitch gains */  
19.      省略  
20.     /* yaw gains */  
21.      省略  
22.     /* angular rate limits */  
23.     param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);  
24.     _params.mc_rate_max(0) = math::radians(_params.roll_rate_max);  
25.     param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);  
26.     _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);  
27.     param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);  
28.     _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);  
29.     /* manual rate control scale and auto mode roll/pitch rate limits */  
30.     param_get(_params_handles.acro_roll_max, &v);  
31.     _params.acro_rate_max(0) = math::radians(v);  
32.     param_get(_params_handles.acro_pitch_max, &v);  
33.     _params.acro_rate_max(1) = math::radians(v);  
34.     param_get(_params_handles.acro_yaw_max, &v);  
35.     _params.acro_rate_max(2) = math::radians(v);  
36.     /* stick deflection needed in rattitude mode to control rates not angles */  
37.     param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);  
38.     _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);  
39.     return OK;  
40. }  

        重點分析一下上述代碼：其中param_get()函數比較重要，特別是內部使用的lock和unlock的使用（主要就是通過sem信號量控制對某一數據的互斥訪問）。

[plain] view plain copy

1. Int param_get(param_t param, void *val)  
2. {  
3.     int result = -1;  
4.     param_lock();  
5.     const void *v = param_get_value_ptr(param);  
6.     if (val != NULL) {  
7.         memcpy(val, v, param_size(param));  
8.         result = 0;  
9.     }  
10.     param_unlock();  
11.     return result;  
12. }  

        上述使用的*lock和*unlock通過sem實現互斥訪問（進臨界區），源碼如下。

[plain] view plain copy

1. /** lock the parameter store */  
2. static void param_lock(void)  
3. {  
4.     //do {} while (px4_sem_wait(¶m_sem) != 0);  
5. }  
6. /** unlock the parameter store */  
7. static void param_unlock(void)  
8. {  
9.     //px4_sem_post(¶m_sem);  
10. }  

        上面是開源代碼中的，代碼裏面把lock和unlock函數都寫成空函數了，那還有屁用啊。應該是由於程序開發和版本控制不是一個人，有的程序開發到一半人走了，搞版本控制的，又找不到新的人來進行開發，擱置了忘記修改回來了吧；再或者別的什麼意圖。

        經過上述分析，該parameters_update()函數主要就是獲取roll、pitch、yaw的PID參數的。並對三種飛行模式（stablize、auto、acro）下的最大姿態速度做了限制。

3、NuttX任務使能

[plain] view plain copy

1. /* wakeup source: vehicle attitude */  
2. px4_pollfd_struct_t fds[1];  
3. fds[0].fd = _ctrl_state_sub;  
4. fds[0].events = POLLIN;  

        注意上面的fd的賦值。隨後進入任務的循環函數：while (!_task_should_exit){}。都是一樣的模式，在姿態解算時也是使用的該種方式。

4、阻塞等待數據

 

[plain] view plain copy

1. /* wait for up to 100ms for data */  
2.     int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);  
3.     /* timed out - periodic check for _task_should_exit */  
4.     if (pret == 0) {  
5.         continue;  
6.     }  
7.     /* this is undesirable but not much we can do - might want to flag unhappy status */  
8.     if (pret < 0) {  
9.         warn("mc att ctrl: poll error %d, %d", pret, errno);  
10.         /* sleep a bit before next try */  
11.         usleep(100000);  
12.         continue;  
13.     }  
14.     perf_begin(_loop_perf);  

        首先是px4_poll()配置阻塞時間100ms（uORB模型的函數API）。然後是打開MAVLINK協議，記錄數據。如果poll失敗，直接使用關鍵詞continue從頭開始運行（注意while和continue的組合使用）。其中的usleep(10000)函數屬於線程級睡眠函數，使當前線程掛起。原文解釋爲：

        “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。”

    上面最後一個perf_begin(_loop_perf)，是一個空函數，帶perf開頭的都是空函數，它的作用主要是“Empty function calls forroscompatibility”。

 

5、重點來了（獲取當前姿態Current）

 

        終於到了姿態控制器了，興奮不？別隻顧着興奮了，好好理解一下。尤其是下面的幾個*poll函數，特別重要，後期算法中的很多數據都是通過這個幾個*poll()函數獲取的，也是uORB模型，不理解這個後去會很暈的，別說沒提醒啊；代碼中沒有一點冗餘的部分，每一個函數、每一行都是其意義所在。

[plain] view plain copy

1. /* run controller on attitude changes */  
2.     if (fds[0].revents & POLLIN) {  
3.         static uint64_t last_run = 0;  
4.         float dt = (hrt_absolute_time() - last_run) / 1000000.0f;  
5.         last_run = hrt_absolute_time();  
6.         /* guard against too small (<2ms) and too large (>20ms) dt's */  
7.         if (dt < 0.002f) {  
8.             dt = 0.002f;  
9.         } else if (dt > 0.02f) {  
10.             dt = 0.02f;  
11.         }  
12.         /* copy attitude and control state topics *///獲取當前姿態數據  
13.         orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);  
14.         /* check for updates in other topics */  
15.         parameter_update_poll();  
16.         vehicle_control_mode_poll();  
17.         arming_status_poll();  
18.         vehicle_manual_poll();  
19.         vehicle_status_poll();  
20.         vehicle_motor_limits_poll();  

         注意上面的revents，要與events區分開來，兩者的區別如下：

    pollevent_t events;  /* The input event flags */

    pollevent_t revents; /* The output event flags */

        首先就是判斷姿態控制器的控制任務是否已經使能，然後就是檢測通過hrt獲取時間精度的所需時間，並且約束在2ms至20ms以內。完了，orb_copy()函數怎麼用的忘記了。。。。

[plain] view plain copy

1. /**  
2.  * Fetch data from a topic.  
3. * This is the only operation that will reset the internal marker that  
4.  * indicates that a topic has been updated for a subscriber. Once poll  
5.  * or check return indicating that an updaet is available, this call  
6.  * must be used to update the subscription.  
7. * @param meta    The uORB metadata (usually from the ORB_ID() macro)  
8.  *      for the topic.  
9.  * @param handle  A handle returned from orb_subscribe.  
10.  * @param buffer  Pointer to the buffer receiving the data, or NULL  
11.  *      if the caller wants to clear the updated flag without  
12.  *      using the data.  
13.  * @return    OK on success, ERROR otherwise with errno set accordingly.  
14.  */  
15. int  orb_copy(const struct orb_metadata *meta, int handle, void *buffer)  
16. {  
17.     return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer);  
18. }  

        第三個參數就是爲了保存通過orb_subscribe()函數訂閱獲得的有效數據，該部分獲取的是_ctrl_state，即控制姿態的數據，數據結構如下：（包含三軸加速度、三軸速度、三軸位置、空速、四元數、roll/pitch/yaw的速率）。記住這個copy的內容，後面會用到多次。

        然後就是檢測數據是否已經更新，舉一例說明問題。

[plain] view plain copy

1. /* check for updates in other topics */  
2. parameter_update_poll();  
3. vehicle_status_poll();//注意這個，後面會用到內部的數據處理結果，即發佈和訂閱的ID問題。  

        函數原型：

[plain] view plain copy

1. Void MulticopterAttitudeControl::parameter_update_poll()  
2. {  
3.     bool updated;  
4.     /* Check if parameters have changed */  
5.     orb_check(_params_sub, &updated);  
6.     if (updated) {  
7.         struct parameter_update_s param_update;  
8.         orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);  
9.         parameters_update();  
10.     }  
11. }  

        然後捏：飛行模式判斷是否是MAIN_STATE_RATTITUD模式，該模式是一種新的飛行模式，只控制角速度，不控制角度，俗稱半自穩模式（小舵量自穩大舵量手動），主要用在setpoint中，航點飛行。根據介紹，這個模式只有在pitch和roll都設置爲Rattitude模式時纔有意義，如果yaw也設置了該模式，那麼就會自動被手動模式替代了。所以代碼中只做了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

1. /* 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 */  
2. if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){  
3.         if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||  
4.         fabsf(_manual_control_sp.x) > _params.rattitude_thres){  
5.         _v_control_mode.flag_control_attitude_enabled = false;  
6.             }  
7.         }  

6、姿態控制（這纔是重點）
        確定飛行模式以後，根據前面的代碼分析，在確定了飛行模式以後（判斷當前飛行模式，通過最開始部分的*poll函數獲取，還記得它麼？剛纔提醒過了吧），再進行姿態控制。先來代碼，然後詳細分析。

 

[plain] view plain copy

1. if (_v_control_mode.flag_control_attitude_enabled)  
2. {  
3.             control_attitude(dt);  
4.                 /* publish attitude rates setpoint */  
5.                 _v_rates_sp.roll = _rates_sp(0);  
6.                 _v_rates_sp.pitch = _rates_sp(1);  
7.                 _v_rates_sp.yaw = _rates_sp(2);  
8.                 _v_rates_sp.thrust = _thrust_sp;  
9.                 _v_rates_sp.timestamp = hrt_absolute_time();  
10.                 if (_v_rates_sp_pub != nullptr) {  
11.                     orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);  
12.   
13.                 } else if (_rates_sp_id) {  
14.                     _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);  
15.                 }  
16.             //}  
17.         } else {  
18.             /* attitude controller disabled, poll rates setpoint topic */  
19.             if (_v_control_mode.flag_control_manual_enabled) {  
20.                 /* manual rates control - ACRO mode */  
21.                 _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);  
22.                 _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);  
23.   
24.                 /* publish attitude rates setpoint */  
25.                 _v_rates_sp.roll = _rates_sp(0);  
26.                 _v_rates_sp.pitch = _rates_sp(1);  
27.                 _v_rates_sp.yaw = _rates_sp(2);  
28.                 _v_rates_sp.thrust = _thrust_sp;  
29.                 _v_rates_sp.timestamp = hrt_absolute_time();  
30.                 if (_v_rates_sp_pub != nullptr) {  
31.                     orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);  
32.                 } else if (_rates_sp_id) {  
33.                     _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);  
34.                 }  
35.             } else {  
36.                 /* attitude controller disabled, poll rates setpoint topic */  
37.                 vehicle_rates_setpoint_poll();  
38.                 _rates_sp(0) = _v_rates_sp.roll;  
39.                 _rates_sp(1) = _v_rates_sp.pitch;  
40.                 _rates_sp(2) = _v_rates_sp.yaw;  
41.                 _thrust_sp = _v_rates_sp.thrust;  
42.             }  
43.         }  

        上面的代碼中，初始就是control_attitude(dt)，控制數據都是由它來獲取的。該函數內部做了很多的處理，控制理論基本都是在這個裏面體現的，所以需要深入研究理解它纔可以進一步的研究後續的算法。它的內部會通過算法處理獲得控制量（目標姿態Target），即_rates_sp，一個vector<3>變量，以便後續控制使用。好了，進入正題。

        首先是姿態控制(control_attitude)，然後是速度控制(control_attitude_rates)，一個個來。

6.1、control_attitude()函數(角度控制環)

        獲取目標姿態Target

[plain] view plain copy

1. /**  
2.  * Attitude controller.  
3.  * Input: 'vehicle_attitude_setpoint' topics (depending on mode)  
4.  * Output: '_rates_sp' vector, '_thrust_sp'  
5.  */  
6. Void MulticopterAttitudeControl::control_attitude(float dt)  
7. {  
8.     vehicle_attitude_setpoint_poll();  
9.     _thrust_sp = _v_att_sp.thrust;  
10.     /* construct attitude setpoint rotation matrix */  
11.     math::Matrix<3, 3> R_sp;  
12.     R_sp.set(_v_att_sp.R_body);  
13.     /* get current rotation matrix from control state quaternions */  
14.     math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);  
15.     math::Matrix<3, 3> R = q_att.to_dcm();  
16.     /* all input data is ready, run controller itself */  
17.     /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch 約兩倍*/   
18.     math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));  
19.     math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));  
20.     /* axis and sin(angle) of desired rotation */  
21.     math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);  
22.     /* calculate angle error */  
23.     float e_R_z_sin = e_R.length();  
24.     float e_R_z_cos = R_z * R_sp_z;  
25.     /* calculate weight for yaw control */  
26.     float yaw_w = R_sp(2, 2) * R_sp(2, 2);  
27.     /* calculate rotation matrix after roll/pitch only rotation */  
28.     math::Matrix<3, 3> R_rp;  
29.     if (e_R_z_sin > 0.0f) {  
30.         /* get axis-angle representation */  
31.         float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);  
32.         math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;  
33.         e_R = e_R_z_axis * e_R_z_angle;  
34.         /* cross product matrix for e_R_axis */  
35.         math::Matrix<3, 3> e_R_cp;  
36.         e_R_cp.zero();  
37.         e_R_cp(0, 1) = -e_R_z_axis(2);  
38.         e_R_cp(0, 2) = e_R_z_axis(1);  
39.         e_R_cp(1, 0) = e_R_z_axis(2);  
40.         e_R_cp(1, 2) = -e_R_z_axis(0);  
41.         e_R_cp(2, 0) = -e_R_z_axis(1);  
42.         e_R_cp(2, 1) = e_R_z_axis(0);  
43.         /* rotation matrix for roll/pitch only rotation */  
44.         R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));  
45.     } else {  
46.         /* zero roll/pitch rotation */  
47.         R_rp = R;  
48.     }  
49.     /* R_rp and R_sp has the same Z axis, calculate yaw error */  
50.     math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));  
51.     math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));  
52.     e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;  
53.     if (e_R_z_cos < 0.0f) {  
54.         /* for large thrust vector rotations use another rotation method:  
55.          * calculate angle and axis for R -> R_sp rotation directly */  
56.         math::Quaternion q;  
57.         q.from_dcm(R.transposed() * R_sp);  
58.         math::Vector<3> e_R_d = q.imag();  
59.         e_R_d.normalize();  
60.         e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));  
61.         /* use fusion of Z axis based rotation and direct rotation */  
62.         float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;  
63.         e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;  
64.     }  
65.     /* calculate angular rates setpoint */  
66.     _rates_sp = _params.att_p.emult(e_R);  
67.     /* limit rates */  
68.     for (int i = 0; i < 3; i++) {  
69.         _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));  
70.     }  
71.     /* feed forward yaw setpoint rate */  
72.     _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;  
73. }  

         詳細分析：首先就是通過uORB模型檢測姿態數據是否已經更新。檢測到更新數據以後，把數據拷貝到當前，並通過_thrust_sp = _v_att_sp.thrust把油門控制量賦值給控制變量。

        然後捏：構建姿態旋轉矩陣（目標狀態，所謂的TargetRotation）。

[plain] view plain copy

1.     /* construct attitude setpoint rotation matrix */  
2.     math::Matrix<3,3> R_sp;  
3. R_sp.set(_v_att_sp.R_body);//不在贅述，在姿態解算時使用了同樣的方法  

 

        然後捏：通過控制四元數獲取當前狀態的旋轉矩陣DCM，後面在計算誤差以後旋轉到b系時使用到了該處的DCM。即由姿態解算得到的有效姿態信息。

[plain] view plain copy

1. /* get current rotation matrix from control state quaternions */  
2.     math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);  
3.     math::Matrix<3, 3> R = q_att.to_dcm();  
4.     通過math庫構建四元數；獲取DCM的函數原型：無可厚非，都懂的  
5.     /*** create rotation matrix for the quaternion */  
6.     Matrix<3, 3> to_dcm(void) const {  
7.         Matrix<3, 3> R;  
8.         float aSq = data[0] * data[0];  
9.         float bSq = data[1] * data[1];  
10.         float cSq = data[2] * data[2];  
11.         float dSq = data[3] * data[3];  
12.         R.data[0][0] = aSq + bSq - cSq - dSq;  
13.         R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);  
14.         R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);  
15.         R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);  
16.         R.data[1][1] = aSq - bSq + cSq - dSq;  
17.         R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);  
18.         R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);  
19.         R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);  
20.         R.data[2][2] = aSq - bSq - cSq + dSq;  
21.         return R;  
22.     }  
23. };  

        然後捏：取兩個矩陣中的Z軸向量，即YAW-axis。

[plain] view plain copy

1. /* all input data is ready, run controller itself */  
2.     /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch 這個地方應該知道旋轉按照ZYX來進行的*/  
3.     math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));  
4.     math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));  

 

        然後捏：當前姿態的z軸和目標姿態的z軸的誤差大小（即需要旋轉的角度）並旋轉到b系（即先對齊Z軸）。

[plain] view plain copy

1. /* axis and sin(angle) of desired rotation */  
2.     math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);  

        R_z%R_sp_z叉積，還記得這個麼?在mahony算法中已經出現過一次了，就是求取誤差的，本來應該z軸相互重合的，如果不是0就作爲誤差項。然後再左乘旋轉矩陣旋轉到b系。

        轉置源碼：

[plain] view plain copy

1. Matrix3<T> Matrix3<T>::transposed(void) const  
2. {  
3.     return Matrix3<T>(Vector3<T>(a.x, b.x, c.x),  
4.                       Vector3<T>(a.y, b.y, c.y),  
5.                       Vector3<T>(a.z, b.z, c.z));  
6. }  

        然後捏：計算姿態角度誤差（姿態誤差），一個數學知識背景：由公式a×b=︱a︱︱b︱sinθ，a•b=︱a︱︱b︱cosθ，這裏的R_z和R_sp_z都是單位向量，模值爲1，因此誤差向量e_R（a×b叉積就是誤差）的模就是sinθ，點積就是cosθ。

[plain] view plain copy

1. /* calculate angle error */  
2.     float e_R_z_sin = e_R.length();  
3. float e_R_z_cos = R_z * R_sp_z;  

        然後捏：計算yaw的權重（不懂，誰幫忙解釋一下原因。。跪謝）

[plain] view plain copy

1.     /* calculate weight for yaw control */  
2.     float yaw_w = R_sp(2, 2) * R_sp(2, 2);//不懂  
3. 第一行的這個權重純粹是因爲如果不轉動roll-pitch的話那應該是1，而如果轉動的話，那個權重會平方倍衰減 （來自MR的解釋）。  

        然後捏：因爲多軸的yaw響應一般比roll/pitch慢了接近一倍，因此將兩者解耦(需要理解解耦的目的)，先補償roll-pitch的變化，計算R_rp。

[plain] view plain copy

1. /* calculate rotation matrix after roll/pitch only rotation */  
2.     math::Matrix<3, 3> R_rp;  
3.     if (e_R_z_sin > 0.0f) {  
4.         /* get axis-angle representation */  
5.         float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);  
6.         math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;  
7.         e_R = e_R_z_axis * e_R_z_angle;//很大的用途，下面的R_rp求取公式就是利用歐拉角計算的。  
8.         /* cross product matrix for e_R_axis */  
9.         math::Matrix<3, 3> e_R_cp;  
10.         e_R_cp.zero();  
11.         e_R_cp(0, 1) = -e_R_z_axis(2);  
12.         e_R_cp(0, 2) = e_R_z_axis(1);  
13.         e_R_cp(1, 0) = e_R_z_axis(2);  
14.         e_R_cp(1, 2) = -e_R_z_axis(0);  
15.         e_R_cp(2, 0) = -e_R_z_axis(1);  
16.         e_R_cp(2, 1) = e_R_z_axis(0);  
17.         /* rotation matrix for roll/pitch only rotation */  
18.         R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));//羅德里格旋轉公式：Rodrigues rotation formula  
19.     } else {  
20.         /* zero roll/pitch rotation */  
21.         R_rp = R;  
22.     }  

        首先需要明確的就是上述處理過程中的DCM量都是通過歐拉角來表示的，這個主要就是考慮在控制時需要明確具體的歐拉角的大小，還有就是算法的解算過程是通過矩陣微分方程推導得到的（參考《慣性技術_鄧正隆》_P148-P152以及《慣性導航_秦永元》_P342），並且在《慣性技術_鄧正隆》_P154頁介紹了姿態矩陣的實時解算方法。再判斷兩個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該步計算主要就是利用e_R_z_sin=e_R.length()，往上看就是了，不會這麼快就忘記了吧？！)。然後就是e_R =e_R_z_axis* e_R_z_angle了（主要就是爲了誤差向量用角度量表示）。

        然後捏：計算yaw的誤差，該誤差是roll_pitch獲取的z軸和目標姿態的z軸的誤差。

[plain] view plain copy

1. /* R_rp and R_sp has the same Z axis, calculate yaw error */  
2.     math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));  
3.     math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));  
4.     e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;  

        該部分同樣是根據向量的叉積和點積求出誤差角度的正弦和餘弦，再反正切求出角度（又忘記了？回頭看吧）。

        上面介紹的是在小角度變化時，如果是大角度變化時（大於90°，可能性比較小，還是集中在上面的算法吧）使用如何方法處理。

[plain] view plain copy

1. if (e_R_z_cos < 0.0f) {  
2.         /* for large thrust vector rotations use another rotation method:  
3.          * calculate angle and axis for R->R_sp rotation directly */  
4.         math::Quaternion q;  
5.         q.from_dcm(R.transposed() * R_sp);  
6.         math::Vector<3> e_R_d = q.imag();  
7.         e_R_d.normalize();  
8.         e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));//不懂  
9.         /* use fusion of Z axis based rotation and direct rotation */  
10.         float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;  
11.         e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;  
12.     }  

        上面這段代碼比較好理解，主要就是由DCM獲取四元數；然後把四元數的虛部取出賦值給e_R_d(e_R_d = q.imag());然後對其進行歸一化處理；最後2行是先求出互補係數，再通過互補方式求取e_R。

        然後捏：計算角速度變化的大小，並對其進行約束（constrain）。

[plain] view plain copy

1. /* calculate angular rates setpoint */  
2.     _rates_sp = _params.att_p.emult(e_R);  
3.     /* limit rates */  
4.     for (int i = 0; i < 3; i++) {  
5.         _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));  
6.     }  
7.     /* feed forward yaw setpoint rate 因爲yaw響應較慢，再加入一個前饋控制*/  
8. _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;  
9. 上述代碼中的一個emult(e_R)的函數原型：  
10.     Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const  
11.     {  
12.         Matrix<Type, M, N> res;  
13.         const Matrix<Type, M, N> &self = *this;  
14.         for (size_t i = 0; i < M; i++) {  
15.             for (size_t j = 0; j < N; j++) {  
16.                 res(i , j) = self(i, j)*other(i, j);  
17.             }  
18.         }  
19.         return res;  
20. }  

        所以_rates_sp = _params.att_p.emult(e_R)這句話的意思就是用att_p的每一個元素和e_R中對應位置的每一個元素相乘，結果賦值給_rates_sp角速度變量（該死的C++）。

6.2、control_attitude(dt)返回以後

 

[plain] view plain copy

1. /* publish attitude rates setpoint */  
2.     _v_rates_sp.roll = _rates_sp(0);  
3.     _v_rates_sp.pitch = _rates_sp(1);  
4.     _v_rates_sp.yaw = _rates_sp(2);  
5.     _v_rates_sp.thrust = _thrust_sp;  
6.     _v_rates_sp.timestamp = hrt_absolute_time();  
7.     if (_v_rates_sp_pub != nullptr) {  
8.         orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);  
9.         } else if (_rates_sp_id) {  
10.            _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);  
11.                     }  

        上面這部分代碼就通過control_attitude(dt)經過一系列的算法處理過以後獲取的目標內環角速度值，並通過uORB模型發佈出去，包含roll-pitch-yaw、油門量和時間戳。

        該處正好可以再次深入理解一下uORB模型的一些理論。上述代碼涉及了orb_publish()和orb_advertise()兩個函數接口，通常第一次發佈有效數據之前需要使用orb_advertise()函數進行廣播（類似topic register），它發佈成功以後會返回一個handle供orb_publish()發佈時使用，即廣播之後可以使用orb_publish()進行發佈新的數據。orb_advertise()發佈函數有第一個參數類似ID，返回值作爲handle以便區分再次使用orb_publish()時發佈的是何種消息數據，即再次說明orb_publish()需要在orb_advertise()函數接口之後使用。通過查看orb_advertise()函數的代碼原型可以瞭解到，該函數的作用就類似於把需要後續發佈的主題（topic）註冊一下，然後纔可以進行orb_publish()。

        現在最不明瞭的就是這個數據發佈出去以後在哪訂閱了該數據呢或者說給誰用呢？？？自己發佈，自己訂閱，生生不息息，PX4裏面有很多都是自己發佈然後再自己訂閱的，感謝羣友我是肉包子的幫助。細節說明：在task_main()的開頭處就是訂閱各種topics，其中就有一個_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint))訂閱過程（735_linenumber），它就是在該算法執行到最後時發佈的控制量數據“_v_rates_sp”（822），也就是按照前講述的理論，自己訂閱自己發佈的topic，以實現循環控制。其中需要注意的就是發佈時用的ID和訂閱時用的不一致所迷惑了，其實它倆是一樣的;因爲在上述處理過程中是把ORB_ID(vehicle_rates_setpoint)賦值給_rates_sp_id它的（567），賦值過程在發佈topic之前，即在vehicle_status_poll()函數內部（794）。

        前面的算法都是在flag_control_attitude_enabled非零（姿態控制）的情況下實現的。緊接着，是在flag_control_attitude_enabled爲零時，即轉變爲flag_control_manual_enabled：手動控制，方法類似，不在贅述。再接着，連手動控制都爲使能時，只能poll了，並把控制量都置0。

        姿態控制結束。

        姿態速度控制開始。

 

7、姿態速度控制（角速度環）

 

        代碼來也，先感性認識~~~~~~

[plain] view plain copy

1. if (_v_control_mode.flag_control_rates_enabled) {  
2.     control_attitude_rates(dt);  
3.     /* publish actuator controls */  
4.     _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;  
5.     _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;  
6.     _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;  
7.     _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;  
8.     _actuators.timestamp = hrt_absolute_time();  
9.     _actuators.timestamp_sample = _ctrl_state.timestamp;  
10.     _controller_status.roll_rate_integ = _rates_int(0);  
11.     _controller_status.pitch_rate_integ = _rates_int(1);  
12.     _controller_status.yaw_rate_integ = _rates_int(2);  
13.     _controller_status.timestamp = hrt_absolute_time();  
14.     if (!_actuators_0_circuit_breaker_enabled) {  
15.         if (_actuators_0_pub != nullptr) {  
16.             orb_publish(_actuators_id, _actuators_0_pub, &_actuators);  
17.             perf_end(_controller_latency_perf);  
18.         } else if (_actuators_id) {  
19.           _actuators_0_pub = orb_advertise(_actuators_id, &_actuators);  
20.                     }  
21.                 }  
22.     /* publish controller status */  
23.     if(_controller_status_pub != nullptr) {  
24.     orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);  
25.     } else {  
26.     _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);  
27.                 }  
28.             }  

        進入上述代碼首先就是control_attitude_rates(dt)，該函數的輸入是前面算法處理得到的_rates_sp控制量（目標姿態），輸出是_att_control控制量。其函數原型是：

[plain] view plain copy

1. Void MulticopterAttitudeControl::control_attitude_rates(float dt)  
2. {  
3.     /* reset integral if disarmed */  
4.     if (!_armed.armed || !_vehicle_status.is_rotary_wing) {  
5.         _rates_int.zero();  
6.     }  
7.     /* current body angular rates */  
8.     math::Vector<3> rates;  
9.     rates(0) = _ctrl_state.roll_rate;  
10.     rates(1) = _ctrl_state.pitch_rate;  
11.     rates(2) = _ctrl_state.yaw_rate;  
12.     /* angular rates error */  
13.     math::Vector<3> rates_err = _rates_sp - rates;//目標姿態-當前姿態  
14.     _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;  
15.     _rates_sp_prev = _rates_sp;  
16.     _rates_prev = rates;  
17.     /* update integral only if not saturated on low limit and if motor commands are not saturated */  
18.     if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {  
19.         for (int i = 0; i < 3; i++) {  
20.             if (fabsf(_att_control(i)) < _thrust_sp) {  
21.                 float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;  
22.                 if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&  
23.                     _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {  
24.                     _rates_int(i) = rate_i;  
25.                 }  
26.             }  
27.         }  
28.     }  
29. }  

        主要就是通過_ctrl_state數據結構（前面說過要記住它的吧，當前姿態信息）把需要的有效數據賦值給rates，然後通過rates進行一系列的算法處理。該過程中最最最需要注意的就是這個_ctrl_state變量的獲取過程，其實還是通過uORB。前面也涉及過多次，比如control_attitude()函數內部使用它構造狀態四元數。

        如下非常重要。。。。。。打通姿態解算和姿態控制部分。

        數據獲取過程：

        Quaterion_CF姿態解算算法：（需要對代碼有個整體把握，不然會很暈啊，還有就是關於姿態解算部分使用的CF時，在PX4Firmware/src/module/attitude_estimator_q中）。首先是通過姿態解算部分獲取當前的姿態信息（Quaterion_CF），獲取之後通過uORB模型發佈：

[plain] view plain copy

1. /* publish to control state topic */(646)  
2. orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);  

        Ekf2姿態解算算法：（還是需要對代碼有個整體把握，不然還是會很暈啊，還有就是關於姿態解算部分使用的ekf2時，在PX4Firmware/src/module/ekf2中）。首先是通過姿態解算部分獲取當前的姿態信息（ekf2），獲取之後通過uORB模型發佈：

[plain] view plain copy

1. // publish control state data(475)  
2. if (_control_state_pub == nullptr) {  
3.     _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);  
4. } else {  
5.     orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);  
6.     }  

        關於到底使用哪種解算算法在啓動腳本rc_mc_app裏面涉及了關於姿態解算用什麼算法的問題，裏面給了一個宏，通過宏定義選取的。而且在使用四元數的互補算法和ekf2的算法裏面都對結算到的姿態信息進行了發佈處理，以便供姿態控制時訂閱使用。

        然後再姿態控制中通過uORB模型訂閱：

[plain] view plain copy

1. _ctrl_state_sub = orb_subscribe(ORB_ID(control_state));(736)  
2. orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);(787)  

        再然後就是姿態控制量（_att_control）的獲取：獲取原則是由預期姿態控制獲取的角速度值與通過uORB獲得的角速度值做差（該部分差值代表error=target-current，_ctrl_state應該是要控制的控制量）。rates_err的獲取就是通過經典的PD控制器了，然後再加個前饋。還未使用I控制器；在後面會單獨使用。

[plain] view plain copy

1. /* angular rates error */  
2. math::Vector<3> rates_err = _rates_sp - rates;  
3. _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;  
4. _rates_sp_prev = _rates_sp;  
5. _rates_prev = rates;  
6. I控制器的使用（注意使用條件）。  
7.     /* update integral only if not saturated on low limit and if motor commands are not saturated */  
8.     if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {  
9.         for (int i = 0; i < 3; i++) {  
10.             if (fabsf(_att_control(i)) < _thrust_sp) {  
11.                 float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;  
12.                 if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&  
13.                     _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {  
14.                     _rates_int(i) = rate_i;  
15.                 }  
16.             }  
17.         }  
18.     }  

        其中fabsf()的函數原型是（取絕對值）：

[plain] view plain copy

1. float fabsf(float x)  
2. {  
3.   return ((x < 0) ? -x : x);  
4. }  

    常用的幾種取絕對值的函數：

        int abs(int i);         //處理int類型的取絕對值

        double fabs(double i); //處理double類型的取絕對值

        float fabsf(float i);  //處理float類型的取絕對值

        注意上面的fabsf(_att_control(i)) <_thrust_sp)這個判斷項，符合就執行積分。這個做主要是爲了安全考慮，當roll的變化值需要很大時，就停止積分項的累加以便防止積分項產生較大的誤差。

        別看這個_thrust_sp單單的一個控制量，其實它可麻煩了，不對整體核心的解算和控制（姿態解算姿態控制、位置解算位置控制）有個深入理解的話，很難看懂這部分。下面詳細介紹一下這個控制量的獲取過程，耐心看，別暈了。介紹還是需要正向介紹，在看的時候可以反向看，比較容易理解。

        首先是_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));（813），訂閱所需的控制量。

        然後再attitude control裏面處理：_thrust_sp =_v_att_sp.thrust（653）

    上面是訂閱拷貝和使用部分，下面就是發佈部分。

        發佈分爲兩個地方，一個是mc_pos_control和mavlink_receiver.cpp。主要考慮前者。

        ID重定義：_attitude_setpoint_id= ORB_ID(vehicle_attitude_setpoint);（595）

        正式發佈給mc_att_control: orb_publish(_attitude_setpoint_id,_att_sp_pub,&_att_sp);(1932)

        爲何稱爲正式發佈呢？主要是因爲在mc_pos_control裏面根據不懂的模式進行了多次發佈處理，比如idle狀態下這個_thrust_sp就賦值爲0發佈出去。這個正式發佈出來的纔是我們飛行控制過程中需要考慮的控制量。

        補充mavlink_receiver.cpp

        orb_publish(ORB_ID(vehicle_attitude_setpoint),_att_sp_pub,&_att_sp);（951）

    現在發現這個規律了吧，任務間通信（IPC）都是靠的uORB，找不到來源就查ID吧。

8、發佈控制量

 

[plain] view plain copy

1. /* publish actuator controls */  
2.                 _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;  
3.                 _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;  
4.                 _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;  
5.                 _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;  
6.                 _actuators.timestamp = hrt_absolute_time();  
7.                 _actuators.timestamp_sample = _ctrl_state.timestamp;  
8.                 _controller_status.roll_rate_integ = _rates_int(0);  
9.                 _controller_status.pitch_rate_integ = _rates_int(1);  
10.                 _controller_status.yaw_rate_integ = _rates_int(2);  
11.                 _controller_status.timestamp = hrt_absolute_time();  
12.                 if (!_actuators_0_circuit_breaker_enabled) {  
13.                     if (_actuators_0_pub != nullptr) {  
14.                         orb_publish(_actuators_id, _actuators_0_pub, &_actuators);  
15.                         perf_end(_controller_latency_perf);  
16.                     } else if (_actuators_id) {  
17.                         _actuators_0_pub = orb_advertise(_actuators_id, &_actuators);  
18.                     }  
19.                 }  
20.                 /* publish controller status */  
21.                 if(_controller_status_pub != nullptr) {  
22.                     orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);  
23.                 } else {  
24.                     _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);  
25.                 }  
26.             }  
27.         }  
28.         perf_end(_loop_perf);  
29.     }  
30.     _control_task = -1;  
31.     return;  

        PS：一個比較有趣的東西task handle：“_control_task”

        瞭解姿態控制任務的執行流麼？可以參考這個task handle思考思考。

六、結論

        其實在mc_att_control裏面就完全涵蓋了姿態控制的內環和外環（即角速度控制、角度控制）。主要就是attitude control和attitude rate control兩個部分，前者是控制角度後者是控制角速度並把控制量輸入給mixer。在控制過程中是通過控制電機的速度以實現多旋翼的整體的rpy的速度，通過這個速度隨時間的累加實現角度控制。

        attitude_control 輸入是體軸矩陣R和期望的體軸矩陣Rsp，角度環只是一個P控制，算出來之後輸出的是期望的角速度值rate_sp（這一段已經完成了所需要的角度變化，並將角度的變化值轉換到了需要的角速度值）。並且把加速度值直接輸出給attitude rate control，再經過角速度環的pid控制，輸出值直接就給mixer，然後控制電機輸出了。

        關於這些，主要還是需要理解這個控制過程：一方面是通過姿態解算部分獲取的實時的姿態信息，並通過uORB模型把姿態信息發佈出去；姿態控制部分訂閱姿態解算得到的姿態信息。然後通過attitude control獲取目標姿態和當前姿態的角度差值並經過算法處理得到對應的角速度值，並把這個角速度值輸出給attitude rate control 最終獲取到需求的控制量。輸出給mixer。但是關於上述還是有一個迷惑的地方，就是在attitude control這個裏面輸出的是根據目標姿態計算的角速度值，然後再和attitude rate control 裏面通過uORB獲取的當前的角速度值做差得出角速度差值。。。。本身對這個比較懵逼。其實attitude control輸出是需要達到這個誤差角度時所需要的角速度值，用這個值與當前的角速度值做差，求出現在需要的角速度值而已。這個就是爲什麼控制角速度的原因，進而達到控制角度的效果。