Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Interim checkin

Browse source
This commit is contained in:
Joel Fuster 2015-01-03 12:25:46 -05:00
parent 654c614380
commit 64a78f82f8
2 changed files with 29 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/flight/mixer.c
View file

@ -70,6 +70,9 @@ static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static mixerMode_e currentMixerMode; static mixerMode_e currentMixerMode;
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
static const float lowpassNormalizedButter5[] =
{ 3.2361f, 5.2361f, 5.2361f, 3.2361f, 1.0f };
static const motorMixer_t mixerQuadX[] = { static const motorMixer_t mixerQuadX[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
@ -663,22 +666,29 @@ bool isMixerUsingServos(void)
return useServo; return useServo;
} }
static float lowpass(lowpass_t *filter, float in, int16_t freqIdx ) static void generate_lowpass_coeffs5(int16_t freq)
{
const float lowpassNormalizedButter5[] = { 3.2361f, 5.2361f, 5.2361f, 3.2361f, 1.0f };
}
static float lowpass(lowpass_t *filter, float in, int16_t freq)
{ {
int16_t coefIdx; int16_t coefIdx;
float out; float out;
// Check to see if cutoff frequency changed // Check to see if cutoff frequency changed
if (freqIdx != filter->freqIdx) { if (freq != filter->freq) {
filter->init = false; filter->init = false;
} }
// Initialize if needed // Initialize if needed
if (!filter->init) { if (!filter->init) {
filter->freqIdx = freqIdx; generate_lowpass_coeffs5(freq, filter->b, filter->a);
filter->freq = lowpass_table[filter->freqIdx][0]; //filter->b = &lowpass_table[filter->freqIdx][1];
filter->b = &lowpass_table[filter->freqIdx][1]; //filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
filter->x[coefIdx] = in; filter->x[coefIdx] = in;
filter->y[coefIdx] = in; filter->y[coefIdx] = in;
@ -711,7 +721,7 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) { if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
// Round to nearest // Round to nearest
servo[servoIdx] = (int16_t)(lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx) + 0.5f); servo[servoIdx] = (int16_t)(lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq) + 0.5f);
// Sanity check // Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
} }

16
src/main/flight/mixer.h
View file

@ -93,16 +93,24 @@ typedef struct servoParam_t {
} servoParam_t; } servoParam_t;
#define LOWPASS_NUM_COEF 6 #define LOWPASS_NUM_COEF 6
/*
typedef struct lowpass_coeffs_t {
int16_t freq; // 1/1000ths of normalized frequency
float b[LOWPASS_NUM_COEF];
float a[LOWPASS_NUM_COEF];
} lowpass_coeffs_t;
*/
typedef struct lowpass_t { typedef struct lowpass_t {
bool init; bool init;
int16_t freqIdx; int16_t freq; // Normalized freq in 1/1000ths
float freq; float b[LOWPASS_NUM_COEF];
const float *b; float a[LOWPASS_NUM_COEF];
const float *a;
float x[LOWPASS_NUM_COEF]; float x[LOWPASS_NUM_COEF];
float y[LOWPASS_NUM_COEF]; float y[LOWPASS_NUM_COEF];
} lowpass_t; } lowpass_t;
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int16_t servo[MAX_SUPPORTED_SERVOS]; extern int16_t servo[MAX_SUPPORTED_SERVOS];