e839701094d98ea484ddef97cc95853f5049652c
1 /*
2 * Copyright (c) 2014, Texas Instruments Incorporated
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 *
9 * * Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 *
12 * * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * * Neither the name of Texas Instruments Incorporated nor the names of
17 * its contributors may be used to endorse or promote products derived
18 * from this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
33 #include "Utilities.h"
34 #include <math.h>
36 #ifndef M_PI
37 #define M_PI 3.14159265358979323846
38 #endif
40 #define NUM_ERRORVALS 10
42 static float errorVals[NUM_ERRORVALS] = {0};
44 Utilities::Utilities()
45 {
46 reInitErrorVals();
47 }
49 float Utilities::wrapAngle(float angle)
50 {
51 angle = fmod(angle + 180, 360);
52 if (angle < 0) {
53 angle += 360;
54 }
55 return (angle - 180);
56 }
58 float Utilities::wrapAngle360(float angle)
59 {
60 angle = wrapAngle(angle);
61 if (angle < 0) {
62 angle += 360;
63 }
64 return angle;
65 }
67 float Utilities::saturate(float value, float min, float max)
68 {
69 if (value < min) {
70 value = min;
71 }
72 else if (value > max) {
73 value = max;
74 }
75 return value;
76 }
78 bool Utilities::inRange(float value, float lowerBound, float upperBound)
79 {
80 #if 0
81 float sum = 0;
82 for (int i = NUM_ERRORVALS - 1; i > 0; i--) {
83 errorVals[i] = errorVals[i - 1];
84 sum += errorVals[i];
85 }
86 sum += value;
87 errorVals[0] = value;
88 float avg = sum / NUM_ERRORVALS;
89 bool settled = (avg > lowerBound) && (avg < upperBound);
90 return (settled);
91 #else
92 return ((value > lowerBound) && (value < upperBound));
93 #endif
94 }
96 int Utilities::clip(int a)
97 {
98 if (a > 400) {
99 a = 400;
100 }
101 if (a < -400) {
102 a = -400;
103 }
104 return a;
105 }
107 float Utilities::clip(float a)
108 {
109 if (a > 400) {
110 a = 400;
111 }
112 if (a < -400) {
113 a = -400;
114 }
115 return a;
116 }
118 void Utilities::reInitErrorVals()
119 {
120 for (int i = 0; i < (sizeof(errorVals) / sizeof(float)); i++) {
121 errorVals[i] = 0;
122 }
123 }
125 int Utilities::min(int a, int b)
126 {
127 if (a < b) {
128 return a;
129 }
130 else {
131 return b;
132 }
133 }
135 int Utilities::max(int a, int b)
136 {
137 if (a > b) {
138 return a;
139 }
140 else {
141 return b;
142 }
143 }
145 float Utilities::toDegrees(float angle)
146 {
147 return float ((angle * 180.0f) / M_PI);
148 }