forked from surfdado/bldc
-
Notifications
You must be signed in to change notification settings - Fork 0
/
utils.h
132 lines (112 loc) · 5.24 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
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
/*
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>
#include "datatypes.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);
void utils_fft32_bin0(float *real_in, float *real, float *imag);
void utils_fft32_bin1(float *real_in, float *real, float *imag);
void utils_fft32_bin2(float *real_in, float *real, float *imag);
void utils_fft16_bin0(float *real_in, float *real, float *imag);
void utils_fft16_bin1(float *real_in, float *real, float *imag);
void utils_fft16_bin2(float *real_in, float *real, float *imag);
void utils_fft8_bin0(float *real_in, float *real, float *imag);
void utils_fft8_bin1(float *real_in, float *real, float *imag);
void utils_fft8_bin2(float *real_in, float *real, float *imag);
uint8_t utils_second_motor_id(void);
int utils_read_hall(bool is_second_motor, int samples);
float utils_batt_liion_norm_v_to_capacity(float norm_v);
uint16_t utils_median_filter_uint16_run(uint16_t *buffer,
unsigned int *buffer_index, unsigned int filter_len, uint16_t sample);
const char* utils_hw_type_to_string(HW_TYPE hw);
// Return the sign of the argument. -1.0 if negative, 1.0 if zero or positive.
#define SIGN(x) (((x) < 0.0) ? -1.0 : 1.0)
// 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)
// Handy conversions for radians/degrees and RPM/radians-per-second
#define DEG2RAD_f(deg) ((deg) * (float)(M_PI / 180.0))
#define RAD2DEG_f(rad) ((rad) * (float)(180.0 / M_PI))
#define RPM2RADPS_f(rpm) ((rpm) * (float)((2.0 * M_PI) / 60.0))
#define RADPS2RPM_f(rad_per_sec) ((rad_per_sec) * (float)(60.0 / (2.0 * M_PI)))
/**
* 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)))
/**
* A fast approximation of a moving average filter with N samples. See
* https://en.wikipedia.org/wiki/Moving_average#Exponential_moving_average
* https://en.wikipedia.org/wiki/Exponential_smoothing
*
* It is not entirely the same as it behaves like an IIR filter rather than a FIR filter, but takes
* much less memory and is much faster to run.
*/
#define UTILS_LP_MOVING_AVG_APPROX(value, sample, N) UTILS_LP_FAST(value, sample, 2.0 / ((N) + 1.0))
// 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)
// Tables
extern const float utils_tab_sin_32_1[];
extern const float utils_tab_sin_32_2[];
extern const float utils_tab_cos_32_1[];
extern const float utils_tab_cos_32_2[];
#endif /* UTILS_H_ */