From 5aa037a00815ad3934ee8d9908482340f7151ef3 Mon Sep 17 00:00:00 2001
From: xdwgood <1308455330@qq.com>
Date: Tue, 17 Dec 2019 08:55:06 +0800
Subject: [PATCH] enable WV function on land/rtl/loiter/takeoff

---
 src/modules/navigator/land.cpp           | 1 +
 src/modules/navigator/loiter.cpp         | 2 ++
 src/modules/navigator/navigator_mode.cpp | 2 --
 src/modules/navigator/rtl.cpp            | 2 ++
 src/modules/navigator/takeoff.cpp        | 1 +
 5 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp
index 7a17c8cd6bfd..a703edc483e4 100644
--- a/src/modules/navigator/land.cpp
+++ b/src/modules/navigator/land.cpp
@@ -61,6 +61,7 @@ Land::on_activation()
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
+	pos_sp_triplet->current.allow_weather_vane = true;
 
 	_navigator->set_can_loiter_at_sp(false);
 
diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index ba14e09016fa..b62cf6630b19 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -106,6 +106,7 @@ Loiter::set_loiter_position()
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->current.velocity_valid = false;
+	pos_sp_triplet->current.allow_weather_vane = true;
 	pos_sp_triplet->previous.valid = false;
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -131,6 +132,7 @@ Loiter::reposition()
 		// convert mission item to current setpoint
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->current.velocity_valid = false;
+		pos_sp_triplet->current.allow_weather_vane = true;
 		pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
 		pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 6a22426075e4..b89af938da63 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -91,8 +91,6 @@ NavigatorMode::on_inactivation()
 void
 NavigatorMode::on_activation()
 {
-	/* invalidate position setpoint by default */
-	_navigator->get_position_setpoint_triplet()->current.valid = false;
 }
 
 void
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c58111d08cc8..b2a308e64931 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -257,6 +257,8 @@ RTL::set_rtl_item()
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
+	pos_sp_triplet->current.allow_weather_vane = true;
+
 	// Check if we are pretty close to the destination already.
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
 
diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp
index fa0220674b04..ca7ea0515202 100644
--- a/src/modules/navigator/takeoff.cpp
+++ b/src/modules/navigator/takeoff.cpp
@@ -129,6 +129,7 @@ Takeoff::set_takeoff_position()
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.allow_weather_vane = true;
 	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;