forked from vedderb/bldc
-
Notifications
You must be signed in to change notification settings - Fork 1
/
utils.h
91 lines (77 loc) · 3.38 KB
/
utils.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
/*
Copyright 2016 - 2019 Benjamin Vedder [email protected]
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 <http://www.gnu.org/licenses/>.
*/
#ifndef UTILS_H_
#define UTILS_H_
#include <stdbool.h>
#include <stdint.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);
void utils_norm_angle_rad(float *angle);
int utils_truncate_number(float *number, float min, float max);
int utils_truncate_number_int(int *number, int min, int max);
int 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);
float utils_max_abs(float va, float vb);
void utils_byte_to_binary(int x, char *b);
float utils_throttle_curve(float val, float curve_acc, float curve_brake, int mode);
void utils_sys_lock_cnt(void);
void utils_sys_unlock_cnt(void);
uint32_t utils_crc32c(uint8_t *data, uint32_t len);
// Return the sign of the argument. -1 if negative, 1 if zero or positive.
#define SIGN(x) ((x < 0) ? -1 : 1)
// Squared
#define SQ(x) ((x) * (x))
// Return the age of a timestamp in seconds
#define UTILS_AGE_S(x) ((float)chVTTimeElapsedSinceX(x) / (float)CH_CFG_ST_FREQUENCY)
// nan and infinity check for floats
#define UTILS_IS_INF(x) ((x) == (1.0 / 0.0) || (x) == (-1.0 / 0.0))
#define UTILS_IS_NAN(x) ((x) != (x))
#define UTILS_NAN_ZERO(x) (x = UTILS_IS_NAN(x) ? 0.0 : x)
/**
* A simple low pass filter.
*
* @param value
* The filtered value.
*
* @param sample
* Next sample.
*
* @param filter_constant
* Filter constant. Range 0.0 to 1.0, where 1.0 gives the unfiltered value.
*/
#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * (value - (sample)))
// Some constants
#define ONE_BY_SQRT3 (0.57735026919)
#define TWO_BY_SQRT3 (2.0f * 0.57735026919)
#define SQRT3_BY_2 (0.86602540378)
#define COS_30_DEG (0.86602540378)
#define SIN_30_DEG (0.5)
#define COS_MINUS_30_DEG (0.86602540378)
#define SIN_MINUS_30_DEG (-0.5)
#endif /* UTILS_H_ */