| ISF 2.1 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
approximations.c
Go to the documentation of this file.
1 // Copyright (c) 2014, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file provides replacement trig functions which operate at reduced power,
27 // while still maintaining sufficient accuracy for sensor fusion.
28
29
30 #include "math.h"
31 #include "basic_types.h"
32 #include "approximations.h"
33
34 // function returns an approximation to angle(deg)=asin(x) for x in the range -1 <= x <= 1
35 // and returns -90 <= angle <= 90 deg
36 // maximum error is 10.29E-6 deg
37 float fasin_deg(float x)
38 {
39 // for robustness, check for invalid argument
40 if (x >= 1.0F) return 90.0F;
41 if (x <= -1.0F) return -90.0F;
42
43 // call the atan which will return an angle in the correct range -90 to 90 deg
44 // this line cannot fail from division by zero or negative square root since |x| < 1
45 return (fatan_deg(x / sqrtf(1.0F - x * x)));
46 }
47
48 // function returns an approximation to angle(deg)=acos(x) for x in the range -1 <= x <= 1
49 // and returns 0 <= angle <= 180 deg
50 // maximum error is 14.67E-6 deg
51 float facos_deg(float x)
52 {
53 // for robustness, check for invalid arguments
54 if (x >= 1.0F) return 0.0F;
55 if (x <= -1.0F) return 180.0F;
56
57 // call the atan which will return an angle in the incorrect range -90 to 90 deg
58 // these lines cannot fail from division by zero or negative square root
59 if (x == 0.0F) return 90.0F;
60 if (x > 0.0F) return fatan_deg((sqrtf(1.0F - x * x) / x));
61 return 180.0F + fatan_deg((sqrtf(1.0F - x * x) / x));
62 }
63
64 // function returns angle in range -90 to 90 deg
65 // maximum error is 9.84E-6 deg
66 float fatan_deg(float x)
67 {
68 float fangledeg; // compute computed (deg)
69 int8 ixisnegative; // argument x is negative
70 int8 ixexceeds1; // argument x is greater than 1.0
71 int8 ixmapped; // argument in range tan(15 deg) to tan(45 deg)=1.0
72
73 #define TAN15DEG 0.26794919243F // tan(15 deg) = 2 - sqrt(3)
74 #define TAN30DEG 0.57735026919F // tan(30 deg) = 1/sqrt(3)
75
76 // reset all flags
77 ixisnegative = ixexceeds1 = ixmapped = 0;
78
79 // test for negative argument to allow use of tan(-x)=-tan(x)
80 if (x < 0.0F)
81 {
82 x = -x;
83 ixisnegative = 1;
84 }
85
86 // test for argument above 1 to allow use of atan(x)=pi/2-atan(1/x)
87 if (x > 1.0F)
88 {
89 x = 1.0F / x;
90 ixexceeds1 = 1;
91 }
92
93 // at this point, x is in the range 0 to 1 inclusive
94 // map argument onto range -tan(15 deg) to tan(15 deg)
95 // using tan(angle-30deg) = (tan(angle)-tan(30deg)) / (1 + tan(angle)tan(30deg))
96 // tan(15deg) maps to tan(-15 deg) = -tan(15 deg)
97 // 1. maps to (sqrt(3) - 1) / (sqrt(3) + 1) = 2 - sqrt(3) = tan(15 deg)
98 if (x > TAN15DEG)
99 {
100 x = (x - TAN30DEG)/(1.0F + TAN30DEG * x);
101 ixmapped = 1;
102 }
103
104 // call the atan estimator to obtain -15 deg <= angle <= 15 deg
105 fangledeg = fatan_15deg(x);
106
107 // undo the distortions applied earlier to obtain -90 deg <= angle <= 90 deg
108 if (ixmapped) fangledeg += 30.0F;
109 if (ixexceeds1) fangledeg = 90.0F - fangledeg;
110 if (ixisnegative) fangledeg = -fangledeg;
111
112 return (fangledeg);
113 }
114
115 // function returns approximate atan2 angle in range -180 to 180 deg
116 // maximum error is 14.58E-6 deg
117 float fatan2_deg(float y, float x)
118 {
119 // check for zero x to avoid division by zero
120 if (x == 0.0F)
121 {
122 // return 90 deg for positive y
123 if (y > 0.0F) return 90.0F;
124 // return -90 deg for negative y
125 if (y < 0.0F) return -90.0F;
126 // otherwise y= 0.0 and return 0 deg (invalid arguments)
127 return 0.0F;
128 }
129
130 // from here onwards, x is guaranteed to be non-zero
131 // compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg)
132 if (x > 0.0F) return (fatan_deg(y / x));
133 // compute atan2 for quadrant 2 (90 to 180 deg)
134 if ((x < 0.0F) && (y > 0.0F)) return (180.0F + fatan_deg(y / x));
135 // compute atan2 for quadrant 3 (-180 to -90 deg)
136 return (-180.0F + fatan_deg(y / x));
137
138 }
139
140 // approximation to inverse tan function (deg) for x in range
141 // -tan(15 deg) to tan(15 deg) giving an output -15 deg <= angle <= 15 deg
142 // using modified Pade[3/2] approximation
143 float fatan_15deg(float x)
144 {
145 float x2; // x^2
146
147 #define PADE_A 96.644395816F // theoretical Pade[3/2] value is 5/3*180/PI=95.49296
148 #define PADE_B 25.086941612F // theoretical Pade[3/2] value is 4/9*180/PI=25.46479
149 #define PADE_C 1.6867633134F // theoretical Pade[3/2] value is 5/3=1.66667
150
151 // compute the approximation to the inverse tangent
152 // the function is anti-symmetric as required for positive and negative arguments
153 x2 = x * x;
154 return (x * (PADE_A + x2 * PADE_B) / (PADE_C + x2));
155 }
fatan_deg
float fatan_deg(float x)
Definition: approximations.c:66
fasin_deg
float fasin_deg(float x)
Definition: approximations.c:37
int8
signed char int8
Definition: basic_types.h:12
basic_types.h
TAN30DEG
#define TAN30DEG
fatan_15deg
float fatan_15deg(float x)
Definition: approximations.c:143
approximations.h
PADE_C
#define PADE_C
TAN15DEG
#define TAN15DEG
PADE_A
#define PADE_A
PADE_B
#define PADE_B
facos_deg
float facos_deg(float x)
Definition: approximations.c:51
fatan2_deg
float fatan2_deg(float y, float x)
Definition: approximations.c:117
- © Freescale Semiconductor, Inc. 2015. All Rights Reserved.