px4源码解读之fw_att_control

程序和控制流程

个人简单的总结了一下整个程序的流程如下:
这里写图片描述

整个的控制流程图可以在官网中找到:
这里写图片描述

源码解读

在解读源码之前,需要提几个公式,第一个就是协调转弯中的偏航控制,也就是流程图中为什么输入是空速:

{\begin{cases} p = \dot{ϕ} - \dot{ψ} s i n θ & (1-1) \\ q = \dot{θ} c o s ϕ + \dot{ψ} c o s θ s i n ϕ & (1-2) \\ r = - \dot{θ} s i n ϕ + \dot{ψ} c o s θ c o s ϕ & (1-3) \\ \dot{ψ} = \frac{g}{V} t a n ϕ & ( & \text{1-1} 1-4) \end{cases}

$\begin{cases} p=\dot{\phi}-\dot{\psi}sin\theta & \text{(1-1)} \\ q=\dot{\theta}cos\phi+\dot{\psi}cos\theta sin\phi & \text{(1-2)} \\ r=-\dot{\theta}sin\phi +\dot{\psi}cos\theta cos\phi & \text{(1-3)} \\ \dot{\psi}=\frac{g}{V}tan\phi & \text{( & \text{1-1} 1-4)} \end{cases}$
(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官网讨论区