From 288667a08d5fb1dbbd0d4a324a8cd80c05f69941 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Mon, 19 Dec 2022 18:36:40 +0300
Subject: [PATCH] Allow speed be nan

---
 clover/src/simple_offboard.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp
index 7557fd02..2dd70f7e 100644
--- a/clover/src/simple_offboard.cpp
+++ b/clover/src/simple_offboard.cpp
@@ -691,6 +691,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		ENSURE_NON_INF(y);
 		ENSURE_NON_INF(z);
 		ENSURE_NON_INF(yaw_rate);
+		ENSURE_NON_INF(speed); // TODO: allow inf
 
 		if (sp_type == NAVIGATE_GLOBAL) {
 			ENSURE_FINITE(lat);
@@ -784,7 +785,11 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			} else {
 				nav_start = local_position;
 			}
-			nav_speed = speed;
+
+			if (!isnan(speed)) {
+				nav_speed = speed;
+			}
+
 			nav_from_sp_flag = true;
 		}