目錄
程序和控制流程
個人簡單的總結了一下整個程序的流程如下:
整個的控制流程圖可以在官網中找到:
源碼解讀
在解讀源碼之前,需要提幾個公式,第一個就是協調轉彎中的偏航控制,也就是流程圖中爲什麼輸入是空速:
(1)入口函數:fw_att_control_mian(int argc,char *argv[]):
int fw_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: fw_att_control {start|stop|status}");//如果參數不足兩個,則程序結束
return 1;
}
if (!strcmp(argv[1], "start")) {//若參數是start
if (att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
att_control::g_control = new FixedwingAttitudeControl;//分配內存空間
if (att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (PX4_OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;//防止出現野指針,造成內存崩潰問題
warn("start failed");
return 1;
}
/* check if the waiting is necessary at all *///檢查是否有必要等待
if (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
/* avoid memory fragmentation by not exiting start handler until the task has fully started *///通過在任務完全啓動之前不退出啓動處理程序來避免內存碎片
while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {//當任務不在進行時,g_control指向的是空指針
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr) {
warnx("not running");
return 1;
}
delete att_control::g_control;//釋放內存
att_control::g_control = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {//打印狀態
if (att_control::g_control) {
warnx("running");
return 0;
} else {
warnx("not running");
return 1;
}
}
warnx("unrecognized command");
return 1;
}
這段代碼主要是進行命令行參數判斷,我們可以通過串口工具MavProxy進入nsh系統進行查看(或者進入QGC 的MAVLINK consle):
如果判斷任務開始,則進入start()函數
(2)start()
int FixedwingAttitudeControl::start()
{
ASSERT(_control_task == -1); //assert宏的原型定義在<assert.h>中，其作用是如果它的條件返回錯誤，則終止程序執行,即當_control_task==-1,則程序終止
/* start the task */
_control_task = px4_task_spawn_cmd("fw_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_ATTITUDE_CONTROL,
1500,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);//創建一個任務進程
//arg0: 進程名 commander
//arg1: 調度類型（RR or FIFO）the scheduling type (RR or FIFO)
//arg2: 調度優先級
//arg3: 新進程或線程堆棧大小
//arg4: 任務/線程主函數
//arg5: 一個void指針傳遞給新任務,在這種情況下是命令行參數
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return PX4_OK;
}
int fw_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: fw_att_control {start|stop|status}");//如果參數不足兩個,則程序結束
return 1;
}
if (!strcmp(argv[1], "start")) {//若參數是start
if (att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
att_control::g_control = new FixedwingAttitudeControl;//分配內存空間
if (att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (PX4_OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;//防止出現野指針,造成內存崩潰問題
warn("start failed");
return 1;
}
/* check if the waiting is necessary at all *///檢查是否有必要等待
if (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
/* avoid memory fragmentation by not exiting start handler until the task has fully started *///通過在任務完全啓動之前不退出啓動處理程序來避免內存碎片
while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {//當任務不在進行時,g_control指向的是空指針
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr) {
warnx("not running");
return 1;
}
delete att_control::g_control;//釋放內存
att_control::g_control = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {//打印狀態
if (att_control::g_control) {
warnx("running");
return 0;
} else {
warnx("not running");
return 1;
}
}
warnx("unrecognized command");
return 1;
}
start()函數的關鍵在於開了一個名爲”fw_att_control”的線程進行姿態控制.
(3)task_main()
我們直接進入task_main()函數進行核心程序的分析:
首先進行相關數據的訂閱,在msg文件夾下可以找到相對應的結構體.
/*
* do subscriptions進行相關數據的訂閱
*/
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));//姿態數據,滾轉角速度,俯仰角速度,偏航角速度,四元數和四元數重置計數
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//航點屬尾座式垂直起降飛機的性,包括角度,起落架設置,航向控制設置(是否方向舵控制航向)
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));//控制模式,包括是否手動,是否控制爬升率等標誌
_params_sub = orb_subscribe(ORB_ID(parameter_update));//uint32 dummy:用於通知一個或多個參數的更改
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));//手動控制的航點,包括杆位置,模式切換開關等屬性變量
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));//WGS84座標系下的位置,經緯度,三軸速度等
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));//無人機狀態,包括arming state|hil state|navigation state等
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));//檢測降落
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));//電池狀態
parameters_update();//各種參數更新接下來是poll輪詢機制,對於nuttx系統來說,其poll機制是和linux一樣的,可以參考相關資料:linux的poll機制
/* get an initial update for all sensor and status data */ //獲取所有傳感器和狀態數據的初始更新
//_poll()函數是輪詢,內部的代碼是檢測是否更新,若更新了數據,就copy
vehicle_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
battery_status_poll();
_sub_airspeed.update();//更新空速
尾座式垂直起降飛機的
/* wakeup source *///Poll就是監控文件是否可讀的一種機制，作用與select一樣
px4_pollfd_struct_t fds[1];
/* Setup of loop */
fds[0].fd = _att_sub;
fds[0].events = POLLIN;
_task_running = true;這裏的fds[0.fd]爲輪詢的姿態數據,其events是爲了檢測姿態是否發生改變.
具體的關於四元數,歐拉角以及積分器重置等不再說明,下面看我們的controller.其被封裝在基類ECL_Controller中:
class __EXPORT ECL_Controller
{
public:
ECL_Controller(const char *name);
~ECL_Controller() = default;
virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
virtual float control_euler_rate(const struct ECL_ControlData &ctl_data) = 0;
virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0;
/* Setters */
void set_time_constant(float time_constant);
void set_k_p(float k_p);
void set_k_i(float k_i);
void set_k_ff(float k_ff);
void set_integrator_max(float max);
void set_max_rate(float max_rate);
void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;}
/* Getters */
float get_rate_error();
float get_desired_rate();
float get_desired_bodyrate();
void reset_integrator();
protected:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_ff;
float _integrator_max;
float _max_rate;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
};
其具體的實現在它的四個派生類中,我們選取其中pitch controller進行分析.首先看一下控制器最重要的三個函數:
- float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data):P控制器，對俯仰角進行控制。
- float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data):PI控制器，其中也加入了前饋控制，是對俯仰角速度的控制
- float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data):計算俯仰角速度
先看我們的control_attitude()函數,其內部實現的是一個P控制器:
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.airspeed))) {
warnx("not controlling pitch");
return _rate_setpoint;
}
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;//見<<飛行控制系統>>page224式5-77,_tc(0.1f)
/* limit the rate */限制速率,主要防止操作量過大,飛機不穩定
if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
}
}
return _rate_setpoint;
}可參考吳森堂的《飛行控制系統》的姿態控制系統一章.
float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data)函數是爲了角速率控制做轉換的,可見式(1-2):
float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;//姿態角速率,見飛行控制系統58頁的式2-24,這裏是俯仰角速度q
return control_bodyrate(ctl_data);
}
最後就是我們的角速率控制了:
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);//讓_last_output在-1到1之間
}
/* get the usual dt estimate */時間間隔dt
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
if (!lock_integrator && _k_i > 0.0f) {
float id = _rate_error * dt * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
* 防飽和：如果執行機構處於極限狀態，則不允許積分器增加
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value 只允許動作居中：增加價值*/
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id * _k_i;
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);//constrained(受限)
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.sc aler
+ integrator_constrained; //scaler is proportional to 1/airspeed 這個地方就是爲了 提現 速度越大 舵效作用越強(舵量越小) 縮放器正比於空速,最後是一個積分環節,其中_k_ff是前饋控制,課本235頁圖5-40
return math::constrain(_last_output, -1.0f, 1.0f);
}總結
姿態控制的源碼對照官網給出的流程圖還是比較好理解的,但其中的一些嵌入式的東西還需要深入理解.有問題可以多看看官網和去論壇討論:px4官網 討論區
