offboard模式的控制流程分析

2017-04-25 20:53:40 2727

分类专栏：文章标签：

版权

切到offboard模式后，是怎样控制飞行器飞行的呢？下面详细介绍下流程。

一、在mavlink_receiver.cpp中：

MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)

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

我从dronekit发送的是速度设定值，使得offboard_control_mode.ignore_velocity置0，其余置1。

二、在commander.cpp中

case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:

control_mode.flag_control_manual_enabled = false;

control_mode.flag_control_auto_enabled = false;

control_mode.flag_control_offboard_enabled = true;

* The control flags depend on what is ignored according to the offboard control mode topic

* Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)

control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||

!offboard_control_mode.ignore_attitude ||

!offboard_control_mode.ignore_position ||

!offboard_control_mode.ignore_velocity ||

!offboard_control_mode.ignore_acceleration_force;

control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||

!offboard_control_mode.ignore_position ||

!offboard_control_mode.ignore_velocity ||

!offboard_control_mode.ignore_acceleration_force;

control_mode.flag_control_rattitude_enabled = false;

control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&

!status.in_transition_mode;

control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||

!offboard_control_mode.ignore_position) && !status.in_transition_mode &&

!control_mode.flag_control_acceleration_enabled;

control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity ||

!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;

control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode &&

!control_mode.flag_control_acceleration_enabled;

control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||

!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;

break;

分析可得设置了以下控制模式：offboard rates attitude velocity climb_rate altitude

三、在mc_pos_control.cpp中：

MulticopterPositionControl::control_offboard(float dt)里

} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {

/* control velocity */
/* reset position setpoint to current position if needed */
reset_pos_sp();
if (fabsf(_pos_sp_triplet.current.vx) <= FLT_EPSILON &&
fabsf(_pos_sp_triplet.current.vy) <= FLT_EPSILON &&
_local_pos.xy_valid) {

if (!_hold_offboard_xy) {

_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
_hold_offboard_xy = true;
}
_run_pos_control = true;

} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {

/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
if (fabsf(_pos_sp_triplet.current.vz) <= FLT_EPSILON &&
_local_pos.z_valid) {

if (!_hold_offboard_z) {

_pos_sp(2) = _pos(2);
_hold_offboard_z = true;
}
_run_alt_control = true;
} else {

/* set position setpoint move rate */
_vel_sp(2) = _pos_sp_triplet.current.vz;
_run_alt_control = false;
_hold_offboard_z = false;
}
}

这两段代码可以分析出：offboard下的速度模式为：当vx和vy有值时，而vz为0时，可以试想xy平面的移动，实现定高；当vx和vy为0，vz有值时可以实现定点上下移动。

转载地址：http://yklni.baihongyu.com/

你可能感兴趣的文章