pixawk姿態(tài)控制

1、一、開篇        姿態(tài)控制篇終于來了、來了、來了        心情爽不爽？愉悅不愉悅？開心不開心？        喜歡的話就請(qǐng)我吃頓飯吧，哈哈。        其實(shí)這篇blog一周前就應(yīng)該寫的，可惜被上一篇blog霸占了。但是也不算晚，整理了很多算法基礎(chǔ)知識(shí)，使得本篇blog更充實(shí)。一人之力總是有限的，難免有不足之處，大家見諒，有寫的不好的地方勞煩指正?？吹綐?biāo)題了吧，屬于連載篇，所以后續(xù)還會(huì)有相關(guān)問題的補(bǔ)充的。二、版權(quán)聲明博主：

2、summer聲明：喝水不忘挖井人，轉(zhuǎn)載請(qǐng)注明出處。原文地址：聯(lián)系方式：dxl0725技術(shù)交流QQ：1073811738三、實(shí)驗(yàn)平臺(tái)Software Version：PX4FirmwareHardware Version：pixhawkIDE：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)變化引起的。 

3、;       下圖闡釋了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

4、_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_estima

5、tor_q        位置估計(jì) position_estimator_inav        姿態(tài)控制 mc_att_control        位置控制 mc_pos_control2、飛行控制（該部分屬于理論概述）        飛行控制分為姿態(tài)控制和位置控制，該文章主講姿態(tài)控制。        所謂姿態(tài)控制，主要就是在前期姿態(tài)解算的基礎(chǔ)上對(duì)四旋翼飛行器進(jìn)行

6、有效的飛行控制，以達(dá)到所需要的控制效果。在這種情況下，算法要學(xué)會(huì)如何連續(xù)地做決策，并且算法的評(píng)價(jià)應(yīng)該根據(jù)其所做選擇的長(zhǎng)期質(zhì)量來進(jìn)行。舉一個(gè)具體的例子，想想無人機(jī)飛行所面臨的難題：每不到一秒，算法都必須反復(fù)地選擇最佳的行動(dòng)控制?？刂七^程還是以經(jīng)典的PID反饋控制器為主（在控制環(huán)路中可以添加smith預(yù)測(cè)器）。那么如何實(shí)現(xiàn)控制呢?        以四旋翼飛行器為例，主要就是通過改變旋翼的角速度來控制四旋翼無人機(jī)。每個(gè)旋翼產(chǎn)生一個(gè)推力（F1、F2、F3、F4）和一個(gè)力矩，其共同作用構(gòu)成四旋翼無人機(jī)的主推力、偏航力矩、俯仰力矩和滾轉(zhuǎn)力矩。在四旋翼無人機(jī)中，正對(duì)的

7、一對(duì)旋翼旋轉(zhuǎn)方向一致，另外一對(duì)與之相反，來抵消靜態(tài)平穩(wěn)飛行時(shí)的回轉(zhuǎn)效應(yīng)和氣動(dòng)力矩。升降以及RPY的實(shí)現(xiàn)不在贅述?？刂茖?duì)象就是四旋翼無人機(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)。比如

8、同樣是實(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的角速度。  &#

9、160;     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 mean

10、svery 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 r

11、ange is 01. Typical value is 0.80.9. (Foraerial video optimal value may be much smaller to get smooth responce.) Yawovershot should be not more than 2-5%。        摘自：/users/multirotor_pid_tuning3、 進(jìn)入姿態(tài)控制源碼的前期過程        首先感性認(rèn)識(shí)一下姿態(tài)控制部分的框架，控制部分分為內(nèi)

12、外環(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)

13、，如下代碼所示：即mc_att_control:g_control = new MulticopterAttitudeControl;/重點(diǎn)（934），new關(guān)鍵詞應(yīng)該不陌生吧，類似于C語言中的malloc，對(duì)變量進(jìn)行內(nèi)存分配的，即對(duì)姿態(tài)控制過程中使用到的變量賦初值。int mc_att_control_main(int argc, char *argv)if (argc < 2) warnx("usage: mc_att_control start|stop|status");return 1;if (!strcmp(argv1, "start"

14、) 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

15、:g_control;mc_att_control:g_control = nullptr;warnx("start failed");return 1;return 0;        然后捏：start函數(shù)Int MulticopterAttitudeControl:start()ASSERT(_control_task = -1);/* start the task */_control_task = px4_task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SC

16、HED_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)先級(jí)什么的做了重新的define，這么做是便于代碼閱讀）：px4_task_spawn_cmd()，

17、注意它的用法。其函數(shù)原型是：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)先級(jí)，第四個(gè)是任務(wù)的?？臻g大小，第五個(gè)是任務(wù)的入口函數(shù)，最后一個(gè)一般是null。        然后捏：Void MulticopterAttitu

18、deControl:task_main_trampoline(int argc, char *argv)mc_att_control:g_control->task_main();        再然后捏：終于到本體了。Void MulticopterAttitudeControl:task_main()        比較討厭的就是為什么要封裝那么多層，應(yīng)該是水平不夠，還沒有理解此處的用意。下面就是重點(diǎn)了。五、重點(diǎn)1、姿態(tài)控制源碼_訂閱        姿態(tài)控制的代碼比

