From c84c7cdcb4429e6c21162a8a424daaf5f201e264 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Wed, 21 Jun 2017 15:49:24 +0200
Subject: [PATCH] mc_pos_control: set previous triplet point to invalid when
switching to manual
---
.../mc_pos_control/mc_pos_control_main.cpp | 17 ++++++++++++-----
1 file changed, 12 insertions(+), 5 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 1aae791d1f..02292f0334 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -753,13 +753,19 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
- //set current position setpoint invalid if none of them (lat, lon and alt) is finite
- if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) &&
- !PX4_ISFINITE(_pos_sp_triplet.current.lon) &&
- !PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
- _pos_sp_triplet.current.valid = false;
+ /* we need either a valid position setpoint or a valid velocity setpoint */
+ _pos_sp_triplet.current.valid = false;
+ _pos_sp_triplet.previous.valid = false;
+
+ if (PX4_ISFINITE(_pos_sp_triplet.current.lat) && PX4_ISFINITE(_pos_sp_triplet.current.lon)
+ && PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
+ _pos_sp_triplet.current.valid = true;
}
+ if (PX4_ISFINITE(_pos_sp_triplet.previous.lat) && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
+ && PX4_ISFINITE(_pos_sp_triplet.previous.alt)) {
+ _pos_sp_triplet.previous.valid = true;
+ }
}
orb_check(_home_pos_sub, &updated);
@@ -1999,6 +2005,7 @@ MulticopterPositionControl::do_control(float dt)
* controller will not use the old triplets but waits until triplets
* have been updated */
_pos_sp_triplet.current.valid = false;
+ _pos_sp_triplet.previous.valid = false;
_hold_offboard_xy = false;
_hold_offboard_z = false;
--
GitLab