中級7、位置控制pixhawk control.cpp

1、SIEEIUTZXUR IVV1.位置控制的大體過程是什么？（1）copyder 和 navigator 產(chǎn)生的期望位置-_sp_triplet 結(jié)構(gòu)體（2）產(chǎn)生位置/速度設(shè)定值(期望值)-_sp向量和_vel_sp向量（3）產(chǎn)生可利用的速度設(shè)定值(期望值)-_vel_sp向量（4）產(chǎn)生可利用的推力定值(期望值)thrust_sp向量（5）根據(jù)推力向量計(jì)算姿態(tài)設(shè)定值(期望姿態(tài))q_sp 四元數(shù)矩陣和 R_sp 旋轉(zhuǎn)矩陣（6）將之前程序得到的各種信息填充_local_sp 結(jié)構(gòu)體，并發(fā)布出去-_local_sp（第 2、3 步得到的）（7）根據(jù)具體應(yīng)用更改之前得到的姿態(tài)設(shè)定值(期望姿態(tài))，并發(fā)布

2、出去-_att_sp（第 5 步得到的）extern C EXPORTmc_control_main(argc, char *argv);mc_control_main(argc, char *argv)_control:g_control = new MulticoptositionControl;/構(gòu)造函數(shù)if (OK !=_control:g_control-start()進(jìn)入 start()MulticoptositionControl:start()ASSERT(_control_task = -1);/* start the task */_control_task = px4_t

3、ask_spawn_cmd(mc_control,SCHED_DEFAULT,SCHED_PRIORITY_MAX - 5,1900,ositionControl:task_main_troline,/創(chuàng)建線程(px4_main_t)&Multicoptnullptr);if (_control_task task_main();接下來進(jìn)入 task_main()，task_main()特別長/情況也特別復(fù)雜，所以需要分清楚層次/程序運(yùn)行的條件void MulticoptositionControl:task_main()init 部分(只運(yùn)行一次)while (!_task_should_e

4、xit) ;/獲取傳感器數(shù)據(jù)/ _vel(i)賦值/標(biāo)志位賦值等if (_control_mode.flag_control_altitude_enabled |_control_mode.flag_control_ition_enabled |_control_mode.flag_control_climb_rate_enabled |_control_mode.flag_control_velocity_enabled) /基本上運(yùn)行到這都會滿足這個(gè)條件/*高度控制、位置控制、爬升速率控制、速度控制的相關(guān)程序開始*/control_manual(dt);/ control_ooard(dt

5、);/ control_auto(dt);if (!_control_mode.flag_control_manual_enabled & _sp_triplet.current.valid& _sp_triplet.current.type =ition_setpo_s:SETPO_TYPE_IDLE) else if (_control_mode.flag_control_manual_enabled& _vehicle_sus.condition_landed) else /飛行器起飛/降落等情況的處理/飛行器優(yōu)化處理為了得到更好的_vel_sp，比如利用限制水平方向加速度等/這部分為了

6、得到_vel_sp(i)if (_control_mode.flag_control_climb_rate_enabled |_control_mode.flag_control_velocity_enabled) if (_control_mode.flag_control_climb_rate_enabled) if (_control_mode.flag_control_velocity_enabled) if (!control_vel_enabled_prev & _control_mode.flag_control_velocity_enabled) math:Vector thr

7、ust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) +thrust_;/推力設(shè)定值(三維)=速度差*P+速度差的差*D+積分if (_sp_triplet.current.type =ition_setpo_s:SETPO_TYPE_TAKEOFF& !_takeoff_jumped & !_control_mode.flag_control_manual_enabled) if (!_control_mode.flag_control_velocity_enabled) if (!_control_m

8、ode.flag_control_climb_rate_enabled) _vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);/垂直速度低通濾波_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;/垂直加速度低通濾波if (!_control_mode.flag_control_manual_enabled & _sp_triplet.current.valid &_sp_triplet.current.type =ition_s

9、etpo_s:SETPO_TYPE_LAND) /著陸處理if (_control_mode.flag_control_velocity_enabled) /限制最大斜率(xy 方向推力限幅)if (_control_mode.flag_control_altitude_enabled) /推力補(bǔ)償，用于高度控制if (thrust_abs thr_max) /推力限幅/經(jīng)過之前的處理，得到合適的 thrust_spif (_control_mode.flag_control_velocity_enabled) /body_x、body_y、body_z 應(yīng)該是方向余弦矩陣的三個(gè)列向量body

