From 10eb1a1b0c5897410d60b3bb805f193237800e7c Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 30 Mar 2017 17:01:16 +0200 Subject: [PATCH] mc_pos_control: if takeoff setpoint in auto mode, then do not enter ground contact --- .../mc_pos_control/mc_pos_control_main.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 66dc2bc13fab..55603ec3d0f1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -376,6 +376,8 @@ class MulticopterPositionControl : public control::SuperBlock float get_cruising_speed_xy(); + bool in_auto_takeoff(); + /** * limit altitude based on several conditions */ @@ -935,6 +937,17 @@ MulticopterPositionControl::limit_vel_xy_gradually() _vel_sp(1) = _vel_sp(1) / vel_mag_xy * vel_limit; } +bool +MulticopterPositionControl::in_auto_takeoff() +{ + /* + * in auto mode, check if we do a takeoff + */ + return (_pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) || + _control_mode.flag_control_offboard_enabled; +} + float MulticopterPositionControl::get_cruising_speed_xy() { @@ -1871,7 +1884,7 @@ MulticopterPositionControl::control_position(float dt) } /* 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) { + if (_vehicle_land_detected.ground_contact && !in_auto_takeoff()) { /* thrust setpoint in body frame*/ math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp; @@ -1920,11 +1933,7 @@ MulticopterPositionControl::control_position(float dt) // We can only run the control if we're already in-air, have a takeoff setpoint, // or if we're in offboard control. // Otherwise, we should just bail out - const bool got_takeoff_setpoint = (_pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) || - _control_mode.flag_control_offboard_enabled; - - if (_vehicle_land_detected.landed && !got_takeoff_setpoint) { + if (_vehicle_land_detected.landed && !in_auto_takeoff()) { // Keep throttle low while still on ground. thr_max = 0.0f;