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

mc_pos_control: smooth takeoff requires current position as reference

Browse Source
This commit is contained in:
Dennis Mannhart
2018-08-28 10:14:54 +02:00
committed by Dennis Mannhart
parent 6142e2c4b8
commit 3ff8cd33a1
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -1
View File
@@ -122,6 +122,7 @@ private:
int _task_failure_count{0}; /**< counter for task failures */ int _task_failure_count{0}; /**< counter for task failures */
float _takeoff_speed = -1.f; /**< For flighttask interface used only. It can be thrust or velocity setpoints */ float _takeoff_speed = -1.f; /**< For flighttask interface used only. It can be thrust or velocity setpoints */
float _takeoff_reference_z; /**< Z-position when takeoff was initiated */
vehicle_status_s _vehicle_status{}; /**< vehicle status */ vehicle_status_s _vehicle_status{}; /**< vehicle status */
vehicle_land_detected_s _vehicle_land_detected{}; /**< vehicle land detected */ vehicle_land_detected_s _vehicle_land_detected{}; /**< vehicle land detected */
@@ -899,6 +900,7 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl
// takeoff speed. Enable smooth takeoff. // takeoff speed. Enable smooth takeoff.
_in_smooth_takeoff = true; _in_smooth_takeoff = true;
_takeoff_speed = -0.5f; _takeoff_speed = -0.5f;
_takeoff_reference_z = _states.position(2);
} else { } else {
// Default // Default
@@ -927,7 +929,7 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float
// Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached.
if (PX4_ISFINITE(z_sp)) { if (PX4_ISFINITE(z_sp)) {
_in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, -MPC_LAND_ALT2.get()); _in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - MPC_LAND_ALT2.get());
} else { } else {
_in_smooth_takeoff = _takeoff_speed < -vz_sp; _in_smooth_takeoff = _takeoff_speed < -vz_sp;