From ba6bb215607a4470df210ec2388298facba54beb Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@KrausAerospace.com>
Date: Thu, 22 Apr 2021 08:54:02 -0700
Subject: [PATCH] AP_Math: added helper for16bit float conversions

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

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index 0800e10f1b..ddc8b3f900 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -405,6 +405,18 @@ void fill_nanf(float *f, uint16_t count)
 }
 #endif
 
+// Convert 16-bit fixed-point to float
+float fixed2float(const uint16_t input, const uint8_t fractional_bits)
+{
+    return ((float)input / (float)(1U << fractional_bits));
+}
+
+// Convert float to 16-bit fixed-point
+uint16_t float2fixed(const float input, const uint8_t fractional_bits)
+{
+    return (uint16_t)(roundf(input * (1U << fractional_bits)));
+}
+
 /*
   calculate turn rate in deg/sec given a bank angle and airspeed for a
   fixed wing aircraft
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 3b142ad1e8..bd5ad2292a 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -297,6 +297,11 @@ float calc_lowpass_alpha_dt(float dt, float cutoff_freq);
 void fill_nanf(float *f, uint16_t count);
 #endif
 
+// from https://embeddedartistry.com/blog/2018/07/12/simple-fixed-point-conversion-in-c/
+// Convert to/from 16-bit fixed-point and float
+float fixed2float(const uint16_t input, const uint8_t fractional_bits = 8);
+uint16_t float2fixed(const float input, const uint8_t fractional_bits = 8);
+
 /*
   calculate turn rate in deg/sec given a bank angle and airspeed for a
   fixed wing aircraft