rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
bldc/utils_math.c

671 lines
15 KiB
C
Raw Blame History

/*
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
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/>.
*/
#include "utils_math.h"
#include "hal.h"
#include "app.h"
#include <string.h>
#include <stdlib.h>
/*
* Map angle from 0 to 1 in the range min to max. If angle is
* outside of the range it will be less truncated to the closest
* angle. Angle units: Degrees
*/
float utils_map_angle(float angle, float min, float max) {
if (max == min) {
return -1;
}
float range_pos = max - min;
utils_norm_angle(&range_pos);
float range_neg = min - max;
utils_norm_angle(&range_neg);
float margin = range_neg / 2.0;
angle -= min;
utils_norm_angle(&angle);
if (angle > (360 - margin)) {
angle -= 360.0;
}
float res = angle / range_pos;
utils_truncate_number(&res, 0.0, 1.0);
return res;
}
/**
* Truncate absolute values less than tres to zero. The value
* tres will be mapped to 0 and the value max to max.
*/
void utils_deadband(float *value, float tres, float max) {
if (fabsf(*value) < tres) {
*value = 0.0;
} else {
float k = max / (max - tres);
if (*value > 0.0) {
*value = k * *value + max * (1.0 - k);
} else {
*value = -(k * -*value + max * (1.0 - k));
}
}
}
/**
* Get the difference between two angles. Will always be between -180 and +180 degrees.
* @param angle1
* The first angle
* @param angle2
* The second angle
* @return
* The difference between the angles
*/
float utils_angle_difference(float angle1, float angle2) {
float difference = angle1 - angle2;
while (difference < -180.0) difference += 2.0 * 180.0;
while (difference > 180.0) difference -= 2.0 * 180.0;
return difference;
}
/**
* Get the difference between two angles. Will always be between -pi and +pi radians.
* @param angle1
* The first angle in radians
* @param angle2
* The second angle in radians
* @return
* The difference between the angles in radians
*/
float utils_angle_difference_rad(float angle1, float angle2) {
float difference = angle1 - angle2;
while (difference < -M_PI) difference += 2.0 * M_PI;
while (difference > M_PI) difference -= 2.0 * M_PI;
return difference;
}
/**
* Takes the average of a number of angles.
*
* @param angles
* The angles in radians.
*
* @param angles_num
* The number of angles.
*
* @param weights
* The weight of the summarized angles
*
* @return
* The average angle.
*/
float utils_avg_angles_rad_fast(float *angles, float *weights, int angles_num) {
float s_sum = 0.0;
float c_sum = 0.0;
for (int i = 0; i < angles_num; i++) {
float s, c;
utils_fast_sincos_better(angles[i], &s, &c);
s_sum += s * weights[i];
c_sum += c * weights[i];
}
return utils_fast_atan2(s_sum, c_sum);
}
/**
* Get the middle value of three values
*
* @param a
* First value
*
* @param b
* Second value
*
* @param c
* Third value
*
* @return
* The middle value
*/
float utils_middle_of_3(float a, float b, float c) {
float middle;
if ((a <= b) && (a <= c)) {
middle = (b <= c) ? b : c;
} else if ((b <= a) && (b <= c)) {
middle = (a <= c) ? a : c;
} else {
middle = (a <= b) ? a : b;
}
return middle;
}
/**
* Get the middle value of three values
*
* @param a
* First value
*
* @param b
* Second value
*
* @param c
* Third value
*
* @return
* The middle value
*/
int utils_middle_of_3_int(int a, int b, int c) {
int middle;
if ((a <= b) && (a <= c)) {
middle = (b <= c) ? b : c;
} else if ((b <= a) && (b <= c)) {
middle = (a <= c) ? a : c;
} else {
middle = (a <= b) ? a : b;
}
return middle;
}
/**
* Fast atan2
*
* See http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
*
* @param y
* y
*
* @param x
* x
*
* @return
* The angle in radians
*/
float utils_fast_atan2(float y, float x) {
float abs_y = fabsf(y) + 1e-20; // kludge to prevent 0/0 condition
float angle;
if (x >= 0) {
float r = (x - abs_y) / (x + abs_y);
float rsq = r * r;
angle = ((0.1963 * rsq) - 0.9817) * r + (M_PI / 4.0);
} else {
float r = (x + abs_y) / (abs_y - x);
float rsq = r * r;
angle = ((0.1963 * rsq) - 0.9817) * r + (3.0 * M_PI / 4.0);
}
UTILS_NAN_ZERO(angle);
if (y < 0) {
return(-angle);
} else {
return(angle);
}
}
/**
* Fast sine and cosine implementation.
*
* See http://lab.polygonal.de/?p=205
*
* @param angle
* The angle in radians
* WARNING: Don't use too large angles.
*
* @param sin
* A pointer to store the sine value.
*
* @param cos
* A pointer to store the cosine value.
*/
void utils_fast_sincos(float angle, float *sin, float *cos) {
//always wrap input angle to -PI..PI
while (angle < -M_PI) {
angle += 2.0 * M_PI;
}
while (angle > M_PI) {
angle -= 2.0 * M_PI;
}
// compute sine
if (angle < 0.0) {
*sin = 1.27323954 * angle + 0.405284735 * angle * angle;
} else {
*sin = 1.27323954 * angle - 0.405284735 * angle * angle;
}
// compute cosine: sin(x + PI/2) = cos(x)
angle += 0.5 * M_PI;
if (angle > M_PI) {
angle -= 2.0 * M_PI;
}
if (angle < 0.0) {
*cos = 1.27323954 * angle + 0.405284735 * angle * angle;
} else {
*cos = 1.27323954 * angle - 0.405284735 * angle * angle;
}
}
/**
* Fast sine and cosine implementation.
*
* See http://lab.polygonal.de/?p=205
*
* @param angle
* The angle in radians
* WARNING: Don't use too large angles.
*
* @param sin
* A pointer to store the sine value.
*
* @param cos
* A pointer to store the cosine value.
*/
void utils_fast_sincos_better(float angle, float *sin, float *cos) {
//always wrap input angle to -PI..PI
while (angle < -M_PI) {
angle += 2.0 * M_PI;
}
while (angle > M_PI) {
angle -= 2.0 * M_PI;
}
//compute sine
if (angle < 0.0) {
*sin = 1.27323954 * angle + 0.405284735 * angle * angle;
if (*sin < 0.0) {
*sin = 0.225 * (*sin * -*sin - *sin) + *sin;
} else {
*sin = 0.225 * (*sin * *sin - *sin) + *sin;
}
} else {
*sin = 1.27323954 * angle - 0.405284735 * angle * angle;
if (*sin < 0.0) {
*sin = 0.225 * (*sin * -*sin - *sin) + *sin;
} else {
*sin = 0.225 * (*sin * *sin - *sin) + *sin;
}
}
// compute cosine: sin(x + PI/2) = cos(x)
angle += 0.5 * M_PI;
if (angle > M_PI) {
angle -= 2.0 * M_PI;
}
if (angle < 0.0) {
*cos = 1.27323954 * angle + 0.405284735 * angle * angle;
if (*cos < 0.0) {
*cos = 0.225 * (*cos * -*cos - *cos) + *cos;
} else {
*cos = 0.225 * (*cos * *cos - *cos) + *cos;
}
} else {
*cos = 1.27323954 * angle - 0.405284735 * angle * angle;
if (*cos < 0.0) {
*cos = 0.225 * (*cos * -*cos - *cos) + *cos;
} else {
*cos = 0.225 * (*cos * *cos - *cos) + *cos;
}
}
}
/**
* Calculate the values with the lowest magnitude.
*
* @param va
* The first value.
*
* @param vb
* The second value.
*
* @return
* The value with the lowest magnitude.
*/
float utils_min_abs(float va, float vb) {
float res;
if (fabsf(va) < fabsf(vb)) {
res = va;
} else {
res = vb;
}
return res;
}
/**
* Calculate the values with the highest magnitude.
*
* @param va
* The first value.
*
* @param vb
* The second value.
*
* @return
* The value with the highest magnitude.
*/
float utils_max_abs(float va, float vb) {
float res;
if (fabsf(va) > fabsf(vb)) {
res = va;
} else {
res = vb;
}
return res;
}
/**
* Create string representation of the binary content of a byte
*
* @param x
* The byte.
*
* @param b
* Array to store the string representation in.
*/
void utils_byte_to_binary(int x, char *b) {
b[0] = '\0';
int z;
for (z = 128; z > 0; z >>= 1) {
strcat(b, ((x & z) == z) ? "1" : "0");
}
}
float utils_throttle_curve(float val, float curve_acc, float curve_brake, int mode) {
float ret = 0.0;
if (val < -1.0) {
val = -1.0;
}
if (val > 1.0) {
val = 1.0;
}
float val_a = fabsf(val);
float curve;
if (val >= 0.0) {
curve = curve_acc;
} else {
curve = curve_brake;
}
// See
// http://math.stackexchange.com/questions/297768/how-would-i-create-a-exponential-ramp-function-from-0-0-to-1-1-with-a-single-val
if (mode == 0) { // Exponential
if (curve >= 0.0) {
ret = 1.0 - powf(1.0 - val_a, 1.0 + curve);
} else {
ret = powf(val_a, 1.0 - curve);
}
} else if (mode == 1) { // Natural
if (fabsf(curve) < 1e-10) {
ret = val_a;
} else {
if (curve >= 0.0) {
ret = 1.0 - ((expf(curve * (1.0 - val_a)) - 1.0) / (expf(curve) - 1.0));
} else {
ret = (expf(-curve * val_a) - 1.0) / (expf(-curve) - 1.0);
}
}
} else if (mode == 2) { // Polynomial
if (curve >= 0.0) {
ret = 1.0 - ((1.0 - val_a) / (1.0 + curve * val_a));
} else {
ret = val_a / (1.0 - curve * (1.0 - val_a));
}
} else { // Linear
ret = val_a;
}
if (val < 0.0) {
ret = -ret;
}
return ret;
}
uint32_t utils_crc32c(uint8_t *data, uint32_t len) {
uint32_t crc = 0xFFFFFFFF;
for (uint32_t i = 0; i < len;i++) {
uint32_t byte = data[i];
crc = crc ^ byte;
for (int j = 7;j >= 0;j--) {
uint32_t mask = -(crc & 1);
crc = (crc >> 1) ^ (0x82F63B78 & mask);
}
}
return ~crc;
}
// Yes, this is only the average...
void utils_fft32_bin0(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 32;i++) {
*real += real_in[i];
}
*real /= 32.0;
}
void utils_fft32_bin1(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 32;i++) {
*real += real_in[i] * utils_tab_cos_32_1[i];
*imag -= real_in[i] * utils_tab_sin_32_1[i];
}
*real /= 32.0;
*imag /= 32.0;
}
void utils_fft32_bin2(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 32;i++) {
*real += real_in[i] * utils_tab_cos_32_2[i];
*imag -= real_in[i] * utils_tab_sin_32_2[i];
}
*real /= 32.0;
*imag /= 32.0;
}
void utils_fft16_bin0(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 16;i++) {
*real += real_in[i];
}
*real /= 16.0;
}
void utils_fft16_bin1(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 16;i++) {
*real += real_in[i] * utils_tab_cos_32_1[2 * i];
*imag -= real_in[i] * utils_tab_sin_32_1[2 * i];
}
*real /= 16.0;
*imag /= 16.0;
}
void utils_fft16_bin2(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 16;i++) {
*real += real_in[i] * utils_tab_cos_32_2[2 * i];
*imag -= real_in[i] * utils_tab_sin_32_2[2 * i];
}
*real /= 16.0;
*imag /= 16.0;
}
void utils_fft8_bin0(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 8;i++) {
*real += real_in[i];
}
*real /= 8.0;
}
void utils_fft8_bin1(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 8;i++) {
*real += real_in[i] * utils_tab_cos_32_1[4 * i];
*imag -= real_in[i] * utils_tab_sin_32_1[4 * i];
}
*real /= 8.0;
*imag /= 8.0;
}
void utils_fft8_bin2(float *real_in, float *real, float *imag) {
*real = 0.0;
*imag = 0.0;
for (int i = 0;i < 8;i++) {
*real += real_in[i] * utils_tab_cos_32_2[4 * i];
*imag -= real_in[i] * utils_tab_sin_32_2[4 * i];
}
*real /= 8.0;
*imag /= 8.0;
}
// A mapping of a samsung 30q cell for % remaining capacity vs. voltage from
// 4.2 to 3.2, note that the you lose 15% of the 3Ah rated capacity in this range
float utils_batt_liion_norm_v_to_capacity(float norm_v) {
// constants for polynomial fit of lithium ion battery
const float li_p[] = {
-2.979767, 5.487810, -3.501286, 1.675683, 0.317147};
utils_truncate_number(&norm_v,0.0,1.0);
float v2 = norm_v*norm_v;
float v3 = v2*norm_v;
float v4 = v3*norm_v;
float v5 = v4*norm_v;
float capacity = li_p[0] * v5 + li_p[1] * v4 + li_p[2] * v3 +
li_p[3] * v2 + li_p[4] * norm_v;
return capacity;
}
static int uint16_cmp_func (const void *a, const void *b) {
return (*(uint16_t*)a - *(uint16_t*)b);
}
uint16_t utils_median_filter_uint16_run(uint16_t *buffer,
unsigned int *buffer_index, unsigned int filter_len, uint16_t sample) {
buffer[(*buffer_index)++] = sample;
*buffer_index %= filter_len;
uint16_t buffer_sorted[filter_len]; // Assume we have enough stack space
memcpy(buffer_sorted, buffer, sizeof(uint16_t) * filter_len);
qsort(buffer_sorted, filter_len, sizeof(uint16_t), uint16_cmp_func);
return buffer_sorted[filter_len / 2];
}
void utils_rotate_vector3(float *input, float *rotation, float *output, bool reverse) {
float s1, c1, s2, c2, s3, c3;
if (rotation[2] != 0.0) {
s1 = sinf(rotation[2]);
c1 = cosf(rotation[2]);
} else {
s1 = 0.0;
c1 = 1.0;
}
if (rotation[1] != 0.0) {
s2 = sinf(rotation[1]);
c2 = cosf(rotation[1]);
} else {
s2 = 0.0;
c2 = 1.0;
}
if (rotation[0] != 0.0) {
s3 = sinf(rotation[0]);
c3 = cosf(rotation[0]);
} else {
s3 = 0.0;
c3 = 1.0;
}
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3;
float m31 = -s2; float m32 = c2 * s3; float m33 = c2 * c3;
if (reverse) {
output[0] = input[0] * m11 + input[1] * m21 + input[2] * m31;
output[1] = input[0] * m12 + input[1] * m22 + input[2] * m32;
output[2] = input[0] * m13 + input[1] * m23 + input[2] * m33;
} else {
output[0] = input[0] * m11 + input[1] * m12 + input[2] * m13;
output[1] = input[0] * m21 + input[1] * m22 + input[2] * m23;
output[2] = input[0] * m31 + input[1] * m32 + input[2] * m33;
}
}
const float utils_tab_sin_32_1[] = {
0.000000, 0.195090, 0.382683, 0.555570, 0.707107, 0.831470, 0.923880, 0.980785,
1.000000, 0.980785, 0.923880, 0.831470, 0.707107, 0.555570, 0.382683, 0.195090,
0.000000, -0.195090, -0.382683, -0.555570, -0.707107, -0.831470, -0.923880, -0.980785,
-1.000000, -0.980785, -0.923880, -0.831470, -0.707107, -0.555570, -0.382683, -0.195090};
const float utils_tab_sin_32_2[] = {
0.000000, 0.382683, 0.707107, 0.923880, 1.000000, 0.923880, 0.707107, 0.382683,
0.000000, -0.382683, -0.707107, -0.923880, -1.000000, -0.923880, -0.707107, -0.382683,
-0.000000, 0.382683, 0.707107, 0.923880, 1.000000, 0.923880, 0.707107, 0.382683,
0.000000, -0.382683, -0.707107, -0.923880, -1.000000, -0.923880, -0.707107, -0.382683};
const float utils_tab_cos_32_1[] = {
1.000000, 0.980785, 0.923880, 0.831470, 0.707107, 0.555570, 0.382683, 0.195090,
0.000000, -0.195090, -0.382683, -0.555570, -0.707107, -0.831470, -0.923880, -0.980785,
-1.000000, -0.980785, -0.923880, -0.831470, -0.707107, -0.555570, -0.382683, -0.195090,
-0.000000, 0.195090, 0.382683, 0.555570, 0.707107, 0.831470, 0.923880, 0.980785};
const float utils_tab_cos_32_2[] = {
1.000000, 0.923880, 0.707107, 0.382683, 0.000000, -0.382683, -0.707107, -0.923880,
-1.000000, -0.923880, -0.707107, -0.382683, -0.000000, 0.382683, 0.707107, 0.923880,
1.000000, 0.923880, 0.707107, 0.382683, 0.000000, -0.382683, -0.707107, -0.923880,
-1.000000, -0.923880, -0.707107, -0.382683, -0.000000, 0.382683, 0.707107, 0.923880};
Reference in New Issue View Git Blame Copy Permalink