目录
程序和控制流程
个人简单的总结了一下整个程序的流程如下:
整个的控制流程图可以在官网中找到:
源码解读
在解读源码之前,需要提几个公式,第一个就是协调转弯中的偏航控制,也就是流程图中为什么输入是空速:
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪p=ϕ˙−ψ˙sinθq=θ˙cosϕ+ψ˙cosθsinϕr=−θ˙sinϕ+ψ˙cosθcosϕψ˙=gVtanϕ(1-1)(1-2)(1-3)( & \text{1-1} 1-4)
{
p
=
ϕ
˙
−
ψ
˙
s
i
n
θ
(1-1)
q
=
θ
˙
c
o
s
ϕ
+
ψ
˙
c
o
s
θ
s
i
n
ϕ
(1-2)
r
=
−
θ
˙
s
i
n
ϕ
+
ψ
˙
c
o
s
θ
c
o
s
ϕ
(1-3)
ψ
˙
=
g
V
t
a
n
ϕ
( & \text{1-1} 1-4)
(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")) {
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;
}
if (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
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);
_control_task = px4_task_spawn_cmd("fw_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_ATTITUDE_CONTROL,
1500,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
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")) {
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;
}
if (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
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文件夹下可以找到相对应的结构体.
_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));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_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机制
vehicle_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
battery_status_poll();
_sub_airspeed.update();
尾座式垂直起降飞机的
px4_pollfd_struct_t fds[1];
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;
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;}
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)
{
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;
}
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
_rate_setpoint = pitch_error / _tc;
限制速率,主要防止操作量过大,飞机不稳定
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)
{
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
return control_bodyrate(ctl_data);
}
最后就是我们的角速率控制了:
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
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);
}
时间间隔dt
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
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;
if (_last_output < -1.0f) {
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
id = math::min(id, 0.0f);
}
_integrator += id * _k_i;
}
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.sc aler
+ integrator_constrained;
return math::constrain(_last_output, -1.0f, 1.0f);
}
总结
姿态控制的源码对照官网给出的流程图还是比较好理解的,但其中的一些嵌入式的东西还需要深入理解.有问题可以多看看官网和去论坛讨论:px4官网 讨论区
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)