PX4 code learning series blog (6)——offboard mode position control code analysis (reprinted before, this is the second reprint)

I just found out that this article was reprinted once in August last year

https://blog.csdn.net/sinat_16643223/article/details/107874349

Reprinted from: https://blog.csdn.net/iamqianrenzhan/article/details/78066369

PX4 code learning series blog (6)-offboard mode position control code analysis

Thousands of people cut 2017-09-22 19:49:03 2961 Collection 17

Category column: px4 Article tags: pixhawk PX4 offboard position control

Last release: 2017-09-22 19:49:03 First release: 2017-09-22 19:49:03

Copyright notice: This article is the original article of the blogger, following <a href="http://creativecommons.org/licenses/by-sa/4.0/" target="_blank" rel="noopener"> CC 4.0 BY-SA < /a>Copyright agreement, please attach the original source link and this statement for reprinting.

Link to this article: https://blog.csdn.net/iamqianrenzhan/article/details/78066369

copyright

The following modules are needed to analyze the code of the offboard mode

local_position_estimator
mavlink
mc_pos_control
mc_att_control
mixer

Program data trend

mavlink

The general offboard mode needs to send two mavlink messages to the flight controller:

//飞控在接收到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
}

First analyze what is the content of the two messages issued by the handle_message_set_position_target_local_ned function
. Offboard_control_mode_s structure issued:

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);

The read vehicle_control_mode_s structure determines whether it is currently in offboard mode, and if it is, the position_setpoint_triplet message is
sent. The position_setpoint_triplet_s structure sent:

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

There is such a line in mocap.cpp under Firmware\src\modules\local_position_estimator\sensors

static const uint32_t       MOCAP_TIMEOUT =     200000; // 0.2 s

This means that the sending frequency of the att_pos_mocap message must be greater than 5Hz, otherwise it will be timeout, but in the actual test process, the sending frequency is above 30Hz to be stable and not timeout . This is because its program is written too harshly, as long as it is valid every two frames If the time stamp interval of the data is greater than 0.2s, it will timeout.

There is such a paragraph in BlockLocalPositionEstimator.cpp under Firmware\src\modules\local_position_estimator

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

It will be reinitialized as long as it times out.

If the externally estimated position is accurate enough, you can choose not to use other sensors on the flight controller, which can be set in the parameter LPE_FUSION.

mc_pos_control

Then go to the position control module of the rotor: open
the mc_pos_control_main.cpp file under the directory E:\github\Firmware\src\modules\mc_pos_control and
open the task_main function.

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

It subscribes to the two messages of position_setpoint_triplet and vehicle_control_mode.
After that

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

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

do_control called

control_non_manual(dt);

control_non_manual called

control_offboard(dt);
control_position();

control_offboard is used to determine whether it is position control or attitude control, and to obtain and release the control amount.
I use position and yaw angle control here, so the content executed in this function is

_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;

Finally, the message of vehicle_local_position_setpoint is released

_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 calls the calculate_velocity_setpoint function and calculate_thrust_setpoint function, calculate_velocity_setpoint calculates the speed deviation according to the position deviation, the code inside is not long, just post it directly

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;
    }
}

In the calculation velocity setpoint function 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");
    }
}
//后面是对起飞速度的设置和各个方向速度的一个限制。

The calculate_thrust_setpoint function calculates the attitude setpoint based on the speed setpoint and then publishes the attitude setpoint vehicle_attitude_setpoint.

mc_att_control

Turn to the attitude control module.
It subscribes to the two messages vehicle_control_mode and 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));

The attitude control module converts the attitude set point into the required angular velocity.
From the position setting point to the final three directions of angular velocity setting, careful analysis of the intermediate code is more complicated. The specific mathematical formulas are a huge project to organize. A general idea is:

This module converts the attitude set point into an angular rate set point, and then controls the motor in the mixer.