PX4代码学习系列博客(6)——offboard模式位置控制代码分析

分析offboard模式的代码需要用到以下几个模块

local_position_estimator
mavlink
mc_pos_control
mc_att_control
mixer

程序数据走向

mavlink

一般的offboard模式需要给飞控发送两条mavlink消息：

//飞控在接收到mavlink设置点消息的时候执行下面函数
void MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
{
    //这个函数把消息解析后发布了两个消息：
    //offboard_control_mode和position_setpoint_triplet
}

//飞控在接收到mavlink当前点消息用来进行外部位置估计的时候执行下面函数
void MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
{
    //这个函数把消息解析后发布消息：
    //att_pos_mocap
}

首先分析handle_message_set_position_target_local_ned这个函数发出的两条消息各是什么内容
发出的offboard_control_mode_s结构体:

offboard_control_mode.timestamp = hrt_absolute_time();
offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);

读取的vehicle_control_mode_s结构体判断当前是否是offboard模式，如果是则发送position_setpoint_triplet消息
发出的position_setpoint_triplet_s结构体:

pos_sp_triplet.current.type
pos_sp_triplet.timestamp = hrt_absolute_time();
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = set_position_target_local_ned.x;
pos_sp_triplet.current.y = set_position_target_local_ned.y;
pos_sp_triplet.current.z = set_position_target_local_ned.z;
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
pos_sp_triplet.current.acceleration_is_force = is_force_sp;
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;

local_position_estimator

在Firmware\src\modules\local_position_estimator\sensors下的mocap.cpp中有这么一行

static const uint32_t       MOCAP_TIMEOUT =     200000; // 0.2 s

这代表att_pos_mocap消息的发送频率必须大于5Hz，不然就会timeout，但是在实际测试过程中，发送的频率在30Hz以上才能稳定不timeout，这是因为它的程序写的太苛刻，只要每两帧有效数据的时间戳间隔大于0.2s就会timeout。

在Firmware\src\modules\local_position_estimator下的BlockLocalPositionEstimator.cpp中有这么一段

if (mocapUpdated) {
    if (_sensorTimeout & SENSOR_MOCAP) 
    {
        mocapInit();
    {
        mocapCorrect();
    }
}

只要超时就会重新初始化。

如果外部估计的位置足够精确，可以选择不用飞控上的其他传感器，可以在参数LPE_FUSION中设置。

mc_pos_control

然后转到旋翼的位置控制模块：
在目录E:\github\Firmware\src\modules\mc_pos_control下的mc_pos_control_main.cpp文件
打开task_main函数，发现

_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

它订阅了position_setpoint_triplet和vehicle_control_mode这两个消息。
然后是

update_velocity_derivative();
//这个函数用来更新速度偏差，如果目标位置丢失，则速度偏差为0。

do_control(dt);
//这里面的一些分支判断用的是vehicle_control_mode_s这个结构体。

do_control调用了

control_non_manual(dt);

control_non_manual调用了

control_offboard(dt);
control_position();

control_offboard用来判断是位置控制还是姿态控制并把控制量获取并发布。
我这里用的是位置和偏航角控制，所以在这个函数中执行的内容有

_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_run_pos_control = true;
_hold_offboard_xy = false;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;

最后发布vehicle_local_position_setpoint这个消息

_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);

control_position调用calculate_velocity_setpoint函数和calculate_thrust_setpoint函数，calculate_velocity_setpoint根据位置偏差计算速度偏差，里面代码不长，直接贴出

void MulticopterPositionControl::control_position(float dt)
{
    calculate_velocity_setpoint(dt);

    if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||
        _control_mode.flag_control_acceleration_enabled) 
    {
        calculate_thrust_setpoint(dt);

    } else {
        _reset_int_z = true;
    }
}

在计算速度设置点函数calculate_velocity_setpoint中：

/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) 
{

    // If for any reason, we get a NaN position setpoint, we better just stay where we are.
    if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1))) 
    {
        _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
        _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);

    } else 
    {
        _vel_sp(0) = 0.0f;
        _vel_sp(1) = 0.0f;
        warn_rate_limited("Caught invalid pos_sp in x and y");

    }
}

limit_altitude();

if (_run_alt_control) 
{
    if (PX4_ISFINITE(_pos_sp(2))) 
    {
        _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);

    } else 
    {
        _vel_sp(2) = 0.0f;
        warn_rate_limited("Caught invalid pos_sp in z");
    }
}
//后面是对起飞速度的设置和各个方向速度的一个限制。

calculate_thrust_setpoint函数根据速度设置点计算姿态设置点然后发布姿态设置点vehicle_attitude_setpoint。

mc_att_control

再转到姿态控制模块
它订阅了vehicle_control_mode和vehicle_attitude_setpoint这两个消息。

_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));

姿态控制模块把姿态设置点转换为需要的角速度。
从位置设置点到最后的三个方向的设置角速度，中间代码仔细分析比较复杂，具体的数学公式整理起来是一个浩大的工程，一个大概的思路是：

这个模块把姿态设置点转化为角速率设置点，然后在mixer中控制电机。