From 4b70584cf483c1db941d9e2bb6f3b7099dbb47c0 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@yuneecresearch.com>
Date: Wed, 12 Sep 2018 15:31:01 +0200
Subject: [PATCH] mc_pos_control takeoff: control velocity if velocity
estimation is available, otherwise just keep thrust vector pointing upward
---
src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++++-
1 file changed, 8 insertions(+), 1 deletion(-)
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 54ea44e050..26a9bffd19 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -684,7 +684,14 @@ MulticopterPositionControl::run()
// don't control position in xy
setpoint.x = setpoint.y = NAN;
setpoint.vx = setpoint.vy = NAN;
- setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; // just keeping pointing upwards
+ setpoint.thrust[0] = setpoint.thrust[1] = NAN;
+
+ if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) {
+ setpoint.vx = setpoint.vy = NAN; // try to keep zero velocity
+
+ } else {
+ setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; // just keeping pointing upwards
+ }
}
if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) {
--
GitLab