mirror of https://github.com/rusefi/bldc.git
76 lines
1.7 KiB
C
76 lines
1.7 KiB
C
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include <time.h>
|
|
|
|
#include "utils_math.h"
|
|
|
|
static float interpolate_angles_rad(float a1, float a2, float weight_a1) {
|
|
float weight_a2 = 1.0 - weight_a1;
|
|
|
|
float sin_a1 = sinf(a1);
|
|
float cos_a1 = cosf(a1);
|
|
float sin_a2 = sinf(a2);
|
|
float cos_a2 = cosf(a2);
|
|
|
|
float sin_mapped = sin_a1 * weight_a1 + sin_a2 * weight_a2;
|
|
float cos_mapped = cos_a1 * weight_a1 + cos_a2 * weight_a2;
|
|
return atan2f(sin_mapped, cos_mapped);
|
|
}
|
|
|
|
// Range 0.0 - 1.0
|
|
float rand01 (void) {
|
|
return ((float)rand() / (float)RAND_MAX);
|
|
}
|
|
|
|
// Range -1.0 to 1.0
|
|
float rand11 (void) {
|
|
return 2.0 * rand01() - 1.0;
|
|
}
|
|
|
|
int main(void) {
|
|
srand(time(NULL));
|
|
|
|
int testnum = 100000;
|
|
|
|
bool ok = true;
|
|
for (int i = 0;i < testnum;i++) {
|
|
float a1 = rand11() * 370.0;
|
|
float a1_tmp = a1;
|
|
utils_norm_angle(&a1_tmp);
|
|
if (a1_tmp > 180.0) {
|
|
a1_tmp -= 360.0;
|
|
}
|
|
float a2 = a1_tmp + rand11() * 60.0;
|
|
float weight = rand01();
|
|
|
|
|
|
float a_correct = interpolate_angles_rad(DEG2RAD_f(a1), DEG2RAD_f(a2), weight);
|
|
float a_util = utils_interpolate_angles_rad(DEG2RAD_f(a1), DEG2RAD_f(a2), weight);
|
|
|
|
float error = a_correct - a_util;
|
|
utils_norm_angle_rad(&error);
|
|
|
|
a_correct = RAD2DEG_f(a_correct);
|
|
a_util = RAD2DEG_f(a_util);
|
|
error = RAD2DEG_f(error);
|
|
|
|
// Allow for 1.5 degrees difference as interpolation between sin/cos is not the same as
|
|
// interpolation between the angles.
|
|
if (fabsf(error) > 1.5) {
|
|
ok = false;
|
|
printf("Error occurred after %d tests\r\n", i + 1);
|
|
printf("a1: %.1f, a2: %.1f, weight_a1: %.1f => Correct: %.1f Util: %.1f Error: %.1f\r\n",
|
|
a1, a2, weight, a_correct, a_util, error);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (ok) {
|
|
printf("All %d tests passed!\r\n", testnum);
|
|
}
|
|
|
|
return 0;
|
|
}
|