2014-01-09 09:02:38 -08:00
|
|
|
/*
|
2019-02-18 10:30:19 -08:00
|
|
|
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
|
2014-01-09 09:02:38 -08:00
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
This file is part of the VESC firmware.
|
|
|
|
|
|
|
|
The VESC firmware is free software: you can redistribute it and/or modify
|
2014-01-09 09:02:38 -08:00
|
|
|
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.
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
The VESC firmware is distributed in the hope that it will be useful,
|
2014-01-09 09:02:38 -08:00
|
|
|
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/>.
|
|
|
|
*/
|
|
|
|
|
2022-03-16 10:43:25 -07:00
|
|
|
#ifndef UTILS_MATH_H_
|
|
|
|
#define UTILS_MATH_H_
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
#include <stdbool.h>
|
2019-02-18 10:30:19 -08:00
|
|
|
#include <stdint.h>
|
2020-10-09 12:08:48 -07:00
|
|
|
#include "datatypes.h"
|
2015-12-08 12:01:23 -08:00
|
|
|
|
2014-07-27 10:40:38 -07:00
|
|
|
void utils_step_towards(float *value, float goal, float step);
|
2014-03-15 14:32:00 -07:00
|
|
|
float utils_calc_ratio(float low, float high, float val);
|
2014-03-13 07:28:56 -07:00
|
|
|
void utils_norm_angle(float *angle);
|
2022-03-30 15:21:16 -07:00
|
|
|
float utils_map_angle(float angle, float min, float max);
|
2015-12-08 12:01:23 -08:00
|
|
|
void utils_norm_angle_rad(float *angle);
|
2021-10-07 19:58:15 -07:00
|
|
|
bool utils_truncate_number(float *number, float min, float max);
|
|
|
|
bool utils_truncate_number_int(int *number, int min, int max);
|
|
|
|
bool utils_truncate_number_abs(float *number, float max);
|
2014-03-30 15:51:59 -07:00
|
|
|
float utils_map(float x, float in_min, float in_max, float out_min, float out_max);
|
2015-08-23 09:26:05 -07:00
|
|
|
int utils_map_int(int x, int in_min, int in_max, int out_min, int out_max);
|
2014-11-25 12:05:46 -08:00
|
|
|
void utils_deadband(float *value, float tres, float max);
|
2015-04-26 15:02:32 -07:00
|
|
|
float utils_angle_difference(float angle1, float angle2);
|
2015-12-08 12:01:23 -08:00
|
|
|
float utils_angle_difference_rad(float angle1, float angle2);
|
|
|
|
float utils_avg_angles_rad_fast(float *angles, float *weights, int angles_num);
|
2015-08-23 09:26:05 -07:00
|
|
|
float utils_middle_of_3(float a, float b, float c);
|
|
|
|
int utils_middle_of_3_int(int a, int b, int c);
|
2015-12-08 12:01:23 -08:00
|
|
|
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);
|
2016-11-04 07:18:34 -07:00
|
|
|
float utils_min_abs(float va, float vb);
|
2017-09-04 12:12:43 -07:00
|
|
|
float utils_max_abs(float va, float vb);
|
2016-11-04 07:18:34 -07:00
|
|
|
void utils_byte_to_binary(int x, char *b);
|
2017-09-06 12:13:28 -07:00
|
|
|
float utils_throttle_curve(float val, float curve_acc, float curve_brake, int mode);
|
2019-02-18 10:30:19 -08:00
|
|
|
uint32_t utils_crc32c(uint8_t *data, uint32_t len);
|
2020-01-28 10:46:19 -08:00
|
|
|
void utils_fft32_bin0(float *real_in, float *real, float *imag);
|
2020-01-20 00:39:33 -08:00
|
|
|
void utils_fft32_bin1(float *real_in, float *real, float *imag);
|
|
|
|
void utils_fft32_bin2(float *real_in, float *real, float *imag);
|
2020-01-28 10:46:19 -08:00
|
|
|
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);
|
2020-04-18 00:48:25 -07:00
|
|
|
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);
|
2022-02-28 05:29:59 -08:00
|
|
|
void utils_rotate_vector3(float *input, float *rotation, float *output, bool reverse);
|
2014-01-09 06:20:26 -08:00
|
|
|
|
2021-11-27 14:30:10 -08:00
|
|
|
// 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)
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
|
|
// Squared
|
|
|
|
#define SQ(x) ((x) * (x))
|
2015-02-19 12:20:07 -08:00
|
|
|
|
2022-03-07 08:33:51 -08:00
|
|
|
// Two-norm of 2D vector
|
|
|
|
//#define NORM2(x,y) (sqrt(SQ(x) + SQ(y)))
|
|
|
|
#define NORM2_f(x,y) (sqrtf(SQ(x) + SQ(y)))
|
|
|
|
|
2015-12-10 14:21:04 -08:00
|
|
|
// 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))
|
2017-10-20 11:06:06 -07:00
|
|
|
#define UTILS_NAN_ZERO(x) (x = UTILS_IS_NAN(x) ? 0.0 : x)
|
2015-12-10 14:21:04 -08:00
|
|
|
|
2021-10-13 08:26:19 -07:00
|
|
|
// Handy conversions for radians/degrees and RPM/radians-per-second
|
2021-10-13 09:13:51 -07:00
|
|
|
#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)))
|
|
|
|
|
2022-02-28 05:29:59 -08:00
|
|
|
#ifndef MIN
|
|
|
|
#define MIN(a,b) (((a)<(b))?(a):(b))
|
|
|
|
#endif
|
|
|
|
#ifndef MAX
|
|
|
|
#define MAX(a,b) (((a)>(b))?(a):(b))
|
|
|
|
#endif
|
2021-10-13 08:26:19 -07:00
|
|
|
|
2015-12-23 15:43:31 -08:00
|
|
|
/**
|
|
|
|
* 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.
|
|
|
|
*/
|
2020-01-12 12:25:21 -08:00
|
|
|
#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample)))
|
2015-12-23 15:43:31 -08:00
|
|
|
|
2021-10-31 05:40:43 -07:00
|
|
|
/**
|
|
|
|
* 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))
|
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
// Constants
|
2017-09-04 12:12:43 -07:00
|
|
|
#define ONE_BY_SQRT3 (0.57735026919)
|
|
|
|
#define TWO_BY_SQRT3 (2.0f * 0.57735026919)
|
|
|
|
#define SQRT3_BY_2 (0.86602540378)
|
2018-12-12 13:41:10 -08:00
|
|
|
#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)
|
2017-09-04 12:12:43 -07:00
|
|
|
|
2020-01-20 00:39:33 -08:00
|
|
|
// 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[];
|
|
|
|
|
2022-03-16 10:43:25 -07:00
|
|
|
#endif /* UTILS_MATH_H_ */
|