From 22ac8240a2a00005efb682af9ab76721c22e1480 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andis=20Z=C4=ABle?= <andis.jargans@gmail.com>
Date: Fri, 12 Apr 2024 11:51:10 +0300
Subject: [PATCH] Added absolute subtract

---
 firmware/src/utils/utils.cpp | 20 +++++++++++++++++++-
 firmware/src/utils/utils.h   |  4 ++++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/firmware/src/utils/utils.cpp b/firmware/src/utils/utils.cpp
index fae8eda..cc6dbe5 100644
--- a/firmware/src/utils/utils.cpp
+++ b/firmware/src/utils/utils.cpp
@@ -57,7 +57,6 @@ uint16_t util::sat_subtract(uint16_t x, uint16_t y)
 	if(z > x) return 0;
 	else return z;
 }
-
 uint32_t util::sat_subtract(uint32_t x, uint32_t y)
 {
 	uint32_t z = x - y;
@@ -66,6 +65,25 @@ uint32_t util::sat_subtract(uint32_t x, uint32_t y)
 	else return z;
 }
 
+uint8_t util::abs_subtract(uint8_t x, uint8_t y)
+{
+	if(x > y) return x - y;
+	else return y-x;
+}
+
+uint16_t util::abs_subtract(uint16_t x, uint16_t y)
+{
+	if(x > y) return x - y;
+	else return y-x;
+}
+
+uint32_t util::abs_subtract(uint32_t x, uint32_t y)
+{
+	if(x > y) return x - y;
+	else return y-x;
+}
+
+
 uint16_t util::sat_cast(uint32_t x)
 {
 	if(x > 0x0000FFFF) return 0xFFFF;
diff --git a/firmware/src/utils/utils.h b/firmware/src/utils/utils.h
index d411a15..1e45bfe 100644
--- a/firmware/src/utils/utils.h
+++ b/firmware/src/utils/utils.h
@@ -27,6 +27,10 @@ uint8_t sat_subtract(uint8_t x, uint8_t y);
 uint16_t sat_subtract(uint16_t x, uint16_t y);
 uint32_t sat_subtract(uint32_t x, uint32_t y);
 
+uint8_t abs_subtract(uint8_t x, uint8_t y);
+uint16_t abs_subtract(uint16_t x, uint16_t y);
+uint32_t abs_subtract(uint32_t x, uint32_t y);
+
 uint16_t interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
 uint16_t interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);