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

Interim checkin

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
Show Stats 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 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[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
@ -663,22 +666,29 @@ bool isMixerUsingServos(void)
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;
float out;
// Check to see if cutoff frequency changed
if (freqIdx != filter->freqIdx) {
if (freq != filter->freq) {
filter->init = false;
}
// Initialize if needed
if (!filter->init) {
filter->freqIdx = freqIdx;
filter->freq = lowpass_table[filter->freqIdx][0];
filter->b = &lowpass_table[filter->freqIdx][1];
filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
generate_lowpass_coeffs5(freq, filter->b, filter->a);
//filter->b = &lowpass_table[filter->freqIdx][1];
//filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
filter->x[coefIdx] = in;
filter->y[coefIdx] = in;
@ -711,7 +721,7 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
// 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
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;
#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 {
bool init;
int16_t freqIdx;
float freq;
const float *b;
const float *a;
int16_t freq; // Normalized freq in 1/1000ths
float b[LOWPASS_NUM_COEF];
float a[LOWPASS_NUM_COEF];
float x[LOWPASS_NUM_COEF];
float y[LOWPASS_NUM_COEF];
} lowpass_t;
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int16_t servo[MAX_SUPPORTED_SERVOS];