From 2a943f4bd09a604d0ed25d22eac1690de97966b0 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 7 Jun 2017 13:56:13 +0200 Subject: [PATCH] mc_pos_control: only set current_setpoint to true if triplet valid --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 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 5f618ff882..601f8368eb 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1392,9 +1392,6 @@ void MulticopterPositionControl::control_auto(float dt) _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &_curr_pos_sp.data[0], &_curr_pos_sp.data[1]); - } else { - _curr_pos_sp(0) = _pos(0); - _curr_pos_sp(1) = _pos(1); } //only project setpoints if they are finite, else use current position @@ -1555,6 +1552,7 @@ void MulticopterPositionControl::control_auto(float dt) _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } + /* * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl. * this makes the takeoff finish smoothly.