RMUL2025/User/component/mixer.c

95 lines
2.0 KiB
C
Raw Normal View History

2025-03-11 21:32:41 +08:00
/*
*/
#include "mixer.h"
#include "math.h"
/**
* @brief
*
* @param mixer
* @param mode
* @return int8_t 0
*/
int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode) {
if (mixer == NULL) return -1;
mixer->mode = mode;
return 0;
}
/**
* @brief
*
* @param mixer
* @param move_vec
* @param out
* @param len
* @param scale
* @return int8_t 0
*/
int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
int8_t len, float scale) {
if (mixer == NULL) return -1;
switch (mixer->mode) {
case MIXER_MECANUM:
if (len == 4) {
out[0] = move_vec->vx - move_vec->vy + move_vec->wz;
out[1] = move_vec->vx + move_vec->vy + move_vec->wz;
out[2] = -move_vec->vx + move_vec->vy + move_vec->wz;
out[3] = -move_vec->vx - move_vec->vy + move_vec->wz;
} else {
goto error;
}
break;
case MIXER_PARLFIX4:
if (len == 4) {
out[0] = -move_vec->vx;
out[1] = move_vec->vx;
out[2] = move_vec->vx;
out[3] = -move_vec->vx;
} else {
goto error;
}
case MIXER_PARLFIX2:
if (len == 2) {
out[0] = -move_vec->vx;
out[1] = move_vec->vx;
} else {
goto error;
}
case MIXER_SINGLE:
if (len == 1) {
out[0] = move_vec->vx;
} else {
goto error;
}
case MIXER_OMNICROSS:
case MIXER_OMNIPLUS:
goto error;
}
float abs_max = 0.f;
for (int8_t i = 0; i < len; i++) {
const float abs_val = fabsf(out[i]);
abs_max = (abs_val > abs_max) ? abs_val : abs_max;
}
if (abs_max > 1.f) {
for (int8_t i = 0; i < len; i++) {
out[i] /= abs_max;
}
}
for (int8_t i = 0; i < len; i++) {
out[i] *= scale;
}
return 0;
error:
for (uint8_t i = 0; i < len; i++) out[i] = 0;
return -1;
}