Inline some math functions for speed

appconf/appconf_custom.h

@@ -1,26 +0,0 @@
-/*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
-
-	This file is part of the VESC firmware.
-
-	The VESC firmware is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    The VESC firmware is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-    */
-
-#ifndef APPCONF_APPCONF_EXAMPLE_PPM_H_
-#define APPCONF_APPCONF_EXAMPLE_PPM_H_
-
-// Use custom user application
-#define APPCONF_APP_TO_USE				APP_CUSTOM
-
-#endif

utils_math.c

@@ -20,44 +20,10 @@
 #include "utils_math.h"
 #include "hal.h"
 #include "app.h"
-#include <math.h>
+
 #include <string.h>
 #include <stdlib.h>
 
-void utils_step_towards(float *value, float goal, float step) {
-    if (*value < goal) {
-        if ((*value + step) < goal) {
-            *value += step;
-        } else {
-            *value = goal;
-        }
-    } else if (*value > goal) {
-        if ((*value - step) > goal) {
-            *value -= step;
-        } else {
-            *value = goal;
-        }
-    }
-}
-
-float utils_calc_ratio(float low, float high, float val) {
-	return (val - low) / (high - low);
-}
-
-/**
- * Make sure that 0 <= angle < 360
- *
- * @param angle
- * The angle to normalize.
- */
-void utils_norm_angle(float *angle) {
-	*angle = fmodf(*angle, 360.0);
-
-	if (*angle < 0.0) {
-		*angle += 360.0;
-	}
-}
-
 /*
  * Map angle from 0 to 1 in the range min to max. If angle is
  * outside of the range it will be less truncated to the closest
@@ -86,75 +52,6 @@ float utils_map_angle(float angle, float min, float max) {
 	return res;
 }
 
-/**
- * Make sure that -pi <= angle < pi,
- *
- * TODO: Maybe use fmodf instead?
- *
- * @param angle
- * The angle to normalize in radians.
- * WARNING: Don't use too large angles.
- */
-void utils_norm_angle_rad(float *angle) {
-	while (*angle < -M_PI) {
-		*angle += 2.0 * M_PI;
-	}
-
-	while (*angle >=  M_PI) {
-		*angle -= 2.0 * M_PI;
-	}
-}
-
-bool utils_truncate_number(float *number, float min, float max) {
-	bool did_trunc = false;
-
-	if (*number > max) {
-		*number = max;
-		did_trunc = true;
-	} else if (*number < min) {
-		*number = min;
-		did_trunc = true;
-	}
-
-	return did_trunc;
-}
-
-bool utils_truncate_number_int(int *number, int min, int max) {
-	bool did_trunc = false;
-
-	if (*number > max) {
-		*number = max;
-		did_trunc = true;
-	} else if (*number < min) {
-		*number = min;
-		did_trunc = true;
-	}
-
-	return did_trunc;
-}
-
-bool utils_truncate_number_abs(float *number, float max) {
-	bool did_trunc = false;
-
-	if (*number > max) {
-		*number = max;
-		did_trunc = true;
-	} else if (*number < -max) {
-		*number = -max;
-		did_trunc = true;
-	}
-
-	return did_trunc;
-}
-
-float utils_map(float x, float in_min, float in_max, float out_min, float out_max) {
-	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
-int utils_map_int(int x, int in_min, int in_max, int out_min, int out_max) {
-	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
 /**
  * Truncate absolute values less than tres to zero. The value
  * tres will be mapped to 0 and the value max to max.
@@ -289,21 +186,6 @@ int utils_middle_of_3_int(int a, int b, int c) {
 	return middle;
 }
 
-// Fast inverse square-root
-// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-float utils_fast_inv_sqrt(float x) {
-	union {
-		float as_float;
-		long as_int;
-	} un;
-
-	float xhalf = 0.5f*x;
-	un.as_float = x;
-	un.as_int = 0x5f3759df - (un.as_int >> 1);
-	un.as_float = un.as_float * (1.5f - xhalf * un.as_float * un.as_float);
-	return un.as_float;
-}
-
 /**
  * Fast atan2
  *
@@ -341,40 +223,6 @@ float utils_fast_atan2(float y, float x) {
 	}
 }
 
-/**
- * Truncate the magnitude of a vector.
- *
- * @param x
- * The first component.
- *
- * @param y
- * The second component.
- *
- * @param max
- * The maximum magnitude.
- *
- * @return
- * True if saturation happened, false otherwise
- */
-bool utils_saturate_vector_2d(float *x, float *y, float max) {
-	bool retval = false;
-	float mag = NORM2_f(*x, *y);
-	max = fabsf(max);
-
-	if (mag < 1e-10) {
-		mag = 1e-10;
-	}
-
-	if (mag > max) {
-		const float f = max / mag;
-		*x *= f;
-		*y *= f;
-		retval = true;
-	}
-
-	return retval;
-}
-
 /**
  * Fast sine and cosine implementation.
  *

utils_math.h

@@ -22,27 +22,18 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
+
 #include "datatypes.h"
 
-void utils_step_towards(float *value, float goal, float step);
-float utils_calc_ratio(float low, float high, float val);
-void utils_norm_angle(float *angle);
 float utils_map_angle(float angle, float min, float max);
-void utils_norm_angle_rad(float *angle);
-bool utils_truncate_number(float *number, float min, float max);
-bool utils_truncate_number_int(int *number, int min, int max);
-bool utils_truncate_number_abs(float *number, float max);
-float utils_map(float x, float in_min, float in_max, float out_min, float out_max);
-int utils_map_int(int x, int in_min, int in_max, int out_min, int out_max);
 void utils_deadband(float *value, float tres, float max);
 float utils_angle_difference(float angle1, float angle2);
 float utils_angle_difference_rad(float angle1, float angle2);
 float utils_avg_angles_rad_fast(float *angles, float *weights, int angles_num);
 float utils_middle_of_3(float a, float b, float c);
 int utils_middle_of_3_int(int a, int b, int c);
-float utils_fast_inv_sqrt(float x);
 float utils_fast_atan2(float y, float x);
-bool utils_saturate_vector_2d(float *x, float *y, float max);
 void utils_fast_sincos(float angle, float *sin, float *cos);
 void utils_fast_sincos_better(float angle, float *sin, float *cos);
 float utils_min_abs(float va, float vb);
@@ -132,4 +123,113 @@ extern const float utils_tab_sin_32_2[];
 extern const float utils_tab_cos_32_1[];
 extern const float utils_tab_cos_32_2[];
 
+// Inline functions
+inline void utils_step_towards(float *value, float goal, float step) {
+    if (*value < goal) {
+        if ((*value + step) < goal) {
+            *value += step;
+        } else {
+            *value = goal;
+        }
+    } else if (*value > goal) {
+        if ((*value - step) > goal) {
+            *value -= step;
+        } else {
+            *value = goal;
+        }
+    }
+}
+
+/**
+ * Make sure that 0 <= angle < 360
+ *
+ * @param angle
+ * The angle to normalize.
+ */
+inline void utils_norm_angle(float *angle) {
+	*angle = fmodf(*angle, 360.0);
+
+	if (*angle < 0.0) {
+		*angle += 360.0;
+	}
+}
+
+/**
+ * Make sure that -pi <= angle < pi,
+ *
+ * @param angle
+ * The angle to normalize in radians.
+ * WARNING: Don't use too large angles.
+ */
+inline void utils_norm_angle_rad(float *angle) {
+	while (*angle < -M_PI) { *angle += 2.0 * M_PI; }
+	while (*angle >=  M_PI) { *angle -= 2.0 * M_PI; }
+}
+
+inline void utils_truncate_number(float *number, float min, float max) {
+	if (*number > max) {
+		*number = max;
+	} else if (*number < min) {
+		*number = min;
+	}
+}
+
+inline void utils_truncate_number_int(int *number, int min, int max) {
+	if (*number > max) {
+		*number = max;
+	} else if (*number < min) {
+		*number = min;
+	}
+}
+
+inline void utils_truncate_number_abs(float *number, float max) {
+	if (*number > max) {
+		*number = max;
+	} else if (*number < -max) {
+		*number = -max;
+	}
+}
+
+inline float utils_map(float x, float in_min, float in_max, float out_min, float out_max) {
+	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+inline int utils_map_int(int x, int in_min, int in_max, int out_min, int out_max) {
+	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+/**
+ * Truncate the magnitude of a vector.
+ *
+ * @param x
+ * The first component.
+ *
+ * @param y
+ * The second component.
+ *
+ * @param max
+ * The maximum magnitude.
+ *
+ * @return
+ * True if saturation happened, false otherwise
+ */
+inline bool utils_saturate_vector_2d(float *x, float *y, float max) {
+	bool retval = false;
+	float mag = NORM2_f(*x, *y);
+	max = fabsf(max);
+
+	if (mag < 1e-10) {
+		mag = 1e-10;
+	}
+
+	if (mag > max) {
+		const float f = max / mag;
+		*x *= f;
+		*y *= f;
+		retval = true;
+	}
+
+	return retval;
+}
+
 #endif  /* UTILS_MATH_H_ */