From e1c006c25d622a9f3f36267d050096aadb1e9d4e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 21 Jul 2022 11:26:09 +1000
Subject: [PATCH] AP_Math: added double_to_int32 and double_to_uint32

---
 libraries/AP_Math/AP_Math.cpp | 10 ++++++++++
 libraries/AP_Math/AP_Math.h   |  7 +++++++
 2 files changed, 17 insertions(+)

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index 98ac8c203a..d684eee755 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -510,3 +510,13 @@ uint32_t float_to_uint32(const float v)
 {
     return uint32_t(constrain_float(v, 0, UINT32_MAX));
 }
+
+uint32_t double_to_uint32(const double v)
+{
+    return uint32_t(constrain_double(v, 0, UINT32_MAX));
+}
+
+int32_t double_to_int32(const double v)
+{
+    return int32_t(constrain_double(v, INT32_MIN, UINT32_MAX));
+}
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 1f439f57c3..0abeac0157 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -204,6 +204,11 @@ inline uint64_t constrain_uint64(const uint64_t amt, const uint64_t low, const u
     return constrain_value(amt, low, high);
 }
 
+inline double constrain_double(const double amt, const double low, const double high)
+{
+    return constrain_value(amt, low, high);
+}
+
 // degrees -> radians
 static inline constexpr ftype radians(ftype deg)
 {
@@ -367,4 +372,6 @@ int16_t float_to_int16(const float v);
 uint16_t float_to_uint16(const float v);
 int32_t float_to_int32(const float v);
 uint32_t float_to_uint32(const float v);
+uint32_t double_to_uint32(const double v);
+int32_t double_to_int32(const double v);