#98 Refactoring. Moved functions to num.c

bitcraze · Mar 10, 2016 · 3fb2649 · 3fb2649
1 parent 3158387
commit 3fb2649
Show file tree

Hide file tree

Showing 3 changed files with 59 additions and 46 deletions.
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
@@ -46,13 +46,9 @@
 #else
   #include "lps25h.h"
 #endif
+#include "num.h"
 
 
-#undef max
-#define max(a,b) ((a) > (b) ? (a) : (b))
-#undef min
-#define min(a,b) ((a) < (b) ? (a) : (b))
-
 /**
  * Defines in what divided update rate should the attitude
  * control loop run relative the rate control loop.
@@ -155,8 +151,6 @@ static void distributePower(const uint16_t thrust, const int16_t roll,
                             const int16_t pitch, const int16_t yaw);
 static uint16_t limitThrust(int32_t value);
 static void stabilizerTask(void* param);
-static float constrain(float value, const float minVal, const float maxVal);
-static float deadband(float value, const float threshold);
 
 void stabilizerInit(void)
 {
@@ -594,40 +588,7 @@ static void distributePower(const uint16_t thrust, const int16_t roll,
 
 static uint16_t limitThrust(int32_t value)
 {
-  if(value > UINT16_MAX)
-  {
-    value = UINT16_MAX;
-  }
-  else if(value < 0)
-  {
-    value = 0;
-  }
-
-  return (uint16_t)value;
-}
-
-// Constrain value between min and max
-static float constrain(float value, const float minVal, const float maxVal)
-{
-  return min(maxVal, max(minVal,value));
-}
-
-// Deadzone
-static float deadband(float value, const float threshold)
-{
-  if (fabs(value) < threshold)
-  {
-    value = 0;
-  }
-  else if (value > 0)
-  {
-    value -= threshold;
-  }
-  else if (value < 0)
-  {
-    value += threshold;
-  }
-  return value;
+  return limitUint16(value);
 }
 
 LOG_GROUP_START(ctrltarget)

diff --git a/src/utils/interface/num.h b/src/utils/interface/num.h
@@ -29,7 +29,17 @@
 
 #include <stdint.h>
 
+#undef max
+#define max(a,b) ((a) > (b) ? (a) : (b))
+#undef min
+#define min(a,b) ((a) < (b) ? (a) : (b))
+
+
 uint16_t single2half(float number);
 float half2single(uint16_t number);
 
+uint16_t limitUint16(int32_t value);
+float constrain(float value, const float minVal, const float maxVal);
+float deadband(float value, const float threshold);
+
 #endif
diff --git a/src/utils/src/num.c b/src/utils/src/num.c
@@ -24,7 +24,14 @@
  * num.c - 16bit floating point handling functions
  */
 
-/* To not use the GCC implementation, uint16_t is used to carry fp16 values
+#include <math.h>
+#include <stdint.h>
+
+#include "num.h"
+
+/* Half precision floating point **********************************************
+ *
+ * To not use the GCC implementation, uint16_t is used to carry fp16 values
  *
  * FP16 or Half precision floating points is specified by IEEE 754 as binary 16.
  * (float is specified as binary 32). This implementation is NOT GUARANTEED to
@@ -38,10 +45,6 @@
  *  * Faster and smaller than the GCC implementation 
  */
 
-#include "num.h"
-
-#include <stdint.h>
-
 uint16_t single2half(float number)
 {
     uint32_t num = *((uint32_t*)&number);
@@ -79,3 +82,42 @@ float half2single(uint16_t number)
 
     return *(float*)&fp32;
 }
+
+
+/*****************************************************************************/
+
+uint16_t limitUint16(int32_t value)
+{
+  if(value > UINT16_MAX)
+  {
+    value = UINT16_MAX;
+  }
+  else if(value < 0)
+  {
+    value = 0;
+  }
+
+  return (uint16_t)value;
+}
+
+float constrain(float value, const float minVal, const float maxVal)
+{
+  return min(maxVal, max(minVal,value));
+}
+
+float deadband(float value, const float threshold)
+{
+  if (fabs(value) < threshold)
+  {
+    value = 0;
+  }
+  else if (value > 0)
+  {
+    value -= threshold;
+  }
+  else if (value < 0)
+  {
+    value += threshold;
+  }
+  return value;
+}