e839701094d98ea484ddef97cc95853f5049652c
[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / utility / Utilities.cpp
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;
107 float Utilities::clip(float a)
109     if (a > 400) {
110         a = 400;
111     }
112     if (a < -400) {
113         a = -400;
114     }
115     return a;
118 void Utilities::reInitErrorVals()
120     for (int i = 0; i < (sizeof(errorVals) / sizeof(float)); i++) {
121         errorVals[i] = 0;
122     }
125 int Utilities::min(int a, int b)
127     if (a < b) {
128         return a;
129     }
130     else {
131         return b;
132     }
135 int Utilities::max(int a, int b)
137     if (a > b) {
138         return a;
139     }
140     else {
141         return b;
142     }
145 float Utilities::toDegrees(float angle)
147     return float ((angle * 180.0f) / M_PI);