pixawk姿態(tài)控制

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

2、summer聲明：喝水不忘挖井人，轉載請注明出處。原文地址：聯(lián)系方式：dxl0725技術交流QQ：1073811738三、實驗平臺Software Version：PX4FirmwareHardware Version：pixhawkIDE：eclipse Juno （Windows）四、基礎知識1、寫在前面            無人機控制部分主要分為兩個部分，姿態(tài)控制部分和位置控制部分；位置控制可用遠程遙控控制，而姿態(tài)控制一般由無人機系統(tǒng)自動完成。姿態(tài)控制是非常重要的，因為無人機的位置變化都是由姿態(tài)變化引起的。 

3、;       下圖闡釋了PX4源碼中的兩個環(huán)路控制，分為姿態(tài)控制和位置控制。        補充：關于Pixhawk原生固件中姿態(tài)(估計/控制)和位置(估計/控制)源碼的應用問題PX4Fireware原生固件中的modules中姿態(tài)估計有多種：Attitude_estimator_ekf、Attitude_estimator_q、ekf_att_pos_estimator。位置估計有：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        四旋翼用到以上哪些估計和控制算法呢？這部分在啟動代碼rc.mc_app里面有詳細的說明。    默認的是：        姿態(tài)估計 Attitude_estima

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

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

7、一對旋翼旋轉方向一致，另外一對與之相反，來抵消靜態(tài)平穩(wěn)飛行時的回轉效應和氣動力矩。升降以及RPY的實現(xiàn)不在贅述?？刂茖ο缶褪撬男頍o人機，其動力學模型可以描述為：將其視為有一個力和三個力矩的三維剛體。如下給出了小角度變化條件下的四旋翼無人機的近似動力學模型：        PS：PX4的姿態(tài)控制部分使用的是roll-pitch和yaw分開控制的（是為了解耦控制行為），即tilt和torsion兩個環(huán)節(jié)。感性認識一下，如下圖：        根據(jù)經(jīng)驗所得，控制toll-pitch比控制yaw更容易實現(xiàn)。比如

8、同樣是實現(xiàn)10°的變化，roll-pitch需要60ms左右；但是yaw控制器卻需要接近150ms。（上面兩幅圖是出自DJI某哥寫的論文里面，僅作參考，結合理解Pixhawk）    控制流程：        1）、預處理：各參數(shù)的初始化。        2）、穩(wěn)定roll-pitch的角速度。        3）、穩(wěn)定roll-pitch的角度。        4）、穩(wěn)定yaw的角速度。  &#

9、160;     5）、穩(wěn)定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 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、 進入姿態(tài)控制源碼的前期過程        首先感性認識一下姿態(tài)控制部分的框架，控制部分分為內

12、外環(huán)控制，內環(huán)控制角速度、外環(huán)控制角度。控制過程是先根據(jù)目標姿態(tài)（target）和當前姿態(tài)（current）求出偏差角，然后通過角速度來修正這個偏差角，最終到達目標姿態(tài)。        和姿態(tài)解算算法的流程幾乎類似，主要的代碼流程首先就是按照C+語言的格式引用C語言的main函數(shù)，但是在該處變成了：extern "C" _EXPORT int mc_att_control_main(int argc, char *argv)。        然后捏：跳轉到所謂的main函數(shù)，該部分有個要注意的點

