Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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
+8 -5
View File
@@ -1351,8 +1351,9 @@ MulticopterPositionControl::task_main()
} }
if(!_control_mode.flag_control_velocity_enabled) { if(!_control_mode.flag_control_velocity_enabled) {
/* generate attitude setpoint from manual controls */
/* generate attitude setpoint from manual controls */
if(_control_mode.flag_control_manual_enabled) {
/* move yaw setpoint */ /* move yaw setpoint */
float yaw_sp_move_rate = _manual.r * _params.man_yaw_max; 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); _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
@@ -1365,16 +1366,18 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_body = _wrap_pi(_att.yaw + 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 */ /* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) { if (reset_yaw_sp) {
reset_yaw_sp = false; reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw; _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; math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); 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)); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));