From a1e90f4aa7517094f48eafa0db6cb91e4b11475f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 24 Apr 2016 20:03:05 +0200
Subject: [PATCH] Better loiter handling
---
src/modules/navigator/navigator_main.cpp | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index fec9634893..e8cb02be48 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -418,6 +418,10 @@ Navigator::task_main()
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
+ rep->current.loiter_radius = get_loiter_radius();
+ rep->current.loiter_direction = 1;
+ rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+
// Go on and check which changes had been requested
if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4;
@@ -425,15 +429,11 @@ Navigator::task_main()
rep->current.yaw = NAN;
}
- if (PX4_ISFINITE(cmd.param5)) {
+ if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = cmd.param5 / (double)1e7;
- } else {
- rep->current.lat = get_global_position()->lat;
- }
-
- if (PX4_ISFINITE(cmd.param6)) {
rep->current.lon = cmd.param6 / (double)1e7;
} else {
+ rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
}
--
GitLab