-
Notifications
You must be signed in to change notification settings - Fork 0
/
arm_inlines_angle.h
132 lines (104 loc) · 3 KB
/
arm_inlines_angle.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
/*!\file arm_inlines_angle.h
** \author SMFSW
** \copyright MIT (c) 2017-2024, SMFSW
** \brief Angle conversion inlines
*/
/****************************************************************/
#ifndef ARM_INLINES_ANGLE_H_
#define ARM_INLINES_ANGLE_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <math.h>
/****************************************************************/
#if !defined(M_SQRT2)
#define M_SQRT2 1.41421356237309504880 //!< sqrt(2) approximation constant
#endif
#if !defined(M_PI)
#define M_PI 3.14159265358979323846 //!< Pi approximation constant
#endif
#if !defined(M_TWOPI)
#define M_TWOPI (M_PI * 2.0) //!< 2*Pi approximation constant
#endif
#define M_PI_180 (M_PI / 180.0) //!< Pi/180 approximation constant
#define M_180_PI (180.0 / M_PI) //!< 180/Pi approximation constant
/*!\brief Radians normalization (normalize angle to single rotation)
** \param[in] rad - Radians to convert
** \return Converted angle
**/
__INLINE float RAD_NORM(const float rad)
{
const float val = (rad < 0.0f) ? M_TWOPI : -M_TWOPI;
float r = rad;
while (fabs(r) > M_TWOPI) { r += val; }
return r;
}
/*!\brief Degrees normalization (normalize angle to single rotation)
** \param[in] deg - Degrees to convert
** \return Converted angle
**/
__INLINE float DEG_NORM(const float deg)
{
const float val = (deg < 0.0f) ? 360.0f : -360.0f;
float d = deg;
while (fabs(d) > 360.0f) { d += val; }
return d;
}
#if !defined(ARDUINO)
/*!\brief Radians to degrees conversion
** \note On Arduino platform, RAD_TO_DEG is already defined as conversion value
** \param[in] rad - Radians to convert
** \return Converted angle
**/
__INLINE float RAD_TO_DEG(const float rad)
{
return RAD_NORM(rad) * M_180_PI;
}
/*!\brief Degrees to radians conversion
** \note On Arduino platform, DEG_TO_RAD is already defined as conversion value
** \param[in] deg - Degrees to convert
** \return Converted angle
**/
__INLINE float DEG_TO_RAD(const float deg)
{
return DEG_NORM(deg) * M_PI_180;
}
#endif
/*!\brief Radians to 0-1 scaled float conversion
** \param[in] rad - Radians to convert
** \return Converted angle
**/
__INLINE float RAD_TO_FLOAT(const float rad)
{
return RAD_NORM(rad) / M_TWOPI;
}
/*!\brief 0-1 scaled float to radians conversion
** \param[in] val - 0-1 scaled float to convert
** \return Converted angle
**/
__INLINE float FLOAT_TO_RAD(const float val)
{
return (val - (float) ((intCPU_t) val)) * M_TWOPI;
}
/*!\brief Degrees to 0-1 scaled float conversion
** \param[in] deg - Degrees to convert
** \return Converted angle
**/
__INLINE float DEG_TO_FLOAT(const float deg)
{
return DEG_NORM(deg) / 360.0f;
}
/*!\brief 0-1 scaled float to degrees conversion
** \param[in] val - 0-1 scaled float to convert
** \return Converted angle
**/
__INLINE float FLOAT_TO_DEG(const float val)
{
return (val - (float) ((intCPU_t) val)) * 360.0f;
}
/****************************************************************/
#ifdef __cplusplus
}
#endif
#endif /* ARM_INLINES_ANGLE_H_ */
/****************************************************************/