From 4121cc464db9c3118b8b6171f49cf2e555dbf570 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 14 Oct 2022 07:30:19 +1100
Subject: [PATCH] AP_Common: added Float16_t type

---
 libraries/AP_Common/float16.cpp | 68 +++++++++++++++++++++++++++++++++
 libraries/AP_Common/float16.h   | 15 ++++++++
 2 files changed, 83 insertions(+)
 create mode 100644 libraries/AP_Common/float16.cpp
 create mode 100644 libraries/AP_Common/float16.h

diff --git a/libraries/AP_Common/float16.cpp b/libraries/AP_Common/float16.cpp
new file mode 100644
index 0000000000..e4ac982f48
--- /dev/null
+++ b/libraries/AP_Common/float16.cpp
@@ -0,0 +1,68 @@
+#include "float16.h"
+/*
+  float16 implementation
+
+  algorithm with thanks to libcanard:
+  https://github.com/dronecan/libcanard
+*/
+
+
+float Float16_t::get(void) const
+{
+    union FP32 {
+        uint32_t u;
+        float f;
+    };
+    const union FP32 magic = { (254UL - 15UL) << 23U };
+    const union FP32 was_inf_nan = { (127UL + 16UL) << 23U };
+    union FP32 out;
+
+    out.u = (v16 & 0x7FFFU) << 13U;
+    out.f *= magic.f;
+    if (out.f >= was_inf_nan.f) {
+        out.u |= 255UL << 23U;
+    }
+    out.u |= (v16 & 0x8000UL) << 16U;
+
+    return out.f;
+}
+
+void Float16_t::set(float value)
+{
+    union FP32
+    {
+        uint32_t u;
+        float f;
+    };
+
+    const union FP32 f32inf = { 255UL << 23U };
+    const union FP32 f16inf = { 31UL << 23U };
+    const union FP32 magic = { 15UL << 23U };
+    const uint32_t sign_mask = 0x80000000UL;
+    const uint32_t round_mask = 0xFFFFF000UL;
+
+    union FP32 in;
+    in.f = value;
+    uint32_t sign = in.u & sign_mask;
+    in.u ^= sign;
+
+    v16 = 0;
+
+    if (in.u >= f32inf.u)
+    {
+        v16 = (in.u > f32inf.u) ? (uint16_t)0x7FFFU : (uint16_t)0x7C00U;
+    }
+    else
+    {
+        in.u &= round_mask;
+        in.f *= magic.f;
+        in.u -= round_mask;
+        if (in.u > f16inf.u)
+        {
+            in.u = f16inf.u;
+        }
+        v16 = (uint16_t)(in.u >> 13U);
+    }
+
+    v16 |= (uint16_t)(sign >> 16U);
+}
diff --git a/libraries/AP_Common/float16.h b/libraries/AP_Common/float16.h
new file mode 100644
index 0000000000..898ca64b45
--- /dev/null
+++ b/libraries/AP_Common/float16.h
@@ -0,0 +1,15 @@
+/*
+  implement a Float16 type
+ */
+
+#pragma once
+#include <stdint.h>
+
+struct float16_s {
+    float get(void) const;
+    void set(float value);
+
+    uint16_t v16;
+};
+
+typedef struct float16_s Float16_t;