一、開篇
? ? ? ? 姿態(tài)控制篇終于來了、來了、來了~~~
? ? ? ? 心情爽不爽？愉悅不愉悅？開心不開心？
? ? ? ? 喜歡的話就請我吃頓飯吧，哈哈。
? ? ? ? 其實這篇blog一周前就應該寫的，可惜被上一篇blog霸占了。但是也不算晚，整理了很多算法基礎知識，使得本篇blog更充實。一人之力總是有限的，難免有不足之處，大家見諒，有寫的不好的地方勞煩指正。看到標題了吧，屬于連載篇，所以后續(xù)還會有相關(guān)問題的補充的。
三、實驗平臺
Software?Version：PX4Firmware
Hardware Version：pixhawk
IDE：eclipse Juno （Windows）
四、基礎知識
1、寫在前面 ? ?
? ? ? ? 無人機控制部分主要分為兩個部分，姿態(tài)控制部分和位置控制部分；位置控制可用遠程遙控控制，而姿態(tài)控制一般由無人機系統(tǒng)自動完成。姿態(tài)控制是非常重要的，因為無人機的位置變化都是由姿態(tài)變化引起的。
? ? ? ? 下圖闡釋了PX4源碼中的兩個環(huán)路控制，分為姿態(tài)控制和位置控制。
? ? ? ? 補充：關(guān)于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_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_estimator_q ? ? ? ? 位置估計 position_estimator_inav ? ? ? ? 姿態(tài)控制 mc_att_control ? ? ? ? 位置控制 mc_pos_control
2、 飛行控制（該部分屬于理論概述）
? ? ? ? 飛行控制分為姿態(tài)控制和位置控制，該文章主講姿態(tài)控制。
? ? ? ? 所謂姿態(tài)控制，主要就是在前期姿態(tài)解算的基礎上對四旋翼飛行器進行有效的飛行控制，以達到所需要的控制效果。在這種情況下，算法要學會如何連續(xù)地做決策，并且算法的評價應該根據(jù)其所做選擇的長期質(zhì)量來進行。舉一個具體的例子，想想無人機飛行所面臨的難題：每不到一秒，算法都必須反復地選擇最佳的行動控制。控制過程還是以經(jīng)典的PID反饋控制器為主（在控制環(huán)路中可以添加smith預測器）。那么如何實現(xiàn)控制呢?
? ? ? ? 以四旋翼飛行器為例，主要就是通過改變旋翼的角速度來控制四旋翼無人機。每個旋翼產(chǎn)生一個推力（F1、F2、F3、F4）和一個力矩，其共同作用構(gòu)成四旋翼無人機的主推力、偏航力矩、俯仰力矩和滾轉(zhuǎn)力矩。在四旋翼無人機中，正對的一對旋翼旋轉(zhuǎn)方向一致，另外一對與之相反，來抵消靜態(tài)平穩(wěn)飛行時的回轉(zhuǎn)效應和氣動力矩。升降以及RPY的實現(xiàn)不在贅述。控制對象就是四旋翼無人機，其動力學模型可以描述為：將其視為有一個力和三個力矩的三維剛體。如下給出了小角度變化條件下的四旋翼無人機的近似動力學模型：
? ? ? ? PS：PX4的姿態(tài)控制部分使用的是roll-pitch和yaw分開控制的（是為了解耦控制行為），即tilt和torsion兩個環(huán)節(jié)。感性認識一下，如下圖：
? ? ? ? 根據(jù)經(jīng)驗所得，控制toll-pitch比控制yaw更容易實現(xiàn)。比如同樣是實現(xiàn)10°的變化，roll-pitch需要60ms左右；但是yaw控制器卻需要接近150ms。（上面兩幅圖是出自DJI某哥寫的論文里面，僅作參考，結(jié)合理解Pixhawk ）
? ? 控制流程：
? ? ? ? 1）、預處理：各參數(shù)的初始化。
? ? ? ? 2）、穩(wěn)定roll-pitch的角速度。
? ? ? ? 3）、穩(wěn)定roll-pitch的角度。
? ? ? ? 4）、穩(wěn)定yaw的角速度。
? ? ? ? 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 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、 進入姿態(tài)控制源碼的前期過程
? ? ? ? 首先感性認識一下姿態(tài)控制部分的框架，控制部分分為內(nèi)外環(huán)控制，內(nèi)環(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[])。
? ? ? ? 然后捏：跳轉(zhuǎn)到所謂的main函數(shù)，該部分有個要注意的點，如下代碼所示：即mc_att_control::g_control = new MulticopterAttitudeControl;//重點（934），new關(guān)鍵詞應該不陌生吧，類似于C語言中的malloc，對變量進行內(nèi)存分配的，即對姿態(tài)控制過程中使用到的變量賦初值。
[plain] view plaincopyprint?
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;//重點?? ????????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;?? ????}??
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;//重點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 plaincopyprint?
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;?? }??
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;
}
? ? ? ? 其中上面有個封裝了nuttx自帶的生成task的任務創(chuàng)建函數(shù)（他把優(yōu)先級什么的做了重新的define，這么做是便于代碼閱讀）：px4_task_spawn_cmd()，注意它的用法。其函數(shù)原型是：
[plain] view plaincopyprint?
px4_task_t?px4_task_spawn_cmd(const?char?*name,?int?scheduler,?int?priority,?int?stack_size,?px4_main_t?entry,?? ??????????????????char?*const?argv[])??
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ù)是選擇調(diào)度策略，第三個是任務優(yōu)先級，第四個是任務的棧空間大小，第五個是任務的入口函數(shù)，最后一個一般是null。
? ? ? ? 然后捏：
[html] view plaincopyprint?
Void??MulticopterAttitudeControl::task_main_trampoline(int?argc,?char?*argv[])?? {?? ????mc_att_control::g_control-> task_main();?? }??
Void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
{mc_att_control::g_control->task_main();
}
? ? ? ? 再然后捏：終于到本體了。
[plain] view plaincopyprint?
Void?MulticopterAttitudeControl::task_main(){}??
Void MulticopterAttitudeControl::task_main(){}
? ? ? ? 比較討厭的就是為什么要封裝那么多層，應該是水平不夠，還沒有理解此處的用意。下面就是重點了。
五、重點
1、姿態(tài)控制源碼_訂閱 ? ? ? ? 姿態(tài)控制的代碼比姿態(tài)解算的代碼少了不少，所以接下來分析應該會比較快。
? ? ? ? 首先還是需要通過IPC模型uORB進行訂閱所需要的數(shù)據(jù)。需要注意的一個細節(jié)就是在該算法處理過程中的有效數(shù)據(jù)的用途問題，最后處理過的數(shù)據(jù)最后又被改進程自己訂閱了，然后再處理，再訂閱，一直處于循環(huán)狀態(tài)，這就是所謂的PID反饋控制器吧，最終達到所需求的控制效果，達到控制效果以后就把一系列的控制量置0（類似于idle），該任務一直在運行，隨啟動腳本啟動的。
[plain] view plaincopyprint?
/*?*?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));??
/* * 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)越的地方時變量的命名很好，通俗易懂。
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ù)結(jié)構(gòu)（struct類型）。
[plain] view plaincopyprint?
?/*?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;?? }??
/* 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;
}
? ? ? ? 重點分析一下上述代碼：其中param_get()函數(shù)比較重要，特別是內(nèi)部使用的lock和unlock的使用（主要就是通過sem信號量控制對某一數(shù)據(jù)的互斥訪問）。
[plain] view plaincopyprint?
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;?? }??
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)互斥訪問（進臨界區(qū)），源碼如下。
[plain] view plaincopyprint?
/**?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 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ā)到一半人走了，搞版本控制的，又找不到新的人來進行開發(fā)，擱置了忘記修改回來了吧；再或者別的什么意圖。
? ? ? ? 經(jīng)過上述分析，該parameters_update()函數(shù)主要就是獲取roll、pitch、yaw的PID參數(shù)的。并對三種飛行模式（stablize、auto、acro）下的最大姿態(tài)速度做了限制。
3、NuttX任務使能
[plain] view plaincopyprint?
/*?wakeup?source:?vehicle?attitude?*/?? px4_pollfd_struct_t?fds[1];?? fds[0].fd?=?_ctrl_state_sub;?? fds[0].events?=?POLLIN;??
/* wakeup source: vehicle attitude */px4_pollfd_struct_t fds[1];fds[0].fd = _ctrl_state_sub;fds[0].events = POLLIN;
? ? ? ? 注意上面的fd的賦值。隨后進入任務的循環(huán)函數(shù)：while _task_should_exit ){}。都是一樣的模式，在姿態(tài)解算時也是使用的該種方式。
4、阻塞等待數(shù)據(jù) [plain] view plaincopyprint?
/*?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);??
/* 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()配置阻塞時間100ms（uORB模型的函數(shù)API）。然后是打開MAVLINK協(xié)議，記錄數(shù)據(jù)。如果poll失敗，直接使用關(guān)鍵詞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'usec' has elapsed or a signal is delivered to the calling thread。 ”
??? 上面最后一個perf_begin(_loop_perf)，是一個空函數(shù)，帶perf開頭的都是空函數(shù)，它的作用主要是“Empty function calls forroscompatibility”。
5、重點來了（獲取當前姿態(tài)Current）
? ? ? ? 終于到了姿態(tài)控制器了，興奮不？別只顧著興奮了，好好理解一下。尤其是下面的幾個*poll函數(shù)，特別重要，后期算法中的很多數(shù)據(jù)都是通過這個幾個*poll()函數(shù)獲取的，也是uORB模型，不理解這個后去會很暈的，別說沒提醒啊；代碼中沒有一點冗余的部分，每一個函數(shù)、每一行都是其意義所在。
[plain] view plaincopyprint?
/*?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?*///獲取當前姿態(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();??
/* 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 *///獲取當前姿態(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)控制器的控制任務是否已經(jīng)使能，然后就是檢測通過hrt獲取時間精度的所需時間，并且約束在2ms至20ms以內(nèi)。完了，orb_copy()函數(shù)怎么用的忘記了。。。。
[plain] view plaincopyprint?
/**?? ?*?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);?? }??
/*** 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);
}
? ? ? ? 第三個參數(shù)就是為了保存通過orb_subscribe()函數(shù)訂閱獲得的有效數(shù)據(jù)，該部分獲取的是_ctrl_state，即控制姿態(tài)的數(shù)據(jù)，數(shù)據(jù)結(jié)構(gòu)如下：（包含三軸加速度、三軸速度、三軸位置、空速、四元數(shù)、roll/pitch/yaw的速率）。記住這個 copy 的內(nèi)容，后面會用到多次。
? ? ? ? 然后就是檢測數(shù)據(jù)是否已經(jīng)更新，舉一例說明問題。
[plain] view plaincopyprint?
/*?check?for?updates?in?other?topics?*/?? parameter_update_poll();?? vehicle_status_poll();//注意這個，后面會用到內(nèi)部的數(shù)據(jù)處理結(jié)果，即發(fā)布和訂閱的ID問題。??
/* check for updates in other topics */
parameter_update_poll();
vehicle_status_poll();//注意這個，后面會用到內(nèi)部的數(shù)據(jù)處理結(jié)果，即發(fā)布和訂閱的ID問題。
? ? ? ? 函數(shù)原型：
[plain] view plaincopyprint?
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();?? ????}?? }??
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) 大舵量手動 ），主要用在setpoint中，航點飛行。根據(jù)介紹，這個模式只有在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 plaincopyprint?
/*?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;?? ????????????}?? ????????}??
/* 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)控制（這才是重點）
[plain] view plaincopyprint?
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;?? ????????????}?? ????????}??
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)部做了很多的處理，控制理論基本都是在這個里面體現(xiàn)的，所以需要深入研究理解它才可以進一步的研究后續(xù)的算法。它的內(nèi)部會通過算法處理獲得控制量（目標姿態(tài)Target），即_rates_sp，一個vector<3>變量，以便后續(xù)控制使用。好了，進入正題。
? ? ? ? 首先是姿態(tài)控制(control_attitude)，然后是速度控制(control_attitude_rates)，一個個來。
6.1、control_attitude() 函數(shù) (角度控制環(huán) )
? ? ? ? 獲取目標姿態(tài) Target
[plain] view plaincopyprint?
/**?? ?*?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;?? }??
/*** 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;
}
? ? ? ? ?詳細分析：首先就是通過uORB模型檢測姿態(tài)數(shù)據(jù)是否已經(jīng)更新。檢測到更新數(shù)據(jù)以后，把數(shù)據(jù)拷貝到當前，并通過_thrust_sp = _v_att_sp.thrust把油門控制量賦值給控制變量。
? ? ? ? 然后捏：構(gòu)建姿態(tài)旋轉(zhuǎn)矩陣（目標狀態(tài)，所謂的TargetRotation）。
[plain] view plaincopyprint?
????/*?construct?attitude?setpoint?rotation?matrix?*/?? ????math::Matrix<3,3>?R_sp;?? R_sp.set(_v_att_sp.R_body);//不在贅述，在姿態(tài)解算時使用了同樣的方法??
/* construct attitude setpoint rotation matrix */math::Matrix<3,3> R_sp;
R_sp.set(_v_att_sp.R_body);//不在贅述，在姿態(tài)解算時使用了同樣的方法
? ? ? ? 然后捏：通過控制四元數(shù)獲取當前狀態(tài)的旋轉(zhuǎn)矩陣DCM，后面在計算誤差以后旋轉(zhuǎn)到b系時使用到了該處的DCM。即由姿態(tài)解算得到的有效姿態(tài)信息。
[plain] view plaincopyprint?
/*?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;?? ????}?? };??
/* 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;}
};
? ? ? ? 然后捏：取兩個矩陣中的Z軸向量，即YAW-axis。
[plain] view plaincopyprint?
/*?all?input?data?is?ready,?run?controller?itself?*/?? ????/*?try?to?move?thrust?vector?shortest?way,?because?yaw?response?is?slower?than?roll/pitch?這個地方應該知道旋轉(zhuǎn)按照ZYX來進行的*/?? ????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));??
/* all input data is ready, run controller itself *//* try to move thrust vector shortest way, because yaw response is slower than roll/pitch 這個地方應該知道旋轉(zhuǎn)按照ZYX來進行的*/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));
? ? ? ? 然后捏：當前姿態(tài)的z軸和目標姿態(tài)的z軸的誤差大小（即需要旋轉(zhuǎn)的角度）并旋轉(zhuǎn)到b系（即先對齊Z軸）。
[plain] view plaincopyprint?
/*?axis?and?sin(angle)?of?desired?rotation?*/?? ????math::Vector<3>?e_R?=?R.transposed()?*?(R_z?%?R_sp_z);??
/* axis and sin(angle) of desired rotation */math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
? ? ? ? R_z%R_sp_z叉積，還記得這個么?在mahony算法中已經(jīng)出現(xiàn)過一次了，就是求取誤差的，本來應該z軸相互重合的，如果不是0就作為誤差項。然后再左乘旋轉(zhuǎn)矩陣旋轉(zhuǎn)到b系。
? ? ? ? 轉(zhuǎn)置源碼：
[plain] view plaincopyprint?
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));?? }??
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));
}
? ? ? ? 然后捏：計算姿態(tài)角度誤差
[plain] view plaincopyprint?
/*?calculate?angle?error?*/?? ????float?e_R_z_sin?=?e_R.length();?? float?e_R_z_cos?=?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;
? ? ? ? 然后捏：計算yaw的權(quán)重（不懂，誰幫忙解釋一下原因。。跪謝 ）
[plain] view plaincopyprint?
????/*?calculate?weight?for?yaw?control?*/?? ????float?yaw_w?=?R_sp(2,?2)?*?R_sp(2,?2);//不懂?? 第一行的這個權(quán)重純粹是因為如果不轉(zhuǎn)動roll-pitch的話那應該是1，而如果轉(zhuǎn)動的話，那個權(quán)重會平方倍衰減?（來自MR的解釋）。??
/* calculate weight for yaw control */float yaw_w = R_sp(2, 2) * R_sp(2, 2);//不懂
第一行的這個權(quán)重純粹是因為如果不轉(zhuǎn)動roll-pitch的話那應該是1，而如果轉(zhuǎn)動的話，那個權(quán)重會平方倍衰減 （來自MR的解釋）。
? ? ? ? 然后捏：因為多軸的yaw響應一般比roll/pitch慢了接近一倍，因此將兩者解耦(需要理解解耦的目的)，先補償roll-pitch的變化，計算R_rp。
[plain] view plaincopyprint?
/*?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求取公式就是利用歐拉角計算的。?? ????????/*?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;?? ????}??
/* 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求取公式就是利用歐拉角計算的。/* 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量都是通過歐拉角來表示的，這個主要就是考慮在控制時需要明確具體的歐拉角的大小，還有就是算法的解算過程是通過矩陣微分方程推導得到的（參考《慣性技術(shù)_鄧正隆》_P148-P152以及《慣性導航_秦永元》_P342），并且在《慣性技術(shù)_鄧正隆》_P154頁介紹了姿態(tài)矩陣的實時解算方法 。再判斷兩個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軸和目標姿態(tài)的z軸的誤差。
[plain] view plaincopyprint?
/*?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;??
/* 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ù)向量的叉積和點積求出誤差角度的正弦和余弦，再反正切求出角度（又忘記了？回頭看吧）。
? ? ? ? 上面介紹的是在小角度變化時，如果是大角度變化時（大于90°，可能性比較小，還是集中在上面的算法吧）使用如何方法處理。
[plain] view plaincopyprint?
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;?? ????}??
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());然后對其進行歸一化處理；最后2行是先求出互補系數(shù)，再通過互補方式求取e_R。
? ? ? ? 然后捏：計算角速度變化的大小，并對其進行約束（constrain）。
[plain] view plaincopyprint?
/*?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?因為yaw響應較慢，再加入一個前饋控制*/?? _rates_sp(2)?+=?_v_att_sp.yaw_sp_move_rate?*?yaw_w?*?_params.yaw_ff;?? 上述代碼中的一個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;?? }??
/* 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 因為yaw響應較慢，再加入一個前饋控制*/
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
上述代碼中的一個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的每一個元素和e_R中對應位置的每一個元素相乘，結(jié)果賦值給_rates_sp角速度變量（該死的C++）。
6.2、control_attitude(dt)返回以后
[plain] view plaincopyprint?
/*?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);?? ????????????????????}??
/* 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)過一系列的算法處理過以后獲取的目標內(nèi)環(huán)角速度值，并通過uORB模型發(fā)布出去，包含roll-pitch-yaw、油門量和時間戳。
? ? ? ? 該處正好可以再次深入理解一下uORB模型的一些理論。上述代碼涉及了orb_publish()和orb_advertise()兩個函數(shù)接口，通常第一次發(fā)布有效數(shù)據(jù)之前需要使用orb_advertise()函數(shù)進行廣播（類似topic register），它發(fā)布成功以后會返回一個handle供orb_publish()發(fā)布時使用，即廣播之后可以使用orb_publish()進行發(fā)布新的數(shù)據(jù)。orb_advertise()發(fā)布函數(shù)有第一個參數(shù)類似ID，返回值作為handle以便區(qū)分再次使用orb_publish()時發(fā)布的是何種消息數(shù)據(jù)，即再次說明orb_publish()需要在orb_advertise()函數(shù)接口之后使用。通過查看orb_advertise()函數(shù)的代碼原型可以了解到，該函數(shù)的作用就類似于把需要后續(xù)發(fā)布的主題（topic）注冊一下，然后才可以進行orb_publish()。
? ? ? ? 現(xiàn)在最不明了的就是這個數(shù)據(jù)發(fā)布出去以后在哪訂閱了該數(shù)據(jù)呢或者說給誰用呢？？？自己發(fā)布，自己訂閱，生生不息息，PX4里面有很多都是自己發(fā)布然后再自己訂閱的，感謝群友我是肉包子的幫助。細節(jié)說明：在task_main()的開頭處就是訂閱各種topics，其中就有一個_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint))訂閱過程（735_linenumber），它就是在該算法執(zhí)行到最后時發(fā)布的控制量數(shù)據(jù)“_v_rates_sp” （822），也就是按照前講述的理論，自己訂閱自己發(fā)布的topic，以實現(xiàn)循環(huán)控制。其中需要注意的就是發(fā)布時用的ID和訂閱時用的不一致所迷惑了，其實它倆是一樣的;因為在上述處理過程中是把ORB_ID(vehicle_rates_setpoint)賦值給_rates_sp_id 它的（567），賦值過程在發(fā)布topic之前，即在vehicle_status_poll()函數(shù)內(nèi)部（794）。
? ? ? ? 前面的算法都是在flag_control_attitude_enabled非零（姿態(tài)控制）的情況下實現(xiàn)的。緊接著，是在flag_control_attitude_enabled為零時，即轉(zhuǎn)變?yōu)閒lag_control_manual_enabled：手動控制，方法類似，不在贅述。再接著，連手動控制都為使能時，只能poll了，并把控制量都置0。
? ? ? ? 姿態(tài)控制結(jié)束。
? ? ? ? 姿態(tài)速度控制開始。
7、姿態(tài)速度控制（角速度環(huán)）
? ? ? ? 代碼來也，先感性認識~~~~~~
[plain] view plaincopyprint?
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);?? ????????????????}?? ????????????}??
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);}}
? ? ? ? 進入上述代碼首先就是control_attitude_rates(dt)，該函數(shù)的輸入是前面算法處理得到的_rates_sp控制量（目標姿態(tài)），輸出是_att_control控制量。其函數(shù)原型是：
[plain] view plaincopyprint?
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;//目標姿態(tài)-當前姿態(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;?? ????????????????}?? ????????????}?? ????????}?? ????}?? }??
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;//目標姿態(tài)-當前姿態(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)（前面說過要記住它的吧，當前姿態(tài)信息）把需要的有效數(shù)據(jù)賦值給rates，然后通過rates進行一系列的算法處理。該過程中最最最需要注意的就是這個_ctrl_state 變量的獲取過程，其實還是通過uORB。前面也涉及過多次，比如control_attitude()函數(shù)內(nèi)部使用它構(gòu)造狀態(tài)四元數(shù)。
? ? ? ? 如下非常重要。。。。。。打通姿態(tài)解算和姿態(tài)控制部分。
? ? ? ? 數(shù)據(jù)獲取過程：
? ? ? ? Quaterion_CF姿態(tài)解算算法：（需要對代碼有個整體把握，不然會很暈啊，還有就是關(guān)于姿態(tài)解算部分使用的CF時，在PX4Firmware/src/module/attitude_estimator_q中）。首先是通過姿態(tài)解算部分獲取當前的姿態(tài)信息（Quaterion_CF），獲取之后通過uORB模型發(fā)布：
[plain] view plaincopyprint?
/*?publish?to?control?state?topic?*/(646)?? orb_publish_auto(ORB_ID(control_state),?&_ctrl_state_pub,?&ctrl_state,?&ctrl_inst,?ORB_PRIO_HIGH);??
/* 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)解算算法：（還是需要對代碼有個整體把握，不然還是會很暈啊，還有就是關(guān)于姿態(tài)解算部分使用的ekf2時，在PX4Firmware/src/module/ekf2中）。首先是通過姿態(tài)解算部分獲取當前的姿態(tài)信息（ekf2），獲取之后通過uORB模型發(fā)布：
[plain] view plaincopyprint?
//?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);?? ????}??
// 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)于到底使用哪種解算算法在啟動腳本rc_mc_app里面涉及了關(guān)于姿態(tài)解算用什么算法的問題，里面給了一個宏，通過宏定義選取的。而且在使用四元數(shù)的互補算法和ekf2的算法里面都對結(jié)算到的姿態(tài)信息進行了發(fā)布處理，以便供姿態(tài)控制時訂閱使用。
? ? ? ? 然后再姿態(tài)控制中通過uORB模型訂閱：
[plain] view plaincopyprint?
_ctrl_state_sub?=?orb_subscribe(ORB_ID(control_state));(736)?? orb_copy(ORB_ID(control_state),?_ctrl_state_sub,?&_ctrl_state);(787)??
_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 ）的獲取：獲取原則是由預期姿態(tài)控制獲取的角速度值與通過uORB獲得的角速度值做差（該部分差值代表error=target-current，_ctrl_state應該是要控制的控制量）。rates_err的獲取就是通過經(jīng)典的PD控制器了，然后再加個前饋。還未使用I控制器；在后面會單獨使用。
[plain] view plaincopyprint?
/*?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;?? ????????????????}?? ????????????}?? ????????}?? ????}??
/* 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 plaincopyprint?
float?fabsf(float?x)?? {?? ??return?((x?<?0)???-x?:?x);?? }??
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 )這個判斷項，符合就執(zhí)行積分。這個做主要是為了安全考慮，當roll的變化值需要很大時，就停止積分項的累加以便防止積分項產(chǎn)生較大的誤差。
? ? ? ? 別看這個_thrust_sp 單單的一個控制量，其實它可麻煩了，不對整體核心的解算和控制（姿態(tài)解算姿態(tài)控制、位置解算位置控制）有個深入理解的話，很難看懂這部分。下面詳細介紹一下這個控制量的獲取過程，耐心看，別暈了。介紹還是需要正向介紹，在看的時候可以反向看，比較容易理解。
? ? ? ? 首先是_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));（813），訂閱所需的控制量。
? ? ? ? 然后再attitude control里面處理：_thrust_sp =_v_att_sp .thrust （653）
? ? 上面是訂閱拷貝和使用部分，下面就是發(fā)布部分。
? ? ? ? 發(fā)布分為兩個地方，一個是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ā)布呢？主要是因為在mc_pos_control里面根據(jù)不懂的模式進行了多次發(fā)布處理，比如idle狀態(tài)下這個_thrust_sp 就賦值為0發(fā)布出去。這個正式發(fā)布出來的才是我們飛行控制過程中需要考慮的控制量。
? ? ? ? 補充mavlink_receiver.cpp
? ? ? ? orb_publish(ORB_ID(vehicle_attitude_setpoint),_att_sp_pub ,&_att_sp );（951）
? ? 現(xiàn)在發(fā)現(xiàn)這個規(guī)律了吧，任務間通信（IPC）都是靠的uORB，找不到來源就查ID吧。
8、發(fā)布控制量
[plain] view plaincopyprint?
/*?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;??
/* 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：一個比較有趣的東西task handle：“_control_task”
? ? ? ? 了解姿態(tài)控制任務的執(zhí)行流么？可以參考這個task handle思考思考。
六、結(jié)論 ? ? ? ? 其實在mc_att_control里面就完全涵蓋了姿態(tài)控制的內(nèi)環(huán)和外環(huán)（即角速度控制、角度控制）。主要就是attitude control和attitude rate control兩個部分，前者是控制角度后者是控制角速度并把控制量輸入給mixer。在控制過程中是通過控制電機的速度以實現(xiàn)多旋翼的整體的rpy的速度，通過這個速度隨時間的累加實現(xiàn)角度控制。
? ? ? ? attitude_control 輸入是體軸矩陣R和期望的體軸矩陣Rsp，角度環(huán)只是一個P控制，算出來之后輸出的是期望的角速度值rate_sp（這一段已經(jīng)完成了所需要的角度變化，并將角度的變化值轉(zhuǎn)換到了需要的角速度值）。并且把加速度值直接輸出給attitude rate control，再經(jīng)過角速度環(huán)的pid控制，輸出值直接就給mixer，然后控制電機輸出了。
? ? ? ? 關(guān)于這些，主要還是需要理解這個控制過程：一方面是通過姿態(tài)解算部分獲取的實時的姿態(tài)信息，并通過uORB模型把姿態(tài)信息發(fā)布出去；姿態(tài)控制部分訂閱姿態(tài)解算得到的姿態(tài)信息。然后通過attitude control獲取目標姿態(tài)和當前姿態(tài)的角度差值并經(jīng)過算法處理得到對應的角速度值，并把這個角速度值輸出給attitude rate control 最終獲取到需求的控制量。輸出給mixer。但是關(guān)于上述還是有一個迷惑的地方，就是在attitude control這個里面輸出的是根據(jù)目標姿態(tài)計算的角速度值，然后再和attitude rate control 里面通過uORB獲取的當前的角速度值做差得出角速度差值。。。。本身對這個比較懵逼。其實attitude control輸出是需要達到這個誤差角度時所需要的角速度值，用這個值與當前的角速度值做差，求出現(xiàn)在需要的角速度值而已。這個就是為什么控制角速度的原因，進而達到控制角度的效果。
? ? ? ? 本篇blog寫了很久了也寫了很久，收獲甚多，感觸甚多，愿本篇blog能給正在迷茫的你一點幫助~~~
? ? ? ? 祝愿祖國繁榮昌盛，也希望雷某案早日結(jié)束。有能力的還是移民吧~~~~
總結(jié)
以上是生活随笔 為你收集整理的Pixhawk之姿态控制篇 的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔 網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔 推薦給好友。
