fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 02:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: Fix autonomous landing without GPS

Browse Source 
Due to a regression bug in #1741 the autonomous landing without GPS used manual RC
input to determine setpoints which broke auto landing without GPS.
This commit is contained in:
Johan Jansen
2015-02-16 14:48:49 +01:00
committed by Lorenz Meier
parent 3f45f51d7a
commit 51fcb440d0
1 changed files with 16 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+16 -13
View File
@@ -1351,18 +1351,25 @@ MulticopterPositionControl::task_main()
}
if(!_control_mode.flag_control_velocity_enabled) {
/* generate attitude setpoint from manual controls */
if(_control_mode.flag_control_manual_enabled) {
/* move yaw setpoint */
float yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.mc_att_yaw_p;
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw);
if (yaw_offs < - yaw_offs_max) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - yaw_offs_max);
/* move yaw setpoint */
float yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.mc_att_yaw_p;
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw);
if (yaw_offs < - yaw_offs_max) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + yaw_offs_max);
}
} else if (yaw_offs > yaw_offs_max) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + yaw_offs_max);
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
_att_sp.yaw_sp_move_rate = yaw_sp_move_rate;
_att_sp.thrust = _control_mode.flag_control_altitude_enabled ? _att_sp.thrust : _manual.z;
}
/* reset yaw setpoint to current position if needed */
@@ -1371,10 +1378,6 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_body = _att.yaw;
}
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
_att_sp.thrust = _control_mode.flag_control_altitude_enabled ? _att_sp.thrust : _manual.z;
_att_sp.yaw_sp_move_rate = yaw_sp_move_rate;
math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));