diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index a3b031aa5f..4b329e61cd 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -155,7 +155,7 @@ float degreesToRadians(int16_t degrees)
 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
     long int a = ((long int) destMax - (long int) destMin) * ((long int) x - (long int) srcMin);
     long int b = (long int) srcMax - (long int) srcMin;
-    return ((a / b) - (destMax - destMin)) + destMax;
+    return (a / b) + destMin;
 }
 
 // Normalize a vector
diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc
index 5092678775..5082bc6da3 100644
--- a/src/test/unit/maths_unittest.cc
+++ b/src/test/unit/maths_unittest.cc
@@ -31,6 +31,45 @@ extern "C" {
 #include "unittest_macros.h"
 #include "gtest/gtest.h"
 
+
+TEST(MathsUnittest, TestScaleRange)
+{
+    // Within bounds
+    EXPECT_EQ(scaleRange(0, 0, 10, 0, 100), 0);
+    EXPECT_EQ(scaleRange(10, 0, 10, 0, 100), 100);
+    EXPECT_EQ(scaleRange(0, 0, 100, 0, 10), 0);
+    EXPECT_EQ(scaleRange(100, 0, 100, 0, 10), 10);
+
+    // Scale up
+    EXPECT_EQ(scaleRange(1, 0, 10, 0, 100), 10);
+    EXPECT_EQ(scaleRange(2, 0, 10, 0, 100), 20);
+    EXPECT_EQ(scaleRange(5, 0, 10, 0, 100), 50);
+
+    // Scale down
+    EXPECT_EQ(scaleRange(10, 0, 100, 0, 10), 1);
+    EXPECT_EQ(scaleRange(20, 0, 100, 0, 10), 2);
+    EXPECT_EQ(scaleRange(50, 0, 100, 0, 10), 5);
+}
+
+TEST(MathsUnittest, TestScaleRangeNegatives)
+{
+    // Within bounds
+    EXPECT_EQ(scaleRange(0, -10, 0, -100, 0), 0);
+    EXPECT_EQ(scaleRange(-10, -10, 0, -100, 0), -100);
+    EXPECT_EQ(scaleRange(0, -100, 0, -10, 0), 0);
+    EXPECT_EQ(scaleRange(-100, -100, 0, -10, 0), -10);
+
+    // Scale up
+    EXPECT_EQ(scaleRange(-1, -10, 0, -100, 0), -10);
+    EXPECT_EQ(scaleRange(-2, -10, 0, -100, 0), -20);
+    EXPECT_EQ(scaleRange(-5, -10, 0, -100, 0), -50);
+
+    // Scale down
+    EXPECT_EQ(scaleRange(-10, -100, 0, -10, 0), -1);
+    EXPECT_EQ(scaleRange(-20, -100, 0, -10, 0), -2);
+    EXPECT_EQ(scaleRange(-50, -100, 0, -10, 0), -5);
+}
+
 TEST(MathsUnittest, TestConstrain)
 {
     // Within bounds