Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Failsafe landing without position control fixed

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-25 22:26:51 +02:00
parent 0c1de81785
commit 2453b354fa
2 changed files with 17 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+5 -2
View File
@@ -1747,8 +1747,11 @@ set_control_mode()
control_mode.flag_control_auto_enabled = true; control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true; control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true; /* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_climb_rate_enabled = true;
} }
src/modules/mc_pos_control/mc_pos_control_main.cpp
+12
View File
@@ -1001,6 +1001,18 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = euler(0); _att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1); _att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
} }
_att_sp.thrust = thrust_abs; _att_sp.thrust = thrust_abs;