10.1中介紹了目標位置點的計算邏輯，知道下一時刻的目標位置後，飛控需要根據當前位置進行計算，依次得到期望速度，期望拉力向量，期望姿態。至此就完成了多旋翼的位置控制。
1、期望速度計算
上篇計算得到期望位置之後，根據飛機當前位置，可知道位置差向量。在位置速度串級pid中，位置環只採用了比例，速度環採用了pid。
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if(_run_pos_control)
if
(_run_alt_control)
#限制最大水平速度
確保水平速度向量和大小在限制範圍內，對水平兩個速度進行等比例縮小；
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy =
sqrtf
(_vel_sp(0
)*_vel_sp(0
)+_vel_sp(1
)*_vel_sp(1
));if
(vel_norm_xy > _params.
vel_max(0
))
/* make sure velocity setpoint is saturated in z*/if(
_vel_sp(2
)<
-1.0f
* _params.vel_max_up)if(
_vel_sp(2
)> _params.vel_max_down)
// limit total horizontal acceleration
math::vector<
2> acc_hor;
acc_hor(0
)=(_vel_sp(0
)-_vel_sp_prev(0
))/ dt;
acc_hor(1
)=(_vel_sp(1
)-_vel_sp_prev(1
))/ dt;if(
(acc_hor.
length()
> _params.acc_hor_max)
&!_reset_pos_sp)
// limit vertical acceleration
float acc_v =
(_vel_sp(2
)-_vel_sp_prev(2
))/ dt;if(
(fabsf
(acc_v)
>
2* _params.acc_hor_max)
&!_reset_alt_sp)
_vel_sp_prev = _vel_sp;
_global_vel_sp.vx =
_vel_sp(0
);_global_vel_sp.vy =
_vel_sp(1
);_global_vel_sp.vz =
_vel_sp(2
);
thrust_sp = vel_err.
emult
(_params.vel_p)
+ _vel_err_d.
emult
(_params.vel_d)
+ thrust_int;
#確保公升力大於最小值
確保拉力向量的幅值不能小於最小拉力值，不至於飛機掉下去。
/* limit min lift */if(
-thrust_sp(2
)< thr_min)
/* limit max tilt */
if(thr_min >=
0.0f
&& tilt_max < m_pi_f /2-
0.05f)}
}
if(-
thrust_sp(2
)> thr_max)
else
期望拉力向量其實是指定了機體的期望z軸，期望航向是指定了機體的期望z軸。
即，將拉力向量歸一化，就是往姿態矩陣 的第三列。
/* desired body_z axis = -normalize(thrust_vector) */
math::vector<
3> body_x;
math::vector<
3> body_y;
math::vector<
3> body_z;
if(thrust_abs > sigma)
else
/* vector of desired yaw direction in xy plane, rotated by pi/2 */
math::vector<
3>
y_c(
-sinf
(_att_sp.yaw_body)
,cosf
(_att_sp.yaw_body)
,0.0f);
if(fabsf
(body_z(2
))> sigma)
body_x.
normalize()
;
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for(
int i =
0; i <
3; i++
)
/* copy quaternion setpoint to attitude setpoint topic */
matrix::quatf q_sp = r;
memcpy
(&_att_sp.q_d[0]
, q_sp.
data()
,sizeof
(_att_sp.q_d));
_att_sp.q_d_valid =
true
;/* calculate euler angles, for logging only, must not be used for control */
matrix::eulerf euler = r;
_att_sp.roll_body =
euler(0
);_att_sp.pitch_body =
euler(1
);
PX4飛行模式 多旋翼
遙控器輸入被轉換為橫滾 俯仰和偏航速度，當搖桿回中時飛行器不會保持平衡，可以用於翻滾等特技飛行。遙控器輸入在極限區域被轉換為橫滾 俯仰的角度和偏航的速度，否則被轉換為橫滾 俯仰和偏航角度。遙控器輸入被轉換為橫滾 俯仰的角度和偏航的速度，當搖桿回中後飛行器會保持平衡，之後會隨風或其他因素漂移。橫滾 俯...
空心杯四旋翼TinyLeaf 學習目錄
有一些內容已經寫好，但是部分內容與筆者的畢業設計相關，為避免 在查重時遇到麻煩，有一些章節要等到 查重結束後才能發布，敬請諒解！目錄 一級目錄 二級目錄 最後更新時間 介紹研討群 準備中 2020 1 14 github 準備中 2020 1 14 tinyleaf介紹 2020 1 18 教程ti...
多旋翼無人機拋飛實現流程 APM韌體
呼叫流程 arducopter.cpp fast loop update flight mode fight mode.cpp throw run 當機架構型為 或多旋翼機已經arm時,取消初始化。禁止進入拋飛模式 拋飛過程有兩個重要內容 1 手持無人機拋飛時，使電機不轉 保證安全 2 無人機處於空...