摘自:https://blog.csdn.net/xiaochengyexiao/article/details/70767193?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-7.compare&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-7.compare
offboard模式的控制流程分析
xiaochengyexiao 2017-04-25 20:53:40 2727 收藏 4
分类专栏: px4 offboard mavlink 文章标签: 源代码
版权
切到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有值时可以实现定点上下移动。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)