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

mc_pos_control: consider landing if not auto takeoff and valid

Browse Source
This commit is contained in:
Dennis Mannhart
2017-06-08 13:40:14 +02:00
committed by Lorenz Meier
parent 4692ccf287
commit ec04577e3a
1 changed files with 20 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+20 -22
View File
@@ -1807,33 +1807,31 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
thrust_sp(1) = 0.0f;
}
/* if still or already on ground command zero xy velcoity and zero xy thrust_sp in body frame to consider uneven ground */
if (_vehicle_land_detected.ground_contact && !in_auto_takeoff()) {
if (!in_auto_takeoff) {
if (_vehicle_land_detected.ground_contact) {
/* if still or already on ground command zero xy thrust_sp in body
* frame to consider uneven ground */
/* thrust setpoint in body frame*/
math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
/* if still or already on ground command zero xy thrust_sp in body
* frame to consider uneven ground */
/* we dont want to make any correction in body x and y*/
thrust_sp_body(0) = 0.0f;
thrust_sp_body(1) = 0.0f;
/* thrust setpoint in body frame*/
math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
/* we dont want to make any correction in body x and y*/
thrust_sp_body(0) = 0.0f;
thrust_sp_body(1) = 0.0f;
/* convert back to local frame (NED) */
thrust_sp = _R * thrust_sp_body;
}
/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
/* convert back to local frame (NED) */
thrust_sp = _R * thrust_sp_body;
}
if (_vehicle_land_detected.maybe_landed
&& !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
/* we set thrust to zero
* this will help to decide if we are actually landed or not
*/
thrust_sp.zero();
if (_vehicle_land_detected.maybe_landed) {
/* we set thrust to zero
* this will help to decide if we are actually landed or not
*/
thrust_sp.zero();
}
}
if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) {