10、_z = -thrust_sp / thrust_abs;/body_z 矩陣是推力設(shè)定值矩陣的標(biāo)準(zhǔn)化y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);/y_C 相當(dāng)于是矩陣(-sin(偏航角),cos(偏航角),0)body_x = y_C % body_z;/%是求叉積運(yùn)算body_y = body_z % body_x;/*再求出 R矩陣*/* fill roion matrix */for (i = 0; i 3; i+) R(i, 0) = body_x(i);R(i, 1) = body_y(i);R(i, 2)

11、= body_z(i);/*將 R矩陣 copy 到_att_sp.R_body*/memcpy(&_att_sp.R_body0, R.data, sizeof(_att_sp.R_body);/*由方向余弦旋轉(zhuǎn)矩陣 R 得到四元數(shù)，并 copy 到 att_sp.q_d*/math:Quaternion q_sp;q_sp.from_dcm(R);memcpy(&_att_sp.q_d0, &q_sp.data0, sizeof(_att_sp.q_d);/*由旋轉(zhuǎn)矩陣 R 得到姿態(tài)設(shè)置歐拉角，只是 log 調(diào)試用，不是給控制用的*/math:Vector euler = R.to_eul

12、er();_att_sp.roll_body = euler(0);_att_sp.pitch_body = euler(1);/yaw 已經(jīng)用于構(gòu)建原始矩陣else if (!_control_mode.flag_control_manual_enabled) /沒有位置控制的高度控制(故障安全降落)，固定水平姿態(tài)，不改變 yaw 角/*用于 log，方便調(diào)試*/_local_sp.acc_x = thrust_sp(0) * ONE_G;_local_sp.acc_y = thrust_sp(1) * ONE_G;_local_sp.acc_z = thrust_sp(2) * ONE_G

13、;/*將之前程序得到的各種信息填充_local_sp 結(jié)構(gòu)體，并發(fā)布出去*/* fill localition, velocity and thrust setpo*/_local_sp.timest= hrt_absolute_time();_local_sp.x = _sp(0);_local_sp.y = _sp(1);_local_sp.z = _sp(2);/第二部分第一步：產(chǎn)生位置/速度設(shè)定值(期望值)_local_sp.yaw = _att_sp.yaw_body;_local_sp.vx = _vel_sp(0);_local_sp.vy = _vel_sp(1);_local

14、_sp.vz = _vel_sp(2);/第二部分第二步的重點(diǎn)(1):產(chǎn)生可利用的速度設(shè)定值(期望值)/* publish localition setpo*/if (_local_sp_pub != nullptr) orb_publish(ORB_ID(vehicle_local_ition_setpo), _local_sp_pub, &_local_sp); else _local_sp_pub = orb_advertise(ORB_ID(vehicle_local_ition_setpo), &_local_sp);/*高度控制、位置控制、爬升速率控制、速度控制的相關(guān)程序結(jié)束*/e

15、lse if (_control_mode.flag_control_manual_enabled & _control_mode.flag_control_attitude_enabled) /*此判斷是并列于“高度控制、位置控制、爬升速率控制、速度控制”的判斷*所以會出現(xiàn)混控現(xiàn)象，在執(zhí)行任務(wù)的時(shí)候還可以控制*手動控制和姿態(tài)控制都使能，則運(yùn)行以下程序產(chǎn)生姿態(tài)設(shè)定值*刷新之前求得的_att_sp*/if (!(_control_mode.flag_control_ooard_enabled &!(_control_mode.flag_control_ition_enabled |_contro

16、l_mode.flag_control_velocity_enabled) if (_att_sp_pub != nullptr) orb_publish(_attitude_setpo_id, _att_sp_pub, &_att_sp); else if (_attitude_setpo_id) _att_sp_pub = orb_advertise(_attitude_setpo_id, &_att_sp);/發(fā)布姿態(tài)設(shè)定值，如果位置/速度失能而機(jī)外(ooard)使能，則不發(fā)布姿態(tài)設(shè)定值，因?yàn)檫@種情況/姿態(tài)設(shè)定值是通過 mavlink 應(yīng)用發(fā)布的，飛機(jī)工作于垂直起降或者做一個(gè)過渡，也不發(fā)

17、布，因?yàn)榇?時(shí)由垂直起降控制部分發(fā)布reset_z_manual = _control_mode.flag_armed & _control_mode.flag_control_manual_enabled& !_control_mode.flag_control_climb_rate_enabled;/手動控制后復(fù)位高度控制的積分(懸停油門)，以便更好的轉(zhuǎn)變?yōu)槭謩幽Ｊ娇刂骗h(huán)是什么樣子的？2.當(dāng)需要位置控制時(shí)，用 P- 控制環(huán)；當(dāng)不需要位置控制時(shí)，不要外環(huán)，只用速度環(huán)(內(nèi)環(huán))。程序中需要注意_run_control/_run_alt_control 標(biāo)志位，而_run_control/_run

18、_alt_control 標(biāo)志位的改變又涉及_hold_engaged/_alt_hold_engaged 標(biāo)志位/飛行器的位置或者速度(local_ition_estimator 或者ition_estimator_inav 中的)_(0) = _local_.x;_(1) = _local_.y;if (_params.alt_mode = 1 & _local_.dist_bottom_valid) _(2) = -_local_.dist_bottom; else _(2) = _local_.z;_vel(0) = _local_.vx;_vel(1) = _local_.vy;if

19、 (_params.alt_mode = 1 & _local_.dist_bottom_valid) _vel(2) = -_local_.dist_bottom_rate; else _vel(2) = _local_.vz;/速度的微分_vel_err_d(0) = _vel_x_deriv.update(-_vel(0);_vel_err_d(1) = _vel_y_deriv.update(-_vel(1);_vel_err_d(2) = _vel_z_deriv.update(-_vel(2);if (_control_mode.flag_control_altitude_enab

20、led |_control_mode.flag_control_ition_enabled |_control_mode.flag_control_climb_rate_enabled |_control_mode.flag_control_velocity_enabled |_control_mode.flag_control_acceleration_enabled) _run_control = true;/用于判斷是否需要外環(huán)，當(dāng)為 true 則需要外環(huán)，當(dāng)為 false 則不需要外環(huán)_run_alt_control = true;/在后面會根據(jù)此標(biāo)志位判斷if (_control_m

21、ode.flag_control_manual_enabled) /* manual control */control_manual(dt);_mode_auto = false; else if (_control_mode.flag_control_ooard_enabled) /* ooard control */control_ooard(dt);_mode_auto = false; else /* AUTO */control_auto(dt);if (_run_control) _vel_sp(0) = (_sp(0) - _(0) * _params._p(0);_vel_s

22、p(1) = (_sp(1) - _(1) * _params._p(1);if (_run_alt_control) _vel_sp(2) = (_sp(2) - _(2) * _params._p(2);void MulticoptositionControl:control_manual(float dt)/req_vel_sp 來自于math:Vector req_vel_sp; / X,Y in local frame and Z in global (D), in -1,1 normalized rangereq_vel_sp.zero();if (_control_mode.fl

23、ag_control_altitude_enabled) /* set vertical velocity setpowith throttle stick */req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); / Dif (_control_mode.flag_control_ition_enabled) /* set horizontal velocity setpowith roll/pitch stick */req_vel_sp(0) = _m

24、anual.x;req_vel_sp(1) = _manual.y;if (_control_mode.flag_control_altitude_enabled) /* reset alt setpoto current altitude if needed */reset_alt_sp();if (_control_mode.flag_control_ition_enabled) /* resetition setpoto currentition if needed */reset_sp();/* limit velocity setpo*/float req_vel_sp_norm =

25、 req_vel_sp.length();if (req_vel_sp_norm 1.0f) req_vel_sp /= req_vel_sp_norm;/* _req_vel_sp scaled to 0.1, scale it to max speed and roe around yaw */math:Matrix R_yaw_sp;R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);math:Vector req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_cruise)

26、; / in NED and scaled to actual velocity/* assisted velocity mode: user controls velocity, but if velocity is small enough,ition* hold iivated for the corresponding axis*/* horizontal axes */if (_control_mode.flag_control_ition_enabled) /* check for. hold */if (fabsf(req_vel_sp(0) _params.hold_xy_dz

27、 & fabsf(req_vel_sp(1) _params.hold_xy_dz) /沒有撥動if (!_hold_engaged) if (_params.hold_max_xy FLT_EPSILON | (fabsf(_vel(0) _params.hold_max_xy& fabsf(_vel(1) _params.hold_max_xy) /飛機(jī)速度小于一個(gè)閾值_hold_engaged = true;/激活位置控制，需要外環(huán) else _hold_engaged = false;/不激活位置控制，不需要外環(huán) else _hold_engaged = false;/不激活位置控制，

28、不需要外環(huán)/* set requested velocity setpo*/if (!_hold_engaged) /不激活位置控制，不需要外環(huán)_sp(0) = _(0);/期望位置跟隨實(shí)際位置_sp(1) = _(1);/期望位置跟隨實(shí)際位置_run_control = false; /* request velocity setpoto be used, instead ofition setpo*/判斷是否需要外環(huán)_vel_sp(0) = req_vel_sp_scaled(0);/更新期望速度_vel_sp(1) = req_vel_sp_scaled(1);/更新期望速度/* ver

29、tical axis */if (_control_mode.flag_control_altitude_enabled) /* check for. hold */if (fabsf(req_vel_sp(2) FLT_EPSILON) if (!_alt_hold_engaged) if (_params.hold_max_z FLT_EPSILON | fabsf(_vel(2) _params.hold_max_z) _alt_hold_engaged = true;/激活位置控制，需要外環(huán) else _alt_hold_engaged = false;/不激活位置控制，不需要外環(huán) e

30、lse _alt_hold_engaged = false;/不激活位置控制，不需要外環(huán)/* set requested velocity setpo*/if (!_alt_hold_engaged) _run_alt_control = false; /* request velocity setpoto be used, instead of altitude setpo*/_vel_sp(2) = req_vel_sp_scaled(2);/更新期望速度_sp(2) = _(2);/期望高度跟隨實(shí)際高度_由于_run_control = true; _run_alt_control =

31、true 一直都在程序前面并且循環(huán)，所以默認(rèn)是需要外環(huán)的，外環(huán)計(jì)算出來的期望速度會替代的產(chǎn)生的期望速度；只有當(dāng)撥動了或者速度比較大時(shí)_run_control =false;_run_alt_control = false 才只有速度控制環(huán)control_ooard(dt)和control_auto(dt)是一樣的分析control_ooard(dt);里_run_control/_run_alt_control 可為 false 或者不改control_auto(dt);里_run_control/_run_alt_control 沒有改動同時(shí)可以解釋一個(gè)現(xiàn)象：當(dāng)飛行器是ition(定點(diǎn))模式

32、下，已經(jīng)定好點(diǎn)了，此時(shí)撥動，期望位置一直跟隨實(shí)際位置，當(dāng)不撥動時(shí)，飛行器會重新定到新的位置而不返回之前的定點(diǎn)位置。順著程序再來介紹內(nèi)環(huán)比例math:Vector vel_err = _vel_sp - _vel;if (!control_vel_enabled_prev & _control_mode.flag_control_velocity_enabled) / choose velocity xyz setposucht the resulting thrust setpohas the direction/ given by the lastitude setpo/矯正 xy 速度設(shè)定值_vel_sp(0) = _vel(0) + (-PX4_R(_att