19、姿態(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)的。/* * do subscriptions */_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_at

20、titude_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(ma

21、nual_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í)變量的命名很好

22、，通俗易懂。2、 參數(shù)初始化        緊隨上面的代碼就是參數(shù)數(shù)據(jù)的獲取，parameters主要就是我們前期定義的感興趣的數(shù)據(jù)，在姿態(tài)控制中的這些數(shù)據(jù)都是私有數(shù)據(jù)（private），比如roll、pitch、yaw以及與它們對(duì)應(yīng)的PID參數(shù)。注意區(qū)分_params_handles和_params這兩種數(shù)據(jù)結(jié)構(gòu)（struct類型）。 /* initialize parameters cache */parameters_update();函數(shù)原型欣賞：int MulticopterAttitudeControl:parameters_update()

23、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_

24、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,

25、&_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(_par

26、ams_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 rattitud

27、e 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)部使用的lo

28、ck和unlock的使用（主要就是通過sem信號(hào)量控制對(duì)某一數(shù)據(jù)的互斥訪問）。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)臨

29、界區(qū)），源碼如下。/* 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ā)到一半人

30、走了，搞版本控制的，又找不到新的人來進(jìn)行開發(fā)，擱置了忘記修改回來了吧；再或者別的什么意圖。        經(jīng)過上述分析，該parameters_update()函數(shù)主要就是獲取roll、pitch、yaw的PID參數(shù)的。并對(duì)三種飛行模式（stablize、auto、acro）下的最大姿態(tài)速度做了限制。3、NuttX任務(wù)使能 /* wakeup source: vehicle attitude */px4_pollfd_struct_t fds1;fds0.fd = _ctrl_state_sub;fds0.events = POLLIN;  &

31、#160;     注意上面的fd的賦值。隨后進(jìn)入任務(wù)的循環(huán)函數(shù)：while (!_task_should_exit)。都是一樣的模式，在姿態(tài)解算時(shí)也是使用的該種方式。4、阻塞等待數(shù)據(jù)/* wait for up to 100ms for data */int pret = px4_poll(&fds0, (sizeof(fds) / sizeof(fds0), 100);/* timed out - periodic check for _task_should_exit */if (pret = 0) continue;/* this is undes

32、irable 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）。然后是打開M

33、AVLINK協(xié)議，記錄數(shù)據(jù)。如果poll失敗，直接使用關(guān)鍵詞continue從頭開始運(yùn)行（注意while和continue的組合使用）。其中的usleep(10000)函數(shù)屬于線程級(jí)睡眠函數(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'

34、usec' has elapsed or a signal is delivered to the calling thread?！?#160;   上面最后一個(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ù)都

35、是通過這個(gè)幾個(gè)*poll()函數(shù)獲取的，也是uORB模型，不理解這個(gè)后去會(huì)很暈的，別說沒提醒?。淮a中沒有一點(diǎn)冗余的部分，每一個(gè)函數(shù)、每一行都是其意義所在。 /* run controller on attitude changes */if (fds0.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 (<2

36、ms) 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_co

37、ntrol_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; 

38、;/* The output event flags */        首先就是判斷姿態(tài)控制器的控制任務(wù)是否已經(jīng)使能，然后就是檢測(cè)通過hrt獲取時(shí)間精度的所需時(shí)間，并且約束在2ms至20ms以內(nèi)。完了，orb_copy()函數(shù)怎么用的忘記了。/* * 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.

39、 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 receivi

40、ng 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, bu

41、ffer);        第三個(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ì)用到多次。        然后就是檢測(cè)數(shù)據(jù)是否已經(jīng)更新，舉一例說明問題。/* check for updates in other topics */parameter_update_poll();vehicle_stat

42、us_poll();/注意這個(gè)，后面會(huì)用到內(nèi)部的數(shù)據(jù)處理結(jié)果，即發(fā)布和訂閱的ID問題。        函數(shù)原型：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_upd

43、ate), _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閾值的檢測(cè)。官方介紹：· RATTITUDE Th

44、e 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 mixe

45、r./* 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_RATTITUD

46、E)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)）        確定飛行模式以后，根據(jù)前面的代碼分析，在確定了飛行模式以后（判斷當(dāng)前飛行模式，通過最開始部分的*poll函數(shù)獲取，還記得它么？剛才提醒過了吧），再進(jìn)行姿態(tài)控制。先來代碼

47、，然后詳細(xì)分析。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_rate

48、s_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 rat

49、es 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);

50、_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_i

51、d, &_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_attitud

52、e(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))       

53、; 獲取目標(biāo)姿態(tài)Target/* * 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;/* con

54、struct 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.q0, _ctrl_state.q1, _ctrl_state.q2, _ctrl_state.q3);math:Matrix<3, 3> R = q_att.to_dcm();/* all i

55、nput 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 */ma

56、th: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

57、, 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)

58、= -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 /* ze

59、ro 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.transp