13、，如下代碼所示：即mc_att_control:g_control = new MulticopterAttitudeControl;/重點（934），new關鍵詞應該不陌生吧，類似于C語言中的malloc，對變量進行內存分配的，即對姿態(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;/重點if (mc_att_control:g_control = nullptr) warnx("alloc failed");return 1;if (OK != mc_att_control:g_control->start() /跳轉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;        其中上面有個封裝了nuttx自帶的生成task的任務創(chuàng)建函數(shù)（他把優(yōu)先級什么的做了重新的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)        第一個參數(shù)是namespace，第二個參數(shù)是選擇調度策略，第三個是任務優(yōu)先級，第四個是任務的棧空間大小，第五個是任務的入口函數(shù)，最后一個一般是null。        然后捏：Void MulticopterAttitu

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

19、姿態(tài)解算的代碼少了不少，所以接下來分析應該會比較快。        首先還是需要通過IPC模型uORB進行訂閱所需要的數(shù)據(jù)。需要注意的一個細節(jié)就是在該算法處理過程中的有效數(shù)據(jù)的用途問題，最后處理過的數(shù)據(jù)最后又被改進程自己訂閱了，然后再處理，再訂閱，一直處于循環(huán)狀態(tài)，這就是所謂的PID反饋控制器吧，最終達到所需求的控制效果，達到控制效果以后就把一系列的控制量置0（類似于idle），該任務一直在運行，隨啟動腳本啟動的。/* * 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)越的地方時變量的命名很好

22、，通俗易懂。2、 參數(shù)初始化        緊隨上面的代碼就是參數(shù)數(shù)據(jù)的獲取，parameters主要就是我們前期定義的感興趣的數(shù)據(jù)，在姿態(tài)控制中的這些數(shù)據(jù)都是私有數(shù)據(jù)（private），比如roll、pitch、yaw以及與它們對應的PID參數(shù)。注意區(qū)分_params_handles和_params這兩種數(shù)據(jù)結構（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;        重點分析一下上述代碼：其中param_get()函數(shù)比較重要，特別是內部使用的lo

28、ck和unlock的使用（主要就是通過sem信號量控制對某一數(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實現(xià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ù)了，那還有屁用啊。應該是由于程序開發(fā)和版本控制不是一個人，有的程序開發(fā)到一半人

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

31、#160;     注意上面的fd的賦值。隨后進入任務的循環(huán)函數(shù)：while (!_task_should_exit)。都是一樣的模式，在姿態(tài)解算時也是使用的該種方式。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()配置阻塞時間100ms（uORB模型的函數(shù)API）。然后是打開M

33、AVLINK協(xié)議，記錄數(shù)據(jù)。如果poll失敗，直接使用關鍵詞continue從頭開始運行（注意while和continue的組合使用）。其中的usleep(10000)函數(shù)屬于線程級睡眠函數(shù)，使當前線程掛起。原文解釋為：        “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;   上面最后一個perf_begin(_loop_perf)，是一個空函數(shù)，帶perf開頭的都是空函數(shù)，它的作用主要是“Empty function calls forroscompatibility”。5、重點來了（獲取當前姿態(tài)Current）        終于到了姿態(tài)控制器了，興奮不？別只顧著興奮了，好好理解一下。尤其是下面的幾個*poll函數(shù)，特別重要，后期算法中的很多數(shù)據(jù)都

35、是通過這個幾個*poll()函數(shù)獲取的，也是uORB模型，不理解這個后去會很暈的，別說沒提醒?。淮a中沒有一點冗余的部分，每一個函數(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 */獲取當前姿態(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)控制器的控制任務是否已經(jīng)使能，然后就是檢測通過hrt獲取時間精度的所需時間，并且約束在2ms至20ms以內。完了，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);        第三個參數(shù)就是為了保存通過orb_subscribe()函數(shù)訂閱獲得的有效數(shù)據(jù)，該部分獲取的是_ctrl_state，即控制姿態(tài)的數(shù)據(jù)，數(shù)據(jù)結構如下：（包含三軸加速度、三軸速度、三軸位置、空速、四元數(shù)、roll/pitch/yaw的速率）。記住這個copy的內容，后面會用到多次。        然后就是檢測數(shù)據(jù)是否已經(jīng)更新，舉一例說明問題。/* check for updates in other topics */parameter_update_poll();vehicle_stat

42、us_poll();/注意這個，后面會用到內部的數(shù)據(jù)處理結果，即發(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)大舵量手動），主要用在setpoint中，航點飛行。根據(jù)介紹，這個模式只有在pitch和roll都設置為Rattitude模式時才有意義，如果yaw也設置了該模式，那么就會自動被手動模式替代了。所以代碼中只做了x、y閾值的檢測。官方介紹：· 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)控制（這才是重點）        確定飛行模式以后，根據(jù)前面的代碼分析，在確定了飛行模式以后（判斷當前飛行模式，通過最開始部分的*poll函數(shù)獲取，還記得它么？剛才提醒過了吧），再進行姿態(tài)控制。先來代碼

47、，然后詳細分析。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ù)內部做了很多的處理，控制理論基本都是在這個里面體現(xiàn)的，所以需要深入研究理解它才可以進一步的研究后續(xù)的算法。它的內部會通過算法處理獲得控制量（目標姿態(tài)Target），即_rates_sp，一個vector<3>變量，以便后續(xù)控制使用。好了，進入正題。        首先是姿態(tài)控制(control_attitude)，然后是速度控制(control_attitude_rates)，一個個來。6.1、control_attitude()函數(shù)(角度控制環(huán))       

53、; 獲取目標姿態(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