diff --git a/appconf/appconf_custom.h b/appconf/appconf_custom.h
deleted file mode 100644
index 2752cda6..00000000
--- a/appconf/appconf_custom.h
+++ /dev/null
@@ -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 .
- */
-
-#ifndef APPCONF_APPCONF_EXAMPLE_PPM_H_
-#define APPCONF_APPCONF_EXAMPLE_PPM_H_
-
-// Use custom user application
-#define APPCONF_APP_TO_USE APP_CUSTOM
-
-#endif
diff --git a/utils_math.c b/utils_math.c
index 35ae4fec..1b454aa6 100644
--- a/utils_math.c
+++ b/utils_math.c
@@ -20,44 +20,10 @@
#include "utils_math.h"
#include "hal.h"
#include "app.h"
-#include
+
#include
#include
-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.
*
diff --git a/utils_math.h b/utils_math.h
index 5d1775e1..5ca15997 100644
--- a/utils_math.h
+++ b/utils_math.h
@@ -22,27 +22,18 @@
#include
#include
+#include
+
#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_ */