From a9020d7b51b7dd5e4ce18f32477906ea5f69c7c0 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Thu, 1 Jan 2015 11:56:41 -0500 Subject: [PATCH 01/24] Servo filtering test --- src/main/flight/notch_table.h | 102 ++++++++++++++++++++++++++++++++++ 1 file changed, 102 insertions(+) create mode 100755 src/main/flight/notch_table.h diff --git a/src/main/flight/notch_table.h b/src/main/flight/notch_table.h new file mode 100755 index 0000000000..e543e3b485 --- /dev/null +++ b/src/main/flight/notch_table.h @@ -0,0 +1,102 @@ +float notch_table[][6] = { + { 3.500175e-03, 9.978056e-01, -1.995129e+00, 9.978056e-01, -1.995129e+00, 9.956112e-01 }, + { 7.000350e-03, 9.956208e-01, -1.989316e+00, 9.956208e-01, -1.989316e+00, 9.912416e-01 }, + { 1.050053e-02, 9.934455e-01, -1.982568e+00, 9.934455e-01, -1.982568e+00, 9.868910e-01 }, + { 1.400070e-02, 9.912796e-01, -1.974893e+00, 9.912796e-01, -1.974893e+00, 9.825592e-01 }, + { 1.750088e-02, 9.891230e-01, -1.966298e+00, 9.891230e-01, -1.966298e+00, 9.782461e-01 }, + { 2.100105e-02, 9.869758e-01, -1.956791e+00, 9.869758e-01, -1.956791e+00, 9.739515e-01 }, + { 2.450123e-02, 9.848376e-01, -1.946381e+00, 9.848376e-01, -1.946381e+00, 9.696753e-01 }, + { 2.800140e-02, 9.827086e-01, -1.935077e+00, 9.827086e-01, -1.935077e+00, 9.654173e-01 }, + { 3.150158e-02, 9.805887e-01, -1.922887e+00, 9.805887e-01, -1.922887e+00, 9.611773e-01 }, + { 3.500175e-02, 9.784776e-01, -1.909821e+00, 9.784776e-01, -1.909821e+00, 9.569552e-01 }, + { 3.850193e-02, 9.763754e-01, -1.895889e+00, 9.763754e-01, -1.895889e+00, 9.527509e-01 }, + { 4.200210e-02, 9.742820e-01, -1.881101e+00, 9.742820e-01, -1.881101e+00, 9.485641e-01 }, + { 4.550228e-02, 9.721974e-01, -1.865469e+00, 9.721974e-01, -1.865469e+00, 9.443948e-01 }, + { 4.900245e-02, 9.701213e-01, -1.849002e+00, 9.701213e-01, -1.849002e+00, 9.402427e-01 }, + { 5.250263e-02, 9.680539e-01, -1.831713e+00, 9.680539e-01, -1.831713e+00, 9.361078e-01 }, + { 5.600280e-02, 9.659949e-01, -1.813613e+00, 9.659949e-01, -1.813613e+00, 9.319898e-01 }, + { 5.950298e-02, 9.639444e-01, -1.794713e+00, 9.639444e-01, -1.794713e+00, 9.278888e-01 }, + { 6.300315e-02, 9.619022e-01, -1.775027e+00, 9.619022e-01, -1.775027e+00, 9.238044e-01 }, + { 6.650333e-02, 9.598683e-01, -1.754567e+00, 9.598683e-01, -1.754567e+00, 9.197366e-01 }, + { 7.000350e-02, 9.578426e-01, -1.733346e+00, 9.578426e-01, -1.733346e+00, 9.156852e-01 }, + { 7.350368e-02, 9.558250e-01, -1.711377e+00, 9.558250e-01, -1.711377e+00, 9.116501e-01 }, + { 7.700385e-02, 9.538156e-01, -1.688673e+00, 9.538156e-01, -1.688673e+00, 9.076311e-01 }, + { 8.050403e-02, 9.518141e-01, -1.665249e+00, 9.518141e-01, -1.665249e+00, 9.036282e-01 }, + { 8.400420e-02, 9.498205e-01, -1.641119e+00, 9.498205e-01, -1.641119e+00, 8.996411e-01 }, + { 8.750438e-02, 9.478349e-01, -1.616297e+00, 9.478349e-01, -1.616297e+00, 8.956698e-01 }, + { 9.100455e-02, 9.458570e-01, -1.590797e+00, 9.458570e-01, -1.590797e+00, 8.917140e-01 }, + { 9.450473e-02, 9.438869e-01, -1.564635e+00, 9.438869e-01, -1.564635e+00, 8.877738e-01 }, + { 9.800490e-02, 9.419245e-01, -1.537826e+00, 9.419245e-01, -1.537826e+00, 8.838489e-01 }, + { 1.015051e-01, 9.399697e-01, -1.510385e+00, 9.399697e-01, -1.510385e+00, 8.799393e-01 }, + { 1.050053e-01, 9.380224e-01, -1.482328e+00, 9.380224e-01, -1.482328e+00, 8.760448e-01 }, + { 1.085054e-01, 9.360826e-01, -1.453671e+00, 9.360826e-01, -1.453671e+00, 8.721652e-01 }, + { 1.120056e-01, 9.341502e-01, -1.424429e+00, 9.341502e-01, -1.424429e+00, 8.683005e-01 }, + { 1.155058e-01, 9.322253e-01, -1.394620e+00, 9.322253e-01, -1.394620e+00, 8.644505e-01 }, + { 1.190060e-01, 9.303076e-01, -1.364259e+00, 9.303076e-01, -1.364259e+00, 8.606152e-01 }, + { 1.225061e-01, 9.283971e-01, -1.333363e+00, 9.283971e-01, -1.333363e+00, 8.567943e-01 }, + { 1.260063e-01, 9.264939e-01, -1.301950e+00, 9.264939e-01, -1.301950e+00, 8.529878e-01 }, + { 1.295065e-01, 9.245978e-01, -1.270035e+00, 9.245978e-01, -1.270035e+00, 8.491955e-01 }, + { 1.330067e-01, 9.227087e-01, -1.237638e+00, 9.227087e-01, -1.237638e+00, 8.454174e-01 }, + { 1.365068e-01, 9.208267e-01, -1.204774e+00, 9.208267e-01, -1.204774e+00, 8.416533e-01 }, + { 1.400070e-01, 9.189516e-01, -1.171461e+00, 9.189516e-01, -1.171461e+00, 8.379032e-01 }, + { 1.435072e-01, 9.170834e-01, -1.137718e+00, 9.170834e-01, -1.137718e+00, 8.341668e-01 }, + { 1.470074e-01, 9.152220e-01, -1.103561e+00, 9.152220e-01, -1.103561e+00, 8.304441e-01 }, + { 1.505075e-01, 9.133675e-01, -1.069010e+00, 9.133675e-01, -1.069010e+00, 8.267350e-01 }, + { 1.540077e-01, 9.115196e-01, -1.034081e+00, 9.115196e-01, -1.034081e+00, 8.230393e-01 }, + { 1.575079e-01, 9.096785e-01, -9.987933e-01, 9.096785e-01, -9.987933e-01, 8.193570e-01 }, + { 1.610081e-01, 9.078440e-01, -9.631649e-01, 9.078440e-01, -9.631649e-01, 8.156879e-01 }, + { 1.645082e-01, 9.060160e-01, -9.272142e-01, 9.060160e-01, -9.272142e-01, 8.120320e-01 }, + { 1.680084e-01, 9.041946e-01, -8.909597e-01, 9.041946e-01, -8.909597e-01, 8.083891e-01 }, + { 1.715086e-01, 9.023796e-01, -8.544198e-01, 9.023796e-01, -8.544198e-01, 8.047592e-01 }, + { 1.750088e-01, 9.005710e-01, -8.176131e-01, 9.005710e-01, -8.176131e-01, 8.011420e-01 }, + { 1.785089e-01, 8.987688e-01, -7.805583e-01, 8.987688e-01, -7.805583e-01, 7.975376e-01 }, + { 1.820091e-01, 8.969729e-01, -7.432740e-01, 8.969729e-01, -7.432740e-01, 7.939459e-01 }, + { 1.855093e-01, 8.951833e-01, -7.057789e-01, 8.951833e-01, -7.057789e-01, 7.903666e-01 }, + { 1.890095e-01, 8.933999e-01, -6.680918e-01, 8.933999e-01, -6.680918e-01, 7.867998e-01 }, + { 1.925096e-01, 8.916226e-01, -6.302314e-01, 8.916226e-01, -6.302314e-01, 7.832452e-01 }, + { 1.960098e-01, 8.898515e-01, -5.922166e-01, 8.898515e-01, -5.922166e-01, 7.797029e-01 }, + { 1.995100e-01, 8.880864e-01, -5.540660e-01, 8.880864e-01, -5.540660e-01, 7.761728e-01 }, + { 2.030102e-01, 8.863273e-01, -5.157984e-01, 8.863273e-01, -5.157984e-01, 7.726546e-01 }, + { 2.065103e-01, 8.845742e-01, -4.774327e-01, 8.845742e-01, -4.774327e-01, 7.691484e-01 }, + { 2.100105e-01, 8.828270e-01, -4.389875e-01, 8.828270e-01, -4.389875e-01, 7.656541e-01 }, + { 2.135107e-01, 8.810857e-01, -4.004815e-01, 8.810857e-01, -4.004815e-01, 7.621715e-01 }, + { 2.170109e-01, 8.793503e-01, -3.619333e-01, 8.793503e-01, -3.619333e-01, 7.587005e-01 }, + { 2.205110e-01, 8.776206e-01, -3.233617e-01, 8.776206e-01, -3.233617e-01, 7.552411e-01 }, + { 2.240112e-01, 8.758966e-01, -2.847850e-01, 8.758966e-01, -2.847850e-01, 7.517932e-01 }, + { 2.275114e-01, 8.741783e-01, -2.462219e-01, 8.741783e-01, -2.462219e-01, 7.483567e-01 }, + { 2.310116e-01, 8.724657e-01, -2.076906e-01, 8.724657e-01, -2.076906e-01, 7.449314e-01 }, + { 2.345117e-01, 8.707587e-01, -1.692096e-01, 8.707587e-01, -1.692096e-01, 7.415174e-01 }, + { 2.380119e-01, 8.690572e-01, -1.307970e-01, 8.690572e-01, -1.307970e-01, 7.381145e-01 }, + { 2.415121e-01, 8.673613e-01, -9.247099e-02, 8.673613e-01, -9.247099e-02, 7.347226e-01 }, + { 2.450123e-01, 8.656708e-01, -5.424955e-02, 8.656708e-01, -5.424955e-02, 7.313416e-01 }, + { 2.485124e-01, 8.639858e-01, -1.615061e-02, 8.639858e-01, -1.615061e-02, 7.279715e-01 }, + { 2.520126e-01, 8.623061e-01, 2.180808e-02, 8.623061e-01, 2.180808e-02, 7.246122e-01 }, + { 2.555128e-01, 8.606318e-01, 5.960885e-02, 8.606318e-01, 5.960885e-02, 7.212636e-01 }, + { 2.590130e-01, 8.589628e-01, 9.723420e-02, 8.589628e-01, 9.723420e-02, 7.179256e-01 }, + { 2.625131e-01, 8.572990e-01, 1.346668e-01, 8.572990e-01, 1.346668e-01, 7.145981e-01 }, + { 2.660133e-01, 8.556405e-01, 1.718894e-01, 8.556405e-01, 1.718894e-01, 7.112810e-01 }, + { 2.695135e-01, 8.539872e-01, 2.088850e-01, 8.539872e-01, 2.088850e-01, 7.079744e-01 }, + { 2.730137e-01, 8.523390e-01, 2.456367e-01, 8.523390e-01, 2.456367e-01, 7.046779e-01 }, + { 2.765138e-01, 8.506959e-01, 2.821278e-01, 8.506959e-01, 2.821278e-01, 7.013917e-01 }, + { 2.800140e-01, 8.490578e-01, 3.183419e-01, 8.490578e-01, 3.183419e-01, 6.981156e-01 }, + { 2.835142e-01, 8.474248e-01, 3.542625e-01, 8.474248e-01, 3.542625e-01, 6.948496e-01 }, + { 2.870144e-01, 8.457968e-01, 3.898737e-01, 8.457968e-01, 3.898737e-01, 6.915935e-01 }, + { 2.905145e-01, 8.441737e-01, 4.251594e-01, 8.441737e-01, 4.251594e-01, 6.883473e-01 }, + { 2.940147e-01, 8.425555e-01, 4.601040e-01, 8.425555e-01, 4.601040e-01, 6.851110e-01 }, + { 2.975149e-01, 8.409422e-01, 4.946921e-01, 8.409422e-01, 4.946921e-01, 6.818843e-01 }, + { 3.010151e-01, 8.393337e-01, 5.289083e-01, 8.393337e-01, 5.289083e-01, 6.786674e-01 }, + { 3.045152e-01, 8.377300e-01, 5.627376e-01, 8.377300e-01, 5.627376e-01, 6.754600e-01 }, + { 3.080154e-01, 8.361311e-01, 5.961653e-01, 8.361311e-01, 5.961653e-01, 6.722621e-01 }, + { 3.115156e-01, 8.345368e-01, 6.291768e-01, 8.345368e-01, 6.291768e-01, 6.690737e-01 }, + { 3.150158e-01, 8.329473e-01, 6.617578e-01, 8.329473e-01, 6.617578e-01, 6.658946e-01 }, + { 3.185159e-01, 8.313624e-01, 6.938944e-01, 8.313624e-01, 6.938944e-01, 6.627249e-01 }, + { 3.220161e-01, 8.297822e-01, 7.255727e-01, 8.297822e-01, 7.255727e-01, 6.595643e-01 }, + { 3.255163e-01, 8.282065e-01, 7.567793e-01, 8.282065e-01, 7.567793e-01, 6.564130e-01 }, + { 3.290165e-01, 8.266353e-01, 7.875009e-01, 8.266353e-01, 7.875009e-01, 6.532707e-01 }, + { 3.325166e-01, 8.250687e-01, 8.177246e-01, 8.250687e-01, 8.177246e-01, 6.501374e-01 }, + { 3.360168e-01, 8.235065e-01, 8.474377e-01, 8.235065e-01, 8.474377e-01, 6.470131e-01 }, + { 3.395170e-01, 8.219488e-01, 8.766279e-01, 8.219488e-01, 8.766279e-01, 6.438976e-01 }, + { 3.430172e-01, 8.203955e-01, 9.052830e-01, 8.203955e-01, 9.052830e-01, 6.407910e-01 }, + { 3.465173e-01, 8.188465e-01, 9.333914e-01, 8.188465e-01, 9.333914e-01, 6.376931e-01 }, + { 3.500175e-01, 8.173019e-01, 9.609415e-01, 8.173019e-01, 9.609415e-01, 6.346039e-01 } +}; From ceb4690befcd4e344343953208cb5cb893b17df5 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Thu, 1 Jan 2015 11:56:56 -0500 Subject: [PATCH 02/24] Servo filtering test --- src/main/flight/mixer.c | 47 +++++++++++++++++++++++++++++++++++++++++ src/main/flight/mixer.h | 11 ++++++++++ src/main/mw.c | 3 +++ 3 files changed, 61 insertions(+) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 16037d1651..a9191c97ca 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -653,3 +653,50 @@ bool isMixerUsingServos(void) { return useServo; } + +#include "notch_table.h" +#define NOTCH_FILTER_COEFS 3 +static notchFilter_t notchFilters[MAX_SUPPORTED_SERVOS]; + +static float notchFilter(notchFilter_t *filter, float in) +{ + int16_t coefIdx; + float out; + + if (!filter->init) { + filter->freqIdx = 9; + filter->freq = notch_table[filter->freqIdx][0]; + filter->pCoef = ¬ch_table[filter->freqIdx][1]; + for (coefIdx = 0; coefIdx < NOTCH_FILTER_COEFS; coefIdx++) { + filter->in[coefIdx] = in; + filter->out[coefIdx] = in; + } + filter->init = true; + } + + filter->in[2] = filter->in[1]; + filter->in[1] = filter->in[0]; + filter->in[0] = in; + filter->out[2] = filter->out[1]; + filter->out[1] = filter->out[0]; + + out = filter->in[0] * filter->pCoef[0] + + filter->in[1] * filter->pCoef[1] + + filter->in[2] * filter->pCoef[2] - + (filter->out[1] * filter->pCoef[3] + + filter->out[2] * filter->pCoef[4]); + + filter->out[0] = out; + + return out; +} + +void filterServos(void) +{ + int16_t servoIdx; + + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + notchFilter(¬chFilters[servoIdx], servo[servoIdx]); + } +} + diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index f198fd11dd..0cfdef6611 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -90,6 +90,16 @@ typedef struct servoParam_t { int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED } servoParam_t; +#define NOTCH_FILTER_COEFS 3 +typedef struct notchFilter_t { + bool init; + int16_t freqIdx; + float freq; + float *pCoef; + float in[NOTCH_FILTER_COEFS]; + float out[NOTCH_FILTER_COEFS]; +} notchFilter_t; + extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t servo[MAX_SUPPORTED_SERVOS]; @@ -99,5 +109,6 @@ void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerResetMotors(void); void mixTable(void); +void filterServos(void); void writeServos(void); void writeMotors(void); diff --git a/src/main/mw.c b/src/main/mw.c index 54defd191e..fb132c8639 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -696,6 +696,9 @@ void loop(void) ); mixTable(); + if (1) { + filterServos(); + } writeServos(); writeMotors(); } From cf40e3231a91a994a35c27c2aa5ac273e0b598f5 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Thu, 1 Jan 2015 15:03:40 -0500 Subject: [PATCH 03/24] Add configuration of notch filter More fine-grained filter freqs --- src/main/config/config.c | 2 + src/main/flight/mixer.c | 20 ++++--- src/main/flight/mixer.h | 2 + src/main/flight/notch_table.h | 106 +++++++++++++++++----------------- src/main/io/serial_cli.c | 2 + src/main/mw.c | 4 +- 6 files changed, 74 insertions(+), 62 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index f1f5695754..80c8412283 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -428,6 +428,8 @@ static void resetConf(void) currentProfile->mixerConfig.yaw_direction = 1; currentProfile->mixerConfig.tri_unarmed_servo = 1; + currentProfile->mixerConfig.servo_notch_freq_idx = 21; + currentProfile->mixerConfig.servo_notch_enable = 0; // gimbal currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a9191c97ca..68bc39516b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -42,6 +42,8 @@ #include "config/runtime_config.h" #include "config/config.h" +#include "notch_table.h" + #define GIMBAL_SERVO_PITCH 0 #define GIMBAL_SERVO_ROLL 1 @@ -66,6 +68,7 @@ static gimbalConfig_t *gimbalConfig; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static mixerMode_e currentMixerMode; +static notchFilter_t notchFilters[MAX_SUPPORTED_SERVOS]; static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -654,17 +657,18 @@ bool isMixerUsingServos(void) return useServo; } -#include "notch_table.h" -#define NOTCH_FILTER_COEFS 3 -static notchFilter_t notchFilters[MAX_SUPPORTED_SERVOS]; -static float notchFilter(notchFilter_t *filter, float in) +static float notchFilter(notchFilter_t *filter, float in, int16_t freqIdx ) { int16_t coefIdx; float out; + if (freqIdx != filter->freqIdx) { + filter->init = false; + } + if (!filter->init) { - filter->freqIdx = 9; + filter->freqIdx = freqIdx; filter->freq = notch_table[filter->freqIdx][0]; filter->pCoef = ¬ch_table[filter->freqIdx][1]; for (coefIdx = 0; coefIdx < NOTCH_FILTER_COEFS; coefIdx++) { @@ -695,8 +699,10 @@ void filterServos(void) { int16_t servoIdx; - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - notchFilter(¬chFilters[servoIdx], servo[servoIdx]); + if (mixerConfig->servo_notch_enable) { + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + servo[servoIdx] = notchFilter(¬chFilters[servoIdx], servo[servoIdx], mixerConfig->servo_notch_freq_idx); + } } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 0cfdef6611..1f05e5abde 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -66,6 +66,8 @@ typedef struct mixer_t { typedef struct mixerConfig_s { int8_t yaw_direction; uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + int16_t servo_notch_freq_idx; // notch filter frequency selection + int8_t servo_notch_enable; // enable/disable notch filter } mixerConfig_t; typedef struct flight3DConfig_s { diff --git a/src/main/flight/notch_table.h b/src/main/flight/notch_table.h index e543e3b485..dbe0d36f4c 100755 --- a/src/main/flight/notch_table.h +++ b/src/main/flight/notch_table.h @@ -1,102 +1,104 @@ -float notch_table[][6] = { +#define NOTCH_FILTER_NUM 100 +#define NOTCH_FILTER_COEFS 3 +static float notch_table[][6] = { + { 1.750088e-03, 9.989016e-01, -1.997682e+00, 9.989016e-01, -1.997682e+00, 9.978032e-01 }, { 3.500175e-03, 9.978056e-01, -1.995129e+00, 9.978056e-01, -1.995129e+00, 9.956112e-01 }, + { 5.250263e-03, 9.967120e-01, -1.992339e+00, 9.967120e-01, -1.992339e+00, 9.934240e-01 }, { 7.000350e-03, 9.956208e-01, -1.989316e+00, 9.956208e-01, -1.989316e+00, 9.912416e-01 }, + { 8.750438e-03, 9.945319e-01, -1.986058e+00, 9.945319e-01, -1.986058e+00, 9.890639e-01 }, { 1.050053e-02, 9.934455e-01, -1.982568e+00, 9.934455e-01, -1.982568e+00, 9.868910e-01 }, + { 1.225061e-02, 9.923614e-01, -1.978846e+00, 9.923614e-01, -1.978846e+00, 9.847227e-01 }, { 1.400070e-02, 9.912796e-01, -1.974893e+00, 9.912796e-01, -1.974893e+00, 9.825592e-01 }, + { 1.575079e-02, 9.902002e-01, -1.970710e+00, 9.902002e-01, -1.970710e+00, 9.804003e-01 }, { 1.750088e-02, 9.891230e-01, -1.966298e+00, 9.891230e-01, -1.966298e+00, 9.782461e-01 }, + { 1.925096e-02, 9.880482e-01, -1.961658e+00, 9.880482e-01, -1.961658e+00, 9.760965e-01 }, { 2.100105e-02, 9.869758e-01, -1.956791e+00, 9.869758e-01, -1.956791e+00, 9.739515e-01 }, + { 2.275114e-02, 9.859056e-01, -1.951699e+00, 9.859056e-01, -1.951699e+00, 9.718111e-01 }, { 2.450123e-02, 9.848376e-01, -1.946381e+00, 9.848376e-01, -1.946381e+00, 9.696753e-01 }, + { 2.625131e-02, 9.837720e-01, -1.940840e+00, 9.837720e-01, -1.940840e+00, 9.675440e-01 }, { 2.800140e-02, 9.827086e-01, -1.935077e+00, 9.827086e-01, -1.935077e+00, 9.654173e-01 }, + { 2.975149e-02, 9.816475e-01, -1.929092e+00, 9.816475e-01, -1.929092e+00, 9.632950e-01 }, { 3.150158e-02, 9.805887e-01, -1.922887e+00, 9.805887e-01, -1.922887e+00, 9.611773e-01 }, + { 3.325166e-02, 9.795320e-01, -1.916463e+00, 9.795320e-01, -1.916463e+00, 9.590640e-01 }, { 3.500175e-02, 9.784776e-01, -1.909821e+00, 9.784776e-01, -1.909821e+00, 9.569552e-01 }, + { 3.675184e-02, 9.774254e-01, -1.902962e+00, 9.774254e-01, -1.902962e+00, 9.548508e-01 }, { 3.850193e-02, 9.763754e-01, -1.895889e+00, 9.763754e-01, -1.895889e+00, 9.527509e-01 }, + { 4.025201e-02, 9.753276e-01, -1.888601e+00, 9.753276e-01, -1.888601e+00, 9.506553e-01 }, { 4.200210e-02, 9.742820e-01, -1.881101e+00, 9.742820e-01, -1.881101e+00, 9.485641e-01 }, + { 4.375219e-02, 9.732386e-01, -1.873390e+00, 9.732386e-01, -1.873390e+00, 9.464773e-01 }, { 4.550228e-02, 9.721974e-01, -1.865469e+00, 9.721974e-01, -1.865469e+00, 9.443948e-01 }, + { 4.725236e-02, 9.711583e-01, -1.857339e+00, 9.711583e-01, -1.857339e+00, 9.423166e-01 }, { 4.900245e-02, 9.701213e-01, -1.849002e+00, 9.701213e-01, -1.849002e+00, 9.402427e-01 }, + { 5.075254e-02, 9.690866e-01, -1.840460e+00, 9.690866e-01, -1.840460e+00, 9.381731e-01 }, { 5.250263e-02, 9.680539e-01, -1.831713e+00, 9.680539e-01, -1.831713e+00, 9.361078e-01 }, + { 5.425271e-02, 9.670233e-01, -1.822763e+00, 9.670233e-01, -1.822763e+00, 9.340467e-01 }, { 5.600280e-02, 9.659949e-01, -1.813613e+00, 9.659949e-01, -1.813613e+00, 9.319898e-01 }, + { 5.775289e-02, 9.649686e-01, -1.804262e+00, 9.649686e-01, -1.804262e+00, 9.299372e-01 }, { 5.950298e-02, 9.639444e-01, -1.794713e+00, 9.639444e-01, -1.794713e+00, 9.278888e-01 }, + { 6.125306e-02, 9.629222e-01, -1.784968e+00, 9.629222e-01, -1.784968e+00, 9.258445e-01 }, { 6.300315e-02, 9.619022e-01, -1.775027e+00, 9.619022e-01, -1.775027e+00, 9.238044e-01 }, + { 6.475324e-02, 9.608842e-01, -1.764893e+00, 9.608842e-01, -1.764893e+00, 9.217684e-01 }, { 6.650333e-02, 9.598683e-01, -1.754567e+00, 9.598683e-01, -1.754567e+00, 9.197366e-01 }, + { 6.825341e-02, 9.588544e-01, -1.744051e+00, 9.588544e-01, -1.744051e+00, 9.177088e-01 }, { 7.000350e-02, 9.578426e-01, -1.733346e+00, 9.578426e-01, -1.733346e+00, 9.156852e-01 }, + { 7.175359e-02, 9.568328e-01, -1.722454e+00, 9.568328e-01, -1.722454e+00, 9.136656e-01 }, { 7.350368e-02, 9.558250e-01, -1.711377e+00, 9.558250e-01, -1.711377e+00, 9.116501e-01 }, + { 7.525376e-02, 9.548193e-01, -1.700116e+00, 9.548193e-01, -1.700116e+00, 9.096386e-01 }, { 7.700385e-02, 9.538156e-01, -1.688673e+00, 9.538156e-01, -1.688673e+00, 9.076311e-01 }, + { 7.875394e-02, 9.528138e-01, -1.677051e+00, 9.528138e-01, -1.677051e+00, 9.056276e-01 }, { 8.050403e-02, 9.518141e-01, -1.665249e+00, 9.518141e-01, -1.665249e+00, 9.036282e-01 }, + { 8.225411e-02, 9.508163e-01, -1.653272e+00, 9.508163e-01, -1.653272e+00, 9.016326e-01 }, { 8.400420e-02, 9.498205e-01, -1.641119e+00, 9.498205e-01, -1.641119e+00, 8.996411e-01 }, + { 8.575429e-02, 9.488267e-01, -1.628794e+00, 9.488267e-01, -1.628794e+00, 8.976535e-01 }, { 8.750438e-02, 9.478349e-01, -1.616297e+00, 9.478349e-01, -1.616297e+00, 8.956698e-01 }, + { 8.925446e-02, 9.468450e-01, -1.603631e+00, 9.468450e-01, -1.603631e+00, 8.936900e-01 }, { 9.100455e-02, 9.458570e-01, -1.590797e+00, 9.458570e-01, -1.590797e+00, 8.917140e-01 }, + { 9.275464e-02, 9.448710e-01, -1.577798e+00, 9.448710e-01, -1.577798e+00, 8.897420e-01 }, { 9.450473e-02, 9.438869e-01, -1.564635e+00, 9.438869e-01, -1.564635e+00, 8.877738e-01 }, + { 9.625481e-02, 9.429047e-01, -1.551311e+00, 9.429047e-01, -1.551311e+00, 8.858095e-01 }, { 9.800490e-02, 9.419245e-01, -1.537826e+00, 9.419245e-01, -1.537826e+00, 8.838489e-01 }, + { 9.975499e-02, 9.409461e-01, -1.524184e+00, 9.409461e-01, -1.524184e+00, 8.818922e-01 }, { 1.015051e-01, 9.399697e-01, -1.510385e+00, 9.399697e-01, -1.510385e+00, 8.799393e-01 }, + { 1.032552e-01, 9.389951e-01, -1.496433e+00, 9.389951e-01, -1.496433e+00, 8.779902e-01 }, { 1.050053e-01, 9.380224e-01, -1.482328e+00, 9.380224e-01, -1.482328e+00, 8.760448e-01 }, + { 1.067553e-01, 9.370516e-01, -1.468074e+00, 9.370516e-01, -1.468074e+00, 8.741031e-01 }, { 1.085054e-01, 9.360826e-01, -1.453671e+00, 9.360826e-01, -1.453671e+00, 8.721652e-01 }, + { 1.102555e-01, 9.351155e-01, -1.439122e+00, 9.351155e-01, -1.439122e+00, 8.702310e-01 }, { 1.120056e-01, 9.341502e-01, -1.424429e+00, 9.341502e-01, -1.424429e+00, 8.683005e-01 }, + { 1.137557e-01, 9.331868e-01, -1.409594e+00, 9.331868e-01, -1.409594e+00, 8.663737e-01 }, { 1.155058e-01, 9.322253e-01, -1.394620e+00, 9.322253e-01, -1.394620e+00, 8.644505e-01 }, + { 1.172559e-01, 9.312655e-01, -1.379507e+00, 9.312655e-01, -1.379507e+00, 8.625310e-01 }, { 1.190060e-01, 9.303076e-01, -1.364259e+00, 9.303076e-01, -1.364259e+00, 8.606152e-01 }, + { 1.207560e-01, 9.293515e-01, -1.348877e+00, 9.293515e-01, -1.348877e+00, 8.587029e-01 }, { 1.225061e-01, 9.283971e-01, -1.333363e+00, 9.283971e-01, -1.333363e+00, 8.567943e-01 }, + { 1.242562e-01, 9.274446e-01, -1.317720e+00, 9.274446e-01, -1.317720e+00, 8.548892e-01 }, { 1.260063e-01, 9.264939e-01, -1.301950e+00, 9.264939e-01, -1.301950e+00, 8.529878e-01 }, + { 1.277564e-01, 9.255449e-01, -1.286054e+00, 9.255449e-01, -1.286054e+00, 8.510899e-01 }, { 1.295065e-01, 9.245978e-01, -1.270035e+00, 9.245978e-01, -1.270035e+00, 8.491955e-01 }, + { 1.312566e-01, 9.236524e-01, -1.253896e+00, 9.236524e-01, -1.253896e+00, 8.473047e-01 }, { 1.330067e-01, 9.227087e-01, -1.237638e+00, 9.227087e-01, -1.237638e+00, 8.454174e-01 }, + { 1.347567e-01, 9.217668e-01, -1.221263e+00, 9.217668e-01, -1.221263e+00, 8.435336e-01 }, { 1.365068e-01, 9.208267e-01, -1.204774e+00, 9.208267e-01, -1.204774e+00, 8.416533e-01 }, + { 1.382569e-01, 9.198883e-01, -1.188172e+00, 9.198883e-01, -1.188172e+00, 8.397765e-01 }, { 1.400070e-01, 9.189516e-01, -1.171461e+00, 9.189516e-01, -1.171461e+00, 8.379032e-01 }, + { 1.417571e-01, 9.180166e-01, -1.154642e+00, 9.180166e-01, -1.154642e+00, 8.360333e-01 }, { 1.435072e-01, 9.170834e-01, -1.137718e+00, 9.170834e-01, -1.137718e+00, 8.341668e-01 }, + { 1.452573e-01, 9.161519e-01, -1.120690e+00, 9.161519e-01, -1.120690e+00, 8.323037e-01 }, { 1.470074e-01, 9.152220e-01, -1.103561e+00, 9.152220e-01, -1.103561e+00, 8.304441e-01 }, + { 1.487574e-01, 9.142939e-01, -1.086334e+00, 9.142939e-01, -1.086334e+00, 8.285878e-01 }, { 1.505075e-01, 9.133675e-01, -1.069010e+00, 9.133675e-01, -1.069010e+00, 8.267350e-01 }, + { 1.522576e-01, 9.124427e-01, -1.051591e+00, 9.124427e-01, -1.051591e+00, 8.248855e-01 }, { 1.540077e-01, 9.115196e-01, -1.034081e+00, 9.115196e-01, -1.034081e+00, 8.230393e-01 }, + { 1.557578e-01, 9.105982e-01, -1.016481e+00, 9.105982e-01, -1.016481e+00, 8.211965e-01 }, { 1.575079e-01, 9.096785e-01, -9.987933e-01, 9.096785e-01, -9.987933e-01, 8.193570e-01 }, + { 1.592580e-01, 9.087604e-01, -9.810205e-01, 9.087604e-01, -9.810205e-01, 8.175208e-01 }, { 1.610081e-01, 9.078440e-01, -9.631649e-01, 9.078440e-01, -9.631649e-01, 8.156879e-01 }, + { 1.627581e-01, 9.069292e-01, -9.452287e-01, 9.069292e-01, -9.452287e-01, 8.138583e-01 }, { 1.645082e-01, 9.060160e-01, -9.272142e-01, 9.060160e-01, -9.272142e-01, 8.120320e-01 }, + { 1.662583e-01, 9.051045e-01, -9.091238e-01, 9.051045e-01, -9.091238e-01, 8.102089e-01 }, { 1.680084e-01, 9.041946e-01, -8.909597e-01, 9.041946e-01, -8.909597e-01, 8.083891e-01 }, + { 1.697585e-01, 9.032863e-01, -8.727243e-01, 9.032863e-01, -8.727243e-01, 8.065725e-01 }, { 1.715086e-01, 9.023796e-01, -8.544198e-01, 9.023796e-01, -8.544198e-01, 8.047592e-01 }, - { 1.750088e-01, 9.005710e-01, -8.176131e-01, 9.005710e-01, -8.176131e-01, 8.011420e-01 }, - { 1.785089e-01, 8.987688e-01, -7.805583e-01, 8.987688e-01, -7.805583e-01, 7.975376e-01 }, - { 1.820091e-01, 8.969729e-01, -7.432740e-01, 8.969729e-01, -7.432740e-01, 7.939459e-01 }, - { 1.855093e-01, 8.951833e-01, -7.057789e-01, 8.951833e-01, -7.057789e-01, 7.903666e-01 }, - { 1.890095e-01, 8.933999e-01, -6.680918e-01, 8.933999e-01, -6.680918e-01, 7.867998e-01 }, - { 1.925096e-01, 8.916226e-01, -6.302314e-01, 8.916226e-01, -6.302314e-01, 7.832452e-01 }, - { 1.960098e-01, 8.898515e-01, -5.922166e-01, 8.898515e-01, -5.922166e-01, 7.797029e-01 }, - { 1.995100e-01, 8.880864e-01, -5.540660e-01, 8.880864e-01, -5.540660e-01, 7.761728e-01 }, - { 2.030102e-01, 8.863273e-01, -5.157984e-01, 8.863273e-01, -5.157984e-01, 7.726546e-01 }, - { 2.065103e-01, 8.845742e-01, -4.774327e-01, 8.845742e-01, -4.774327e-01, 7.691484e-01 }, - { 2.100105e-01, 8.828270e-01, -4.389875e-01, 8.828270e-01, -4.389875e-01, 7.656541e-01 }, - { 2.135107e-01, 8.810857e-01, -4.004815e-01, 8.810857e-01, -4.004815e-01, 7.621715e-01 }, - { 2.170109e-01, 8.793503e-01, -3.619333e-01, 8.793503e-01, -3.619333e-01, 7.587005e-01 }, - { 2.205110e-01, 8.776206e-01, -3.233617e-01, 8.776206e-01, -3.233617e-01, 7.552411e-01 }, - { 2.240112e-01, 8.758966e-01, -2.847850e-01, 8.758966e-01, -2.847850e-01, 7.517932e-01 }, - { 2.275114e-01, 8.741783e-01, -2.462219e-01, 8.741783e-01, -2.462219e-01, 7.483567e-01 }, - { 2.310116e-01, 8.724657e-01, -2.076906e-01, 8.724657e-01, -2.076906e-01, 7.449314e-01 }, - { 2.345117e-01, 8.707587e-01, -1.692096e-01, 8.707587e-01, -1.692096e-01, 7.415174e-01 }, - { 2.380119e-01, 8.690572e-01, -1.307970e-01, 8.690572e-01, -1.307970e-01, 7.381145e-01 }, - { 2.415121e-01, 8.673613e-01, -9.247099e-02, 8.673613e-01, -9.247099e-02, 7.347226e-01 }, - { 2.450123e-01, 8.656708e-01, -5.424955e-02, 8.656708e-01, -5.424955e-02, 7.313416e-01 }, - { 2.485124e-01, 8.639858e-01, -1.615061e-02, 8.639858e-01, -1.615061e-02, 7.279715e-01 }, - { 2.520126e-01, 8.623061e-01, 2.180808e-02, 8.623061e-01, 2.180808e-02, 7.246122e-01 }, - { 2.555128e-01, 8.606318e-01, 5.960885e-02, 8.606318e-01, 5.960885e-02, 7.212636e-01 }, - { 2.590130e-01, 8.589628e-01, 9.723420e-02, 8.589628e-01, 9.723420e-02, 7.179256e-01 }, - { 2.625131e-01, 8.572990e-01, 1.346668e-01, 8.572990e-01, 1.346668e-01, 7.145981e-01 }, - { 2.660133e-01, 8.556405e-01, 1.718894e-01, 8.556405e-01, 1.718894e-01, 7.112810e-01 }, - { 2.695135e-01, 8.539872e-01, 2.088850e-01, 8.539872e-01, 2.088850e-01, 7.079744e-01 }, - { 2.730137e-01, 8.523390e-01, 2.456367e-01, 8.523390e-01, 2.456367e-01, 7.046779e-01 }, - { 2.765138e-01, 8.506959e-01, 2.821278e-01, 8.506959e-01, 2.821278e-01, 7.013917e-01 }, - { 2.800140e-01, 8.490578e-01, 3.183419e-01, 8.490578e-01, 3.183419e-01, 6.981156e-01 }, - { 2.835142e-01, 8.474248e-01, 3.542625e-01, 8.474248e-01, 3.542625e-01, 6.948496e-01 }, - { 2.870144e-01, 8.457968e-01, 3.898737e-01, 8.457968e-01, 3.898737e-01, 6.915935e-01 }, - { 2.905145e-01, 8.441737e-01, 4.251594e-01, 8.441737e-01, 4.251594e-01, 6.883473e-01 }, - { 2.940147e-01, 8.425555e-01, 4.601040e-01, 8.425555e-01, 4.601040e-01, 6.851110e-01 }, - { 2.975149e-01, 8.409422e-01, 4.946921e-01, 8.409422e-01, 4.946921e-01, 6.818843e-01 }, - { 3.010151e-01, 8.393337e-01, 5.289083e-01, 8.393337e-01, 5.289083e-01, 6.786674e-01 }, - { 3.045152e-01, 8.377300e-01, 5.627376e-01, 8.377300e-01, 5.627376e-01, 6.754600e-01 }, - { 3.080154e-01, 8.361311e-01, 5.961653e-01, 8.361311e-01, 5.961653e-01, 6.722621e-01 }, - { 3.115156e-01, 8.345368e-01, 6.291768e-01, 8.345368e-01, 6.291768e-01, 6.690737e-01 }, - { 3.150158e-01, 8.329473e-01, 6.617578e-01, 8.329473e-01, 6.617578e-01, 6.658946e-01 }, - { 3.185159e-01, 8.313624e-01, 6.938944e-01, 8.313624e-01, 6.938944e-01, 6.627249e-01 }, - { 3.220161e-01, 8.297822e-01, 7.255727e-01, 8.297822e-01, 7.255727e-01, 6.595643e-01 }, - { 3.255163e-01, 8.282065e-01, 7.567793e-01, 8.282065e-01, 7.567793e-01, 6.564130e-01 }, - { 3.290165e-01, 8.266353e-01, 7.875009e-01, 8.266353e-01, 7.875009e-01, 6.532707e-01 }, - { 3.325166e-01, 8.250687e-01, 8.177246e-01, 8.250687e-01, 8.177246e-01, 6.501374e-01 }, - { 3.360168e-01, 8.235065e-01, 8.474377e-01, 8.235065e-01, 8.474377e-01, 6.470131e-01 }, - { 3.395170e-01, 8.219488e-01, 8.766279e-01, 8.219488e-01, 8.766279e-01, 6.438976e-01 }, - { 3.430172e-01, 8.203955e-01, 9.052830e-01, 8.203955e-01, 9.052830e-01, 6.407910e-01 }, - { 3.465173e-01, 8.188465e-01, 9.333914e-01, 8.188465e-01, 9.333914e-01, 6.376931e-01 }, - { 3.500175e-01, 8.173019e-01, 9.609415e-01, 8.173019e-01, 9.609415e-01, 6.346039e-01 } + { 1.732587e-01, 9.014745e-01, -8.360487e-01, 9.014745e-01, -8.360487e-01, 8.029490e-01 }, + { 1.750088e-01, 9.005710e-01, -8.176131e-01, 9.005710e-01, -8.176131e-01, 8.011420e-01 } }; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 79b3f27c47..eb94825cb8 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -332,6 +332,8 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, + { "servo_notch_freq_idx", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_notch_freq_idx, 0, 99 }, + { "servo_notch_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_notch_enable, 0, 1 }, { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, diff --git a/src/main/mw.c b/src/main/mw.c index fb132c8639..af57037a89 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -696,9 +696,7 @@ void loop(void) ); mixTable(); - if (1) { - filterServos(); - } + filterServos(); writeServos(); writeMotors(); } From c5b822ecf1d2d5567600721f241bd04b8c955aee Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Thu, 1 Jan 2015 17:08:14 -0500 Subject: [PATCH 04/24] Fix permissions --- src/main/flight/notch_table.h | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 src/main/flight/notch_table.h diff --git a/src/main/flight/notch_table.h b/src/main/flight/notch_table.h old mode 100755 new mode 100644 From 3eee9eb079e34bbb2ec8c0e95fd9d146d2e8fc42 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Thu, 1 Jan 2015 19:24:35 -0500 Subject: [PATCH 05/24] Lowpass testing --- src/main/config/config.c | 4 +- src/main/flight/lowpass_table.h | 104 ++++++++++++++++++++++++++++++++ src/main/flight/mixer.c | 57 +++++++++++++++-- src/main/flight/mixer.h | 14 ++--- src/main/io/serial_cli.c | 4 +- 5 files changed, 166 insertions(+), 17 deletions(-) create mode 100644 src/main/flight/lowpass_table.h diff --git a/src/main/config/config.c b/src/main/config/config.c index 80c8412283..21de5c93b1 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -428,8 +428,8 @@ static void resetConf(void) currentProfile->mixerConfig.yaw_direction = 1; currentProfile->mixerConfig.tri_unarmed_servo = 1; - currentProfile->mixerConfig.servo_notch_freq_idx = 21; - currentProfile->mixerConfig.servo_notch_enable = 0; + currentProfile->mixerConfig.servo_lowpass_freq_idx = 21; + currentProfile->mixerConfig.servo_lowpass_enable = 0; // gimbal currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; diff --git a/src/main/flight/lowpass_table.h b/src/main/flight/lowpass_table.h new file mode 100644 index 0000000000..a4b5ce7c4e --- /dev/null +++ b/src/main/flight/lowpass_table.h @@ -0,0 +1,104 @@ +#define LOWPASS_FILTER_NUM 100 +#define LOWPASS_NUM_COEF 6 +static float lowpass_table[][12] = { + { 2.500000e-03, -3.425227e-02, 1.065271e-01, -8.631566e-02, -3.905318e-02, 8.220568e-02, -2.911187e-02, -4.356644e+00, 7.533406e+00, -6.457224e+00, 2.740841e+00, -4.603782e-01 }, + { 5.000000e-03, 9.594949e-04, -3.757786e-03, 5.699713e-03, -4.135192e-03, 1.404994e-03, -1.712205e-04, -4.818173e+00, 9.277064e+00, -8.922091e+00, 4.285685e+00, -8.224846e-01 }, + { 7.500000e-03, 1.743498e-04, -4.836488e-04, 4.240127e-04, -7.912133e-05, -5.077890e-05, 1.520828e-05, -4.768018e+00, 9.080466e+00, -8.633118e+00, 4.096914e+00, -7.762436e-01 }, + { 1.000000e-02, 2.786778e-05, -1.048774e-04, 1.567428e-04, -1.169318e-04, 4.382585e-05, -6.624334e-06, -4.969154e+00, 9.877940e+00, -9.818872e+00, 4.880540e+00, -9.704543e-01 }, + { 1.250000e-02, 3.771678e-04, -1.059885e-03, 1.107245e-03, -5.459644e-04, 1.253553e-04, -3.589356e-06, -4.553322e+00, 8.240587e+00, -7.400896e+00, 3.293345e+00, -5.797131e-01 }, + { 1.500000e-02, 3.641343e-04, -1.670771e-03, 3.089786e-03, -2.876643e-03, 1.346476e-03, -2.529602e-04, -4.953038e+00, 9.815151e+00, -9.727146e+00, 4.820991e+00, -9.559584e-01 }, + { 1.750000e-02, 1.620037e-03, -7.258472e-03, 1.291276e-02, -1.136920e-02, 4.934118e-03, -8.391887e-04, -4.943426e+00, 9.777900e+00, -9.673006e+00, 4.786021e+00, -9.474886e-01 }, + { 2.000000e-02, 3.101550e-04, -1.237727e-03, 1.926983e-03, -1.432888e-03, 4.853767e-04, -5.180650e-05, -4.936123e+00, 9.749890e+00, -9.632735e+00, 4.760297e+00, -9.413297e-01 }, + { 2.250000e-02, 5.534977e-04, -2.260520e-03, 3.607053e-03, -2.758637e-03, 9.713944e-04, -1.126195e-04, -4.927408e+00, 9.716508e+00, -9.584794e+00, 4.729704e+00, -9.340104e-01 }, + { 2.500000e-02, 5.408189e-04, -2.373426e-03, 4.248880e-03, -3.870097e-03, 1.784493e-03, -3.303950e-04, -4.919587e+00, 9.686785e+00, -9.542449e+00, 4.702905e+00, -9.276541e-01 }, + { 2.750000e-02, 3.502801e-04, -1.361984e-03, 2.079191e-03, -1.509031e-03, 4.785137e-04, -3.652179e-05, -4.910569e+00, 9.652561e+00, -9.493750e+00, 4.672115e+00, -9.203566e-01 }, + { 3.000000e-02, 1.453611e-03, -5.612347e-03, 8.217122e-03, -5.417332e-03, 1.368011e-03, -8.318679e-06, -4.899744e+00, 9.611505e+00, -9.435349e+00, 4.635192e+00, -9.116028e-01 }, + { 3.250000e-02, 1.074577e-03, -3.989564e-03, 5.517087e-03, -3.244842e-03, 5.280205e-04, 1.158066e-04, -4.891322e+00, 9.579953e+00, -9.391041e+00, 4.607554e+00, -9.051436e-01 }, + { 3.500000e-02, 2.005131e-03, -7.736833e-03, 1.123183e-02, -7.164746e-03, 1.561016e-03, 1.052714e-04, -4.880244e+00, 9.538327e+00, -9.332365e+00, 4.570791e+00, -8.965060e-01 }, + { 3.750000e-02, 1.311003e-03, -4.668294e-03, 6.054300e-03, -3.080307e-03, 1.247654e-04, 2.607561e-04, -4.872694e+00, 9.510470e+00, -9.293865e+00, 4.547176e+00, -8.910848e-01 }, + { 4.000000e-02, 2.516459e-03, -1.023862e-02, 1.640094e-02, -1.270471e-02, 4.590988e-03, -5.619403e-04, -4.862729e+00, 9.473496e+00, -9.242415e+00, 4.515363e+00, -8.837123e-01 }, + { 4.250000e-02, 3.998376e-03, -1.431055e-02, 1.806231e-02, -7.926506e-03, -1.066708e-03, 1.248077e-03, -4.846160e+00, 9.411465e+00, -9.155203e+00, 4.460800e+00, -8.708966e-01 }, + { 4.500000e-02, -4.348069e-03, 1.480567e-02, -1.677925e-02, 4.499779e-03, 3.831973e-03, -2.016955e-03, -4.834516e+00, 9.368539e+00, -9.095813e+00, 4.424264e+00, -8.624668e-01 }, + { 4.750000e-02, 3.535345e-03, -1.324834e-02, 1.871796e-02, -1.156596e-02, 2.323784e-03, 2.450004e-04, -4.830814e+00, 9.356017e+00, -9.080188e+00, 4.415799e+00, -8.608062e-01 }, + { 5.000000e-02, 5.595668e-03, -2.078517e-02, 2.858994e-02, -1.637592e-02, 2.183922e-03, 8.027722e-04, -4.815366e+00, 9.299016e+00, -9.001167e+00, 4.367034e+00, -8.495060e-01 }, + { 5.250000e-02, -6.249993e-03, 2.101144e-02, -2.299353e-02, 4.533406e-03, 7.073653e-03, -3.391119e-03, -4.797826e+00, 9.234405e+00, -8.911684e+00, 4.311829e+00, -8.367090e-01 }, + { 5.500000e-02, -6.072127e-03, 1.947955e-02, -1.896575e-02, -2.760329e-04, 9.832365e-03, -4.018237e-03, -4.787198e+00, 9.196202e+00, -8.860144e+00, 4.280925e+00, -8.297648e-01 }, + { 5.750000e-02, -6.744602e-03, 2.253293e-02, -2.449972e-02, 4.596967e-03, 7.829719e-03, -3.740525e-03, -4.776084e+00, 9.156358e+00, -8.806519e+00, 4.248835e+00, -8.225667e-01 }, + { 6.000000e-02, 5.831348e-03, -1.721135e-02, 1.276161e-02, 7.999284e-03, -1.450428e-02, 5.154550e-03, -4.764866e+00, 9.116354e+00, -8.752950e+00, 4.216944e+00, -8.154510e-01 }, + { 6.250000e-02, -5.829863e-03, 1.683686e-02, -1.140031e-02, -1.001138e-02, 1.592147e-02, -5.554742e-03, -4.753981e+00, 9.077766e+00, -8.701582e+00, 4.186544e+00, -8.087097e-01 }, + { 6.500000e-02, -6.396675e-03, 1.935953e-02, -1.591036e-02, -6.118638e-03, 1.439645e-02, -5.376342e-03, -4.742702e+00, 9.037914e+00, -8.648691e+00, 4.155331e+00, -8.018065e-01 }, + { 6.750000e-02, -6.124160e-03, 1.754505e-02, -1.159011e-02, -1.111439e-02, 1.730100e-02, -6.072763e-03, -4.731360e+00, 8.998035e+00, -8.596011e+00, 4.124385e+00, -7.949945e-01 }, + { 7.000000e-02, -6.487342e-03, 1.882125e-02, -1.338584e-02, -9.970211e-03, 1.708311e-02, -6.127115e-03, -4.719920e+00, 8.957991e+00, -8.543337e+00, 4.093571e+00, -7.882396e-01 }, + { 7.250000e-02, -7.902875e-03, 2.519387e-02, -2.524474e-02, 1.280493e-03, 1.170741e-02, -5.112626e-03, -4.708425e+00, 8.917950e+00, -8.490911e+00, 4.063044e+00, -7.815793e-01 }, + { 7.500000e-02, 7.246073e-03, -2.142761e-02, 1.702449e-02, 7.602053e-03, -1.655766e-02, 6.205297e-03, -4.696766e+00, 8.877511e+00, -8.438177e+00, 4.032454e+00, -7.749310e-01 }, + { 7.750000e-02, -8.872072e-03, 2.886006e-02, -3.106564e-02, 5.907475e-03, 1.001469e-02, -4.953130e-03, -4.685044e+00, 8.837029e+00, -8.385595e+00, 4.002069e+00, -7.683518e-01 }, + { 8.000000e-02, -7.166812e-03, 2.011757e-02, -1.316371e-02, -1.265740e-02, 1.985163e-02, -7.108202e-03, -4.673121e+00, 8.796033e+00, -8.332558e+00, 3.971537e+00, -7.617657e-01 }, + { 8.250000e-02, -9.426945e-03, 3.047025e-02, -3.283879e-02, 6.510128e-03, 1.035978e-02, -5.221667e-03, -4.661315e+00, 8.755664e+00, -8.280619e+00, 3.941806e+00, -7.553910e-01 }, + { 8.500000e-02, 8.222838e-03, -2.416405e-02, 1.977743e-02, 7.181679e-03, -1.771974e-02, 6.872273e-03, -4.649176e+00, 8.714288e+00, -8.227518e+00, 3.911470e+00, -7.488967e-01 }, + { 8.750000e-02, -8.173960e-03, 2.316192e-02, -1.691800e-02, -1.061759e-02, 1.969645e-02, -7.344939e-03, -4.637129e+00, 8.673467e+00, -8.175430e+00, 3.881889e+00, -7.426047e-01 }, + { 9.000000e-02, -1.025917e-02, 3.283157e-02, -3.547485e-02, 7.506838e-03, 1.078563e-02, -5.614709e-03, -4.624899e+00, 8.632164e+00, -8.122873e+00, 3.852114e+00, -7.362844e-01 }, + { 9.250000e-02, -8.226243e-03, 2.259511e-02, -1.478790e-02, -1.380563e-02, 2.208133e-02, -8.113738e-03, -4.612398e+00, 8.590112e+00, -8.069544e+00, 3.821991e+00, -7.299079e-01 }, + { 9.500000e-02, -9.065140e-03, 2.603531e-02, -2.089313e-02, -8.181670e-03, 1.948230e-02, -7.670150e-03, -4.599958e+00, 8.548484e+00, -8.017016e+00, 3.792470e+00, -7.236920e-01 }, + { 9.750000e-02, -8.586297e-03, 2.323933e-02, -1.485478e-02, -1.472859e-02, 2.316504e-02, -8.566484e-03, -4.587390e+00, 8.506613e+00, -7.964385e+00, 3.762998e+00, -7.175090e-01 }, + { 1.000000e-01, 9.382142e-03, -2.644025e-02, 2.048966e-02, 9.564682e-03, -2.079652e-02, 8.175378e-03, -4.574718e+00, 8.464577e+00, -7.911754e+00, 3.733635e+00, -7.113713e-01 }, + { 1.025000e-01, -1.051234e-02, 3.116233e-02, -2.906302e-02, -1.418924e-03, 1.684270e-02, -7.433235e-03, -4.562008e+00, 8.422622e+00, -7.859453e+00, 3.704580e+00, -7.053246e-01 }, + { 1.050000e-01, -1.057544e-02, 3.090188e-02, -2.800164e-02, -2.929334e-03, 1.790784e-02, -7.778069e-03, -4.549137e+00, 8.380309e+00, -7.806894e+00, 3.675475e+00, -6.992864e-01 }, + { 1.075000e-01, -9.819528e-03, 2.679775e-02, -1.949433e-02, -1.189741e-02, 2.283750e-02, -8.956156e-03, -4.536160e+00, 8.337846e+00, -7.754364e+00, 3.646500e+00, -6.932983e-01 }, + { 1.100000e-01, -9.837142e-03, 2.631796e-02, -1.803808e-02, -1.378097e-02, 2.409353e-02, -9.350150e-03, -4.523005e+00, 8.294947e+00, -7.701432e+00, 3.617361e+00, -6.872861e-01 }, + { 1.125000e-01, -1.017639e-02, 2.726130e-02, -1.929095e-02, -1.292722e-02, 2.388416e-02, -9.413688e-03, -4.509923e+00, 8.252542e+00, -7.649413e+00, 3.588899e+00, -6.814531e-01 }, + { 1.150000e-01, 1.015923e-02, -2.667364e-02, 1.772987e-02, 1.486872e-02, -2.517905e-02, 9.831870e-03, -4.496610e+00, 8.209529e+00, -7.596769e+00, 3.560141e+00, -6.755654e-01 }, + { 1.175000e-01, 1.037119e-02, -2.701528e-02, 1.784158e-02, 1.515471e-02, -2.557241e-02, 1.003775e-02, -4.483262e+00, 8.166628e+00, -7.544507e+00, 3.531722e+00, -6.697748e-01 }, + { 1.200000e-01, -1.181257e-02, 3.304204e-02, -2.896439e-02, -4.340824e-03, 2.014327e-02, -8.971720e-03, -4.469878e+00, 8.123805e+00, -7.492553e+00, 3.503579e+00, -6.640636e-01 }, + { 1.225000e-01, -1.140189e-02, 3.054412e-02, -2.368378e-02, -1.000573e-02, 2.335725e-02, -9.808852e-03, -4.456289e+00, 8.080500e+00, -7.440172e+00, 3.475276e+00, -6.583322e-01 }, + { 1.250000e-01, -1.172615e-02, 3.138566e-02, -2.482201e-02, -9.203515e-03, 2.315659e-02, -9.891394e-03, -4.442648e+00, 8.037247e+00, -7.388084e+00, 3.447252e+00, -6.526826e-01 }, + { 1.275000e-01, 1.113231e-02, -2.803974e-02, 1.796158e-02, 1.640591e-02, -2.715257e-02, 1.090353e-02, -4.428866e+00, 7.993717e+00, -7.335823e+00, 3.419205e+00, -6.470409e-01 }, + { 1.300000e-01, -1.192680e-02, 3.102273e-02, -2.324996e-02, -1.141183e-02, 2.474397e-02, -1.050723e-02, -4.415045e+00, 7.950276e+00, -7.283885e+00, 3.391442e+00, -6.414791e-01 }, + { 1.325000e-01, -1.342461e-02, 3.708457e-02, -3.436839e-02, -5.178640e-04, 1.914481e-02, -9.373337e-03, -4.401253e+00, 7.907149e+00, -7.232556e+00, 3.364128e+00, -6.360344e-01 }, + { 1.350000e-01, -1.224600e-02, 3.119792e-02, -2.280918e-02, -1.242577e-02, 2.568907e-02, -1.099876e-02, -4.387084e+00, 7.862972e+00, -7.180063e+00, 3.336208e+00, -6.304650e-01 }, + { 1.375000e-01, -1.269449e-02, 3.251642e-02, -2.490941e-02, -1.058515e-02, 2.488532e-02, -1.095170e-02, -4.372937e+00, 7.819085e+00, -7.128139e+00, 3.308707e+00, -6.250039e-01 }, + { 1.400000e-01, -1.306734e-02, 3.351232e-02, -2.643378e-02, -9.316411e-03, 2.439463e-02, -1.098492e-02, -4.358732e+00, 7.775238e+00, -7.076481e+00, 3.281458e+00, -6.196159e-01 }, + { 1.425000e-01, -1.321084e-02, 3.345441e-02, -2.595864e-02, -1.004094e-02, 2.494488e-02, -1.125211e-02, -4.344372e+00, 7.731086e+00, -7.024612e+00, 3.254155e+00, -6.142263e-01 }, + { 1.450000e-01, -1.448517e-02, 3.840976e-02, -3.502563e-02, -1.121088e-03, 2.031519e-02, -1.033286e-02, -4.330133e+00, 7.687573e+00, -6.973777e+00, 3.227554e+00, -6.090114e-01 }, + { 1.475000e-01, -1.340781e-02, 3.296232e-02, -2.440192e-02, -1.205405e-02, 2.634153e-02, -1.187308e-02, -4.315433e+00, 7.642748e+00, -6.921436e+00, 3.200138e+00, -6.036213e-01 }, + { 1.500000e-01, -1.468242e-02, 3.776062e-02, -3.306761e-02, -3.553736e-03, 2.188725e-02, -1.097845e-02, -4.300971e+00, 7.598949e+00, -6.870624e+00, 3.173712e+00, -5.984710e-01 }, + { 1.525000e-01, -1.370549e-02, 3.277409e-02, -2.339364e-02, -1.349318e-02, 2.737231e-02, -1.240719e-02, -4.286082e+00, 7.553951e+00, -6.818436e+00, 3.146533e+00, -5.931559e-01 }, + { 1.550000e-01, -1.506688e-02, 3.795203e-02, -3.285074e-02, -4.147176e-03, 2.243917e-02, -1.140633e-02, -4.271483e+00, 7.510148e+00, -6.767990e+00, 3.120466e+00, -5.881084e-01 }, + { 1.575000e-01, -1.401410e-02, 3.269194e-02, -2.275765e-02, -1.450219e-02, 2.817783e-02, -1.292292e-02, -4.256418e+00, 7.465049e+00, -6.716069e+00, 3.093604e+00, -5.828895e-01 }, + { 1.600000e-01, -1.388180e-02, 3.142773e-02, -2.020277e-02, -1.724335e-02, 2.979110e-02, -1.347694e-02, -4.241484e+00, 7.420626e+00, -6.665220e+00, 3.067457e+00, -5.778465e-01 }, + { 1.625000e-01, -1.483966e-02, 3.467070e-02, -2.594218e-02, -1.164682e-02, 2.681908e-02, -1.292096e-02, -4.226409e+00, 7.375945e+00, -6.614190e+00, 3.041250e+00, -5.727945e-01 }, + { 1.650000e-01, -1.439244e-02, 3.206491e-02, -2.095200e-02, -1.681579e-02, 2.974362e-02, -1.380003e-02, -4.211165e+00, 7.330956e+00, -6.562956e+00, 3.014996e+00, -5.677416e-01 }, + { 1.675000e-01, -1.495264e-02, 3.357522e-02, -2.349927e-02, -1.439986e-02, 2.846461e-02, -1.364574e-02, -4.196006e+00, 7.286493e+00, -6.512598e+00, 2.989339e+00, -5.628375e-01 }, + { 1.700000e-01, -1.511957e-02, 3.356405e-02, -2.337181e-02, -1.468022e-02, 2.873063e-02, -1.390616e-02, -4.180640e+00, 7.241595e+00, -6.461860e+00, 2.963521e+00, -5.579044e-01 }, + { 1.725000e-01, -1.555365e-02, 3.459256e-02, -2.513730e-02, -1.303459e-02, 2.789146e-02, -1.388429e-02, -4.165199e+00, 7.196683e+00, -6.411274e+00, 2.937850e+00, -5.530119e-01 }, + { 1.750000e-01, -1.541292e-02, 3.311801e-02, -2.222737e-02, -1.607977e-02, 2.956644e-02, -1.445204e-02, -4.149692e+00, 7.151808e+00, -6.360922e+00, 2.912388e+00, -5.481766e-01 }, + { 1.775000e-01, -1.565984e-02, 3.348376e-02, -2.293462e-02, -1.548756e-02, 2.935457e-02, -1.462610e-02, -4.134013e+00, 7.106605e+00, -6.310310e+00, 2.886822e+00, -5.433216e-01 }, + { 1.800000e-01, -1.578969e-02, 3.315306e-02, -2.221304e-02, -1.632402e-02, 2.982330e-02, -1.492128e-02, -4.118298e+00, 7.061541e+00, -6.260061e+00, 2.861537e+00, -5.385405e-01 }, + { 1.825000e-01, -1.610868e-02, 3.359472e-02, -2.293283e-02, -1.571058e-02, 2.948937e-02, -1.502614e-02, -4.102523e+00, 7.016532e+00, -6.210061e+00, 2.836462e+00, -5.338152e-01 }, + { 1.850000e-01, -1.620434e-02, 3.321041e-02, -2.225371e-02, -1.649290e-02, 2.995913e-02, -1.535787e-02, -4.086605e+00, 6.971300e+00, -6.159931e+00, 2.811356e+00, -5.290861e-01 }, + { 1.875000e-01, -1.642493e-02, 3.322489e-02, -2.224394e-02, -1.660129e-02, 3.000431e-02, -1.556614e-02, -4.070597e+00, 6.926031e+00, -6.109929e+00, 2.786386e+00, -5.243956e-01 }, + { 1.900000e-01, -1.722250e-02, 3.557102e-02, -2.650956e-02, -1.241280e-02, 2.766324e-02, -1.518434e-02, -4.054651e+00, 6.881214e+00, -6.060677e+00, 2.761917e+00, -5.198281e-01 }, + { 1.925000e-01, -1.698009e-02, 3.374138e-02, -2.323041e-02, -1.580890e-02, 2.954188e-02, -1.587667e-02, -4.038414e+00, 6.835734e+00, -6.010761e+00, 2.737118e+00, -5.151907e-01 }, + { 1.950000e-01, -1.794867e-02, 3.665316e-02, -2.850630e-02, -1.058606e-02, 2.655377e-02, -1.531475e-02, -4.022282e+00, 6.790811e+00, -5.961683e+00, 2.712845e+00, -5.106765e-01 }, + { 1.975000e-01, -1.794329e-02, 3.564865e-02, -2.669683e-02, -1.249809e-02, 2.753918e-02, -1.576577e-02, -4.005909e+00, 6.745390e+00, -5.912154e+00, 2.688366e+00, -5.061204e-01 }, + { 2.000000e-01, -1.729883e-02, 3.239010e-02, -2.107865e-02, -1.822635e-02, 3.076672e-02, -1.686497e-02, -3.989444e+00, 6.699957e+00, -5.862791e+00, 2.664047e+00, -5.016085e-01 }, + { 2.025000e-01, -1.772871e-02, 3.299156e-02, -2.214640e-02, -1.725004e-02, 3.007854e-02, -1.687813e-02, -3.972902e+00, 6.654508e+00, -5.813535e+00, 2.639820e+00, -4.971175e-01 }, + { 2.050000e-01, -1.812021e-02, 3.354211e-02, -2.324432e-02, -1.623752e-02, 2.940495e-02, -1.692692e-02, -3.956327e+00, 6.609220e+00, -5.764646e+00, 2.615859e+00, -4.926929e-01 }, + { 2.075000e-01, 1.788185e-02, -3.174846e-02, 2.027018e-02, 1.929588e-02, -3.104556e-02, 1.760706e-02, -3.939654e+00, 6.563892e+00, -5.715873e+00, 2.592019e+00, -4.883019e-01 }, + { 2.100000e-01, -1.809079e-02, 3.160317e-02, -2.020966e-02, -1.944542e-02, 3.101663e-02, -1.784396e-02, -3.922903e+00, 6.518573e+00, -5.667257e+00, 2.568311e+00, -4.839435e-01 }, + { 2.125000e-01, 1.864039e-02, -3.270708e-02, 2.236978e-02, 1.737444e-02, -2.970854e-02, 1.774041e-02, -3.906098e+00, 6.473353e+00, -5.618918e+00, 2.544811e+00, -4.796365e-01 }, + { 2.150000e-01, -1.877347e-02, 3.222307e-02, -2.174713e-02, -1.809644e-02, 2.997232e-02, -1.806193e-02, -3.889128e+00, 6.427872e+00, -5.570394e+00, 2.521237e+00, -4.753135e-01 }, + { 2.175000e-01, 1.879223e-02, -3.136265e-02, 2.054185e-02, 1.939502e-02, -3.057001e-02, 1.849238e-02, -3.872156e+00, 6.382660e+00, -5.522366e+00, 2.498004e+00, -4.710742e-01 }, + { 2.200000e-01, -1.898487e-02, 3.109918e-02, -2.038745e-02, -1.965261e-02, 3.055183e-02, -1.875616e-02, -3.855041e+00, 6.337266e+00, -5.474250e+00, 2.474755e+00, -4.668318e-01 }, + { 2.225000e-01, -1.998622e-02, 3.371540e-02, -2.524977e-02, -1.488762e-02, 2.760887e-02, -1.820435e-02, -3.837875e+00, 6.291959e+00, -5.426363e+00, 2.451662e+00, -4.626245e-01 }, + { 2.250000e-01, -1.953616e-02, 3.104349e-02, -2.091980e-02, -1.934576e-02, 2.996614e-02, -1.912690e-02, -3.820577e+00, 6.246554e+00, -5.378533e+00, 2.428663e+00, -4.584451e-01 }, + { 2.275000e-01, -1.993634e-02, 3.144058e-02, -2.197981e-02, -1.838341e-02, 2.916658e-02, -1.917183e-02, -3.803316e+00, 6.201503e+00, -5.331263e+00, 2.406016e+00, -4.543469e-01 }, + { 2.300000e-01, -2.062113e-02, 3.290716e-02, -2.497414e-02, -1.550239e-02, 2.728267e-02, -1.894095e-02, -3.785947e+00, 6.156395e+00, -5.284061e+00, 2.383446e+00, -4.502683e-01 }, + { 2.325000e-01, -2.040561e-02, 3.106166e-02, -2.218764e-02, -1.844447e-02, 2.873613e-02, -1.964192e-02, -3.768338e+00, 6.110843e+00, -5.236451e+00, 2.360671e+00, -4.461417e-01 }, + { 2.350000e-01, -2.147974e-02, 3.374553e-02, -2.726145e-02, -1.349154e-02, 2.556251e-02, -1.901871e-02, -3.750842e+00, 6.065879e+00, -5.189689e+00, 2.338420e+00, -4.421382e-01 }, + { 2.375000e-01, -2.076940e-02, 3.024688e-02, -2.177075e-02, -1.916061e-02, 2.861687e-02, -2.022414e-02, -3.733061e+00, 6.020365e+00, -5.142400e+00, 2.315901e+00, -4.380720e-01 }, + { 2.400000e-01, -2.195591e-02, 3.331885e-02, -2.761338e-02, -1.343554e-02, 2.495900e-02, -1.947444e-02, -3.715492e+00, 5.975720e+00, -5.096290e+00, 2.294091e+00, -4.341720e-01 }, + { 2.425000e-01, -2.231199e-02, 3.332121e-02, -2.808680e-02, -1.313027e-02, 2.440305e-02, -1.959056e-02, -3.697675e+00, 5.930614e+00, -5.049733e+00, 2.272043e+00, -4.302134e-01 }, + { 2.450000e-01, -2.228158e-02, 3.210347e-02, -2.658983e-02, -1.483176e-02, 2.507443e-02, -2.011776e-02, -3.679676e+00, 5.885265e+00, -5.003027e+00, 2.249948e+00, -4.262448e-01 }, + { 2.475000e-01, -2.156202e-02, 2.855235e-02, -2.115712e-02, -2.049027e-02, 2.806173e-02, -2.134410e-02, -3.661604e+00, 5.840003e+00, -4.956574e+00, 2.228042e+00, -4.223228e-01 }, + { 2.500000e-01, -2.218616e-02, 2.947092e-02, -2.334593e-02, -1.850965e-02, 2.649985e-02, -2.120181e-02, -3.643553e+00, 5.795039e+00, -4.910570e+00, 2.206402e+00, -4.184580e-01 } +}; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 68bc39516b..61694d3a1d 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -25,12 +25,13 @@ #include "common/axis.h" #include "common/maths.h" - +#if 1 #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" +#endif #include "rx/rx.h" #include "io/gimbal.h" #include "io/escservo.h" @@ -42,7 +43,7 @@ #include "config/runtime_config.h" #include "config/config.h" -#include "notch_table.h" +#include "lowpass_table.h" #define GIMBAL_SERVO_PITCH 0 #define GIMBAL_SERVO_ROLL 1 @@ -68,7 +69,7 @@ static gimbalConfig_t *gimbalConfig; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static mixerMode_e currentMixerMode; -static notchFilter_t notchFilters[MAX_SUPPORTED_SERVOS]; +static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -231,6 +232,8 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon gimbalConfig = gimbalConfigToUse; } +#if 1 + int16_t determineServoMiddleOrForwardFromChannel(int nr) { uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel; @@ -656,8 +659,9 @@ bool isMixerUsingServos(void) { return useServo; } +#endif - +/* static float notchFilter(notchFilter_t *filter, float in, int16_t freqIdx ) { int16_t coefIdx; @@ -694,14 +698,55 @@ static float notchFilter(notchFilter_t *filter, float in, int16_t freqIdx ) return out; } +*/ + +static float lowpass(lowpass_t *filter, float in, int16_t freqIdx ) +{ + int16_t coefIdx; + float out; + + // Check to see if cutoff frequency changed + if (freqIdx != filter->freqIdx) { + filter->init = false; + } + + // Initialize if needed + if (!filter->init) { + filter->freqIdx = freqIdx; + filter->freq = lowpass_table[filter->freqIdx][0]; + filter->pCoef = &lowpass_table[filter->freqIdx][1]; + for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + filter->in[coefIdx] = in; + filter->out[coefIdx] = in; + } + filter->init = true; + } + + // Delays + for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { + filter->in[coefIdx] = filter->in[coefIdx-1]; + filter->out[coefIdx] = filter->out[coefIdx-1]; + } + filter->in[0] = in; + + // Accumulate result + out = filter->in[0] * filter->pCoef[0]; + for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + out += filter->in[coefIdx] * filter->pCoef[coefIdx]; + out -= filter->out[coefIdx] * filter->pCoef[coefIdx + LOWPASS_NUM_COEF - 1]; + } + filter->out[0] = out; + + return out; +} void filterServos(void) { int16_t servoIdx; - if (mixerConfig->servo_notch_enable) { + if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = notchFilter(¬chFilters[servoIdx], servo[servoIdx], mixerConfig->servo_notch_freq_idx); + servo[servoIdx] = (int16_t)lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx); } } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1f05e5abde..ea035dcd31 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -66,8 +66,8 @@ typedef struct mixer_t { typedef struct mixerConfig_s { int8_t yaw_direction; uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - int16_t servo_notch_freq_idx; // notch filter frequency selection - int8_t servo_notch_enable; // enable/disable notch filter + int16_t servo_lowpass_freq_idx; // lowpass servo filter frequency selection + int8_t servo_lowpass_enable; // enable/disable lowpass filter } mixerConfig_t; typedef struct flight3DConfig_s { @@ -92,15 +92,15 @@ typedef struct servoParam_t { int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED } servoParam_t; -#define NOTCH_FILTER_COEFS 3 -typedef struct notchFilter_t { +#define LOWPASS_NUM_COEF 6 +typedef struct lowpass_t { bool init; int16_t freqIdx; float freq; float *pCoef; - float in[NOTCH_FILTER_COEFS]; - float out[NOTCH_FILTER_COEFS]; -} notchFilter_t; + float in[LOWPASS_NUM_COEF]; + float out[LOWPASS_NUM_COEF]; +} lowpass_t; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eb94825cb8..8ab7281499 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -332,8 +332,8 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, - { "servo_notch_freq_idx", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_notch_freq_idx, 0, 99 }, - { "servo_notch_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_notch_enable, 0, 1 }, + { "servo_lowpass_freq_idx", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq_idx, 0, 99}, + { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, From a3cc92347b0074303b6a889f7eac71a33191325e Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Fri, 2 Jan 2015 16:59:42 -0500 Subject: [PATCH 06/24] Lowpass filter tested --- src/main/flight/lowpass_table.h | 201 ++++++++++++++++---------------- src/main/flight/mixer.c | 23 ++-- src/main/flight/mixer.h | 7 +- 3 files changed, 117 insertions(+), 114 deletions(-) mode change 100644 => 100755 src/main/flight/mixer.c diff --git a/src/main/flight/lowpass_table.h b/src/main/flight/lowpass_table.h index a4b5ce7c4e..b89680bfb4 100644 --- a/src/main/flight/lowpass_table.h +++ b/src/main/flight/lowpass_table.h @@ -1,104 +1,103 @@ #define LOWPASS_FILTER_NUM 100 -#define LOWPASS_NUM_COEF 6 static float lowpass_table[][12] = { - { 2.500000e-03, -3.425227e-02, 1.065271e-01, -8.631566e-02, -3.905318e-02, 8.220568e-02, -2.911187e-02, -4.356644e+00, 7.533406e+00, -6.457224e+00, 2.740841e+00, -4.603782e-01 }, - { 5.000000e-03, 9.594949e-04, -3.757786e-03, 5.699713e-03, -4.135192e-03, 1.404994e-03, -1.712205e-04, -4.818173e+00, 9.277064e+00, -8.922091e+00, 4.285685e+00, -8.224846e-01 }, - { 7.500000e-03, 1.743498e-04, -4.836488e-04, 4.240127e-04, -7.912133e-05, -5.077890e-05, 1.520828e-05, -4.768018e+00, 9.080466e+00, -8.633118e+00, 4.096914e+00, -7.762436e-01 }, - { 1.000000e-02, 2.786778e-05, -1.048774e-04, 1.567428e-04, -1.169318e-04, 4.382585e-05, -6.624334e-06, -4.969154e+00, 9.877940e+00, -9.818872e+00, 4.880540e+00, -9.704543e-01 }, - { 1.250000e-02, 3.771678e-04, -1.059885e-03, 1.107245e-03, -5.459644e-04, 1.253553e-04, -3.589356e-06, -4.553322e+00, 8.240587e+00, -7.400896e+00, 3.293345e+00, -5.797131e-01 }, - { 1.500000e-02, 3.641343e-04, -1.670771e-03, 3.089786e-03, -2.876643e-03, 1.346476e-03, -2.529602e-04, -4.953038e+00, 9.815151e+00, -9.727146e+00, 4.820991e+00, -9.559584e-01 }, - { 1.750000e-02, 1.620037e-03, -7.258472e-03, 1.291276e-02, -1.136920e-02, 4.934118e-03, -8.391887e-04, -4.943426e+00, 9.777900e+00, -9.673006e+00, 4.786021e+00, -9.474886e-01 }, - { 2.000000e-02, 3.101550e-04, -1.237727e-03, 1.926983e-03, -1.432888e-03, 4.853767e-04, -5.180650e-05, -4.936123e+00, 9.749890e+00, -9.632735e+00, 4.760297e+00, -9.413297e-01 }, - { 2.250000e-02, 5.534977e-04, -2.260520e-03, 3.607053e-03, -2.758637e-03, 9.713944e-04, -1.126195e-04, -4.927408e+00, 9.716508e+00, -9.584794e+00, 4.729704e+00, -9.340104e-01 }, - { 2.500000e-02, 5.408189e-04, -2.373426e-03, 4.248880e-03, -3.870097e-03, 1.784493e-03, -3.303950e-04, -4.919587e+00, 9.686785e+00, -9.542449e+00, 4.702905e+00, -9.276541e-01 }, - { 2.750000e-02, 3.502801e-04, -1.361984e-03, 2.079191e-03, -1.509031e-03, 4.785137e-04, -3.652179e-05, -4.910569e+00, 9.652561e+00, -9.493750e+00, 4.672115e+00, -9.203566e-01 }, - { 3.000000e-02, 1.453611e-03, -5.612347e-03, 8.217122e-03, -5.417332e-03, 1.368011e-03, -8.318679e-06, -4.899744e+00, 9.611505e+00, -9.435349e+00, 4.635192e+00, -9.116028e-01 }, - { 3.250000e-02, 1.074577e-03, -3.989564e-03, 5.517087e-03, -3.244842e-03, 5.280205e-04, 1.158066e-04, -4.891322e+00, 9.579953e+00, -9.391041e+00, 4.607554e+00, -9.051436e-01 }, - { 3.500000e-02, 2.005131e-03, -7.736833e-03, 1.123183e-02, -7.164746e-03, 1.561016e-03, 1.052714e-04, -4.880244e+00, 9.538327e+00, -9.332365e+00, 4.570791e+00, -8.965060e-01 }, - { 3.750000e-02, 1.311003e-03, -4.668294e-03, 6.054300e-03, -3.080307e-03, 1.247654e-04, 2.607561e-04, -4.872694e+00, 9.510470e+00, -9.293865e+00, 4.547176e+00, -8.910848e-01 }, - { 4.000000e-02, 2.516459e-03, -1.023862e-02, 1.640094e-02, -1.270471e-02, 4.590988e-03, -5.619403e-04, -4.862729e+00, 9.473496e+00, -9.242415e+00, 4.515363e+00, -8.837123e-01 }, - { 4.250000e-02, 3.998376e-03, -1.431055e-02, 1.806231e-02, -7.926506e-03, -1.066708e-03, 1.248077e-03, -4.846160e+00, 9.411465e+00, -9.155203e+00, 4.460800e+00, -8.708966e-01 }, - { 4.500000e-02, -4.348069e-03, 1.480567e-02, -1.677925e-02, 4.499779e-03, 3.831973e-03, -2.016955e-03, -4.834516e+00, 9.368539e+00, -9.095813e+00, 4.424264e+00, -8.624668e-01 }, - { 4.750000e-02, 3.535345e-03, -1.324834e-02, 1.871796e-02, -1.156596e-02, 2.323784e-03, 2.450004e-04, -4.830814e+00, 9.356017e+00, -9.080188e+00, 4.415799e+00, -8.608062e-01 }, - { 5.000000e-02, 5.595668e-03, -2.078517e-02, 2.858994e-02, -1.637592e-02, 2.183922e-03, 8.027722e-04, -4.815366e+00, 9.299016e+00, -9.001167e+00, 4.367034e+00, -8.495060e-01 }, - { 5.250000e-02, -6.249993e-03, 2.101144e-02, -2.299353e-02, 4.533406e-03, 7.073653e-03, -3.391119e-03, -4.797826e+00, 9.234405e+00, -8.911684e+00, 4.311829e+00, -8.367090e-01 }, - { 5.500000e-02, -6.072127e-03, 1.947955e-02, -1.896575e-02, -2.760329e-04, 9.832365e-03, -4.018237e-03, -4.787198e+00, 9.196202e+00, -8.860144e+00, 4.280925e+00, -8.297648e-01 }, - { 5.750000e-02, -6.744602e-03, 2.253293e-02, -2.449972e-02, 4.596967e-03, 7.829719e-03, -3.740525e-03, -4.776084e+00, 9.156358e+00, -8.806519e+00, 4.248835e+00, -8.225667e-01 }, - { 6.000000e-02, 5.831348e-03, -1.721135e-02, 1.276161e-02, 7.999284e-03, -1.450428e-02, 5.154550e-03, -4.764866e+00, 9.116354e+00, -8.752950e+00, 4.216944e+00, -8.154510e-01 }, - { 6.250000e-02, -5.829863e-03, 1.683686e-02, -1.140031e-02, -1.001138e-02, 1.592147e-02, -5.554742e-03, -4.753981e+00, 9.077766e+00, -8.701582e+00, 4.186544e+00, -8.087097e-01 }, - { 6.500000e-02, -6.396675e-03, 1.935953e-02, -1.591036e-02, -6.118638e-03, 1.439645e-02, -5.376342e-03, -4.742702e+00, 9.037914e+00, -8.648691e+00, 4.155331e+00, -8.018065e-01 }, - { 6.750000e-02, -6.124160e-03, 1.754505e-02, -1.159011e-02, -1.111439e-02, 1.730100e-02, -6.072763e-03, -4.731360e+00, 8.998035e+00, -8.596011e+00, 4.124385e+00, -7.949945e-01 }, - { 7.000000e-02, -6.487342e-03, 1.882125e-02, -1.338584e-02, -9.970211e-03, 1.708311e-02, -6.127115e-03, -4.719920e+00, 8.957991e+00, -8.543337e+00, 4.093571e+00, -7.882396e-01 }, - { 7.250000e-02, -7.902875e-03, 2.519387e-02, -2.524474e-02, 1.280493e-03, 1.170741e-02, -5.112626e-03, -4.708425e+00, 8.917950e+00, -8.490911e+00, 4.063044e+00, -7.815793e-01 }, - { 7.500000e-02, 7.246073e-03, -2.142761e-02, 1.702449e-02, 7.602053e-03, -1.655766e-02, 6.205297e-03, -4.696766e+00, 8.877511e+00, -8.438177e+00, 4.032454e+00, -7.749310e-01 }, - { 7.750000e-02, -8.872072e-03, 2.886006e-02, -3.106564e-02, 5.907475e-03, 1.001469e-02, -4.953130e-03, -4.685044e+00, 8.837029e+00, -8.385595e+00, 4.002069e+00, -7.683518e-01 }, - { 8.000000e-02, -7.166812e-03, 2.011757e-02, -1.316371e-02, -1.265740e-02, 1.985163e-02, -7.108202e-03, -4.673121e+00, 8.796033e+00, -8.332558e+00, 3.971537e+00, -7.617657e-01 }, - { 8.250000e-02, -9.426945e-03, 3.047025e-02, -3.283879e-02, 6.510128e-03, 1.035978e-02, -5.221667e-03, -4.661315e+00, 8.755664e+00, -8.280619e+00, 3.941806e+00, -7.553910e-01 }, - { 8.500000e-02, 8.222838e-03, -2.416405e-02, 1.977743e-02, 7.181679e-03, -1.771974e-02, 6.872273e-03, -4.649176e+00, 8.714288e+00, -8.227518e+00, 3.911470e+00, -7.488967e-01 }, - { 8.750000e-02, -8.173960e-03, 2.316192e-02, -1.691800e-02, -1.061759e-02, 1.969645e-02, -7.344939e-03, -4.637129e+00, 8.673467e+00, -8.175430e+00, 3.881889e+00, -7.426047e-01 }, - { 9.000000e-02, -1.025917e-02, 3.283157e-02, -3.547485e-02, 7.506838e-03, 1.078563e-02, -5.614709e-03, -4.624899e+00, 8.632164e+00, -8.122873e+00, 3.852114e+00, -7.362844e-01 }, - { 9.250000e-02, -8.226243e-03, 2.259511e-02, -1.478790e-02, -1.380563e-02, 2.208133e-02, -8.113738e-03, -4.612398e+00, 8.590112e+00, -8.069544e+00, 3.821991e+00, -7.299079e-01 }, - { 9.500000e-02, -9.065140e-03, 2.603531e-02, -2.089313e-02, -8.181670e-03, 1.948230e-02, -7.670150e-03, -4.599958e+00, 8.548484e+00, -8.017016e+00, 3.792470e+00, -7.236920e-01 }, - { 9.750000e-02, -8.586297e-03, 2.323933e-02, -1.485478e-02, -1.472859e-02, 2.316504e-02, -8.566484e-03, -4.587390e+00, 8.506613e+00, -7.964385e+00, 3.762998e+00, -7.175090e-01 }, - { 1.000000e-01, 9.382142e-03, -2.644025e-02, 2.048966e-02, 9.564682e-03, -2.079652e-02, 8.175378e-03, -4.574718e+00, 8.464577e+00, -7.911754e+00, 3.733635e+00, -7.113713e-01 }, - { 1.025000e-01, -1.051234e-02, 3.116233e-02, -2.906302e-02, -1.418924e-03, 1.684270e-02, -7.433235e-03, -4.562008e+00, 8.422622e+00, -7.859453e+00, 3.704580e+00, -7.053246e-01 }, - { 1.050000e-01, -1.057544e-02, 3.090188e-02, -2.800164e-02, -2.929334e-03, 1.790784e-02, -7.778069e-03, -4.549137e+00, 8.380309e+00, -7.806894e+00, 3.675475e+00, -6.992864e-01 }, - { 1.075000e-01, -9.819528e-03, 2.679775e-02, -1.949433e-02, -1.189741e-02, 2.283750e-02, -8.956156e-03, -4.536160e+00, 8.337846e+00, -7.754364e+00, 3.646500e+00, -6.932983e-01 }, - { 1.100000e-01, -9.837142e-03, 2.631796e-02, -1.803808e-02, -1.378097e-02, 2.409353e-02, -9.350150e-03, -4.523005e+00, 8.294947e+00, -7.701432e+00, 3.617361e+00, -6.872861e-01 }, - { 1.125000e-01, -1.017639e-02, 2.726130e-02, -1.929095e-02, -1.292722e-02, 2.388416e-02, -9.413688e-03, -4.509923e+00, 8.252542e+00, -7.649413e+00, 3.588899e+00, -6.814531e-01 }, - { 1.150000e-01, 1.015923e-02, -2.667364e-02, 1.772987e-02, 1.486872e-02, -2.517905e-02, 9.831870e-03, -4.496610e+00, 8.209529e+00, -7.596769e+00, 3.560141e+00, -6.755654e-01 }, - { 1.175000e-01, 1.037119e-02, -2.701528e-02, 1.784158e-02, 1.515471e-02, -2.557241e-02, 1.003775e-02, -4.483262e+00, 8.166628e+00, -7.544507e+00, 3.531722e+00, -6.697748e-01 }, - { 1.200000e-01, -1.181257e-02, 3.304204e-02, -2.896439e-02, -4.340824e-03, 2.014327e-02, -8.971720e-03, -4.469878e+00, 8.123805e+00, -7.492553e+00, 3.503579e+00, -6.640636e-01 }, - { 1.225000e-01, -1.140189e-02, 3.054412e-02, -2.368378e-02, -1.000573e-02, 2.335725e-02, -9.808852e-03, -4.456289e+00, 8.080500e+00, -7.440172e+00, 3.475276e+00, -6.583322e-01 }, - { 1.250000e-01, -1.172615e-02, 3.138566e-02, -2.482201e-02, -9.203515e-03, 2.315659e-02, -9.891394e-03, -4.442648e+00, 8.037247e+00, -7.388084e+00, 3.447252e+00, -6.526826e-01 }, - { 1.275000e-01, 1.113231e-02, -2.803974e-02, 1.796158e-02, 1.640591e-02, -2.715257e-02, 1.090353e-02, -4.428866e+00, 7.993717e+00, -7.335823e+00, 3.419205e+00, -6.470409e-01 }, - { 1.300000e-01, -1.192680e-02, 3.102273e-02, -2.324996e-02, -1.141183e-02, 2.474397e-02, -1.050723e-02, -4.415045e+00, 7.950276e+00, -7.283885e+00, 3.391442e+00, -6.414791e-01 }, - { 1.325000e-01, -1.342461e-02, 3.708457e-02, -3.436839e-02, -5.178640e-04, 1.914481e-02, -9.373337e-03, -4.401253e+00, 7.907149e+00, -7.232556e+00, 3.364128e+00, -6.360344e-01 }, - { 1.350000e-01, -1.224600e-02, 3.119792e-02, -2.280918e-02, -1.242577e-02, 2.568907e-02, -1.099876e-02, -4.387084e+00, 7.862972e+00, -7.180063e+00, 3.336208e+00, -6.304650e-01 }, - { 1.375000e-01, -1.269449e-02, 3.251642e-02, -2.490941e-02, -1.058515e-02, 2.488532e-02, -1.095170e-02, -4.372937e+00, 7.819085e+00, -7.128139e+00, 3.308707e+00, -6.250039e-01 }, - { 1.400000e-01, -1.306734e-02, 3.351232e-02, -2.643378e-02, -9.316411e-03, 2.439463e-02, -1.098492e-02, -4.358732e+00, 7.775238e+00, -7.076481e+00, 3.281458e+00, -6.196159e-01 }, - { 1.425000e-01, -1.321084e-02, 3.345441e-02, -2.595864e-02, -1.004094e-02, 2.494488e-02, -1.125211e-02, -4.344372e+00, 7.731086e+00, -7.024612e+00, 3.254155e+00, -6.142263e-01 }, - { 1.450000e-01, -1.448517e-02, 3.840976e-02, -3.502563e-02, -1.121088e-03, 2.031519e-02, -1.033286e-02, -4.330133e+00, 7.687573e+00, -6.973777e+00, 3.227554e+00, -6.090114e-01 }, - { 1.475000e-01, -1.340781e-02, 3.296232e-02, -2.440192e-02, -1.205405e-02, 2.634153e-02, -1.187308e-02, -4.315433e+00, 7.642748e+00, -6.921436e+00, 3.200138e+00, -6.036213e-01 }, - { 1.500000e-01, -1.468242e-02, 3.776062e-02, -3.306761e-02, -3.553736e-03, 2.188725e-02, -1.097845e-02, -4.300971e+00, 7.598949e+00, -6.870624e+00, 3.173712e+00, -5.984710e-01 }, - { 1.525000e-01, -1.370549e-02, 3.277409e-02, -2.339364e-02, -1.349318e-02, 2.737231e-02, -1.240719e-02, -4.286082e+00, 7.553951e+00, -6.818436e+00, 3.146533e+00, -5.931559e-01 }, - { 1.550000e-01, -1.506688e-02, 3.795203e-02, -3.285074e-02, -4.147176e-03, 2.243917e-02, -1.140633e-02, -4.271483e+00, 7.510148e+00, -6.767990e+00, 3.120466e+00, -5.881084e-01 }, - { 1.575000e-01, -1.401410e-02, 3.269194e-02, -2.275765e-02, -1.450219e-02, 2.817783e-02, -1.292292e-02, -4.256418e+00, 7.465049e+00, -6.716069e+00, 3.093604e+00, -5.828895e-01 }, - { 1.600000e-01, -1.388180e-02, 3.142773e-02, -2.020277e-02, -1.724335e-02, 2.979110e-02, -1.347694e-02, -4.241484e+00, 7.420626e+00, -6.665220e+00, 3.067457e+00, -5.778465e-01 }, - { 1.625000e-01, -1.483966e-02, 3.467070e-02, -2.594218e-02, -1.164682e-02, 2.681908e-02, -1.292096e-02, -4.226409e+00, 7.375945e+00, -6.614190e+00, 3.041250e+00, -5.727945e-01 }, - { 1.650000e-01, -1.439244e-02, 3.206491e-02, -2.095200e-02, -1.681579e-02, 2.974362e-02, -1.380003e-02, -4.211165e+00, 7.330956e+00, -6.562956e+00, 3.014996e+00, -5.677416e-01 }, - { 1.675000e-01, -1.495264e-02, 3.357522e-02, -2.349927e-02, -1.439986e-02, 2.846461e-02, -1.364574e-02, -4.196006e+00, 7.286493e+00, -6.512598e+00, 2.989339e+00, -5.628375e-01 }, - { 1.700000e-01, -1.511957e-02, 3.356405e-02, -2.337181e-02, -1.468022e-02, 2.873063e-02, -1.390616e-02, -4.180640e+00, 7.241595e+00, -6.461860e+00, 2.963521e+00, -5.579044e-01 }, - { 1.725000e-01, -1.555365e-02, 3.459256e-02, -2.513730e-02, -1.303459e-02, 2.789146e-02, -1.388429e-02, -4.165199e+00, 7.196683e+00, -6.411274e+00, 2.937850e+00, -5.530119e-01 }, - { 1.750000e-01, -1.541292e-02, 3.311801e-02, -2.222737e-02, -1.607977e-02, 2.956644e-02, -1.445204e-02, -4.149692e+00, 7.151808e+00, -6.360922e+00, 2.912388e+00, -5.481766e-01 }, - { 1.775000e-01, -1.565984e-02, 3.348376e-02, -2.293462e-02, -1.548756e-02, 2.935457e-02, -1.462610e-02, -4.134013e+00, 7.106605e+00, -6.310310e+00, 2.886822e+00, -5.433216e-01 }, - { 1.800000e-01, -1.578969e-02, 3.315306e-02, -2.221304e-02, -1.632402e-02, 2.982330e-02, -1.492128e-02, -4.118298e+00, 7.061541e+00, -6.260061e+00, 2.861537e+00, -5.385405e-01 }, - { 1.825000e-01, -1.610868e-02, 3.359472e-02, -2.293283e-02, -1.571058e-02, 2.948937e-02, -1.502614e-02, -4.102523e+00, 7.016532e+00, -6.210061e+00, 2.836462e+00, -5.338152e-01 }, - { 1.850000e-01, -1.620434e-02, 3.321041e-02, -2.225371e-02, -1.649290e-02, 2.995913e-02, -1.535787e-02, -4.086605e+00, 6.971300e+00, -6.159931e+00, 2.811356e+00, -5.290861e-01 }, - { 1.875000e-01, -1.642493e-02, 3.322489e-02, -2.224394e-02, -1.660129e-02, 3.000431e-02, -1.556614e-02, -4.070597e+00, 6.926031e+00, -6.109929e+00, 2.786386e+00, -5.243956e-01 }, - { 1.900000e-01, -1.722250e-02, 3.557102e-02, -2.650956e-02, -1.241280e-02, 2.766324e-02, -1.518434e-02, -4.054651e+00, 6.881214e+00, -6.060677e+00, 2.761917e+00, -5.198281e-01 }, - { 1.925000e-01, -1.698009e-02, 3.374138e-02, -2.323041e-02, -1.580890e-02, 2.954188e-02, -1.587667e-02, -4.038414e+00, 6.835734e+00, -6.010761e+00, 2.737118e+00, -5.151907e-01 }, - { 1.950000e-01, -1.794867e-02, 3.665316e-02, -2.850630e-02, -1.058606e-02, 2.655377e-02, -1.531475e-02, -4.022282e+00, 6.790811e+00, -5.961683e+00, 2.712845e+00, -5.106765e-01 }, - { 1.975000e-01, -1.794329e-02, 3.564865e-02, -2.669683e-02, -1.249809e-02, 2.753918e-02, -1.576577e-02, -4.005909e+00, 6.745390e+00, -5.912154e+00, 2.688366e+00, -5.061204e-01 }, - { 2.000000e-01, -1.729883e-02, 3.239010e-02, -2.107865e-02, -1.822635e-02, 3.076672e-02, -1.686497e-02, -3.989444e+00, 6.699957e+00, -5.862791e+00, 2.664047e+00, -5.016085e-01 }, - { 2.025000e-01, -1.772871e-02, 3.299156e-02, -2.214640e-02, -1.725004e-02, 3.007854e-02, -1.687813e-02, -3.972902e+00, 6.654508e+00, -5.813535e+00, 2.639820e+00, -4.971175e-01 }, - { 2.050000e-01, -1.812021e-02, 3.354211e-02, -2.324432e-02, -1.623752e-02, 2.940495e-02, -1.692692e-02, -3.956327e+00, 6.609220e+00, -5.764646e+00, 2.615859e+00, -4.926929e-01 }, - { 2.075000e-01, 1.788185e-02, -3.174846e-02, 2.027018e-02, 1.929588e-02, -3.104556e-02, 1.760706e-02, -3.939654e+00, 6.563892e+00, -5.715873e+00, 2.592019e+00, -4.883019e-01 }, - { 2.100000e-01, -1.809079e-02, 3.160317e-02, -2.020966e-02, -1.944542e-02, 3.101663e-02, -1.784396e-02, -3.922903e+00, 6.518573e+00, -5.667257e+00, 2.568311e+00, -4.839435e-01 }, - { 2.125000e-01, 1.864039e-02, -3.270708e-02, 2.236978e-02, 1.737444e-02, -2.970854e-02, 1.774041e-02, -3.906098e+00, 6.473353e+00, -5.618918e+00, 2.544811e+00, -4.796365e-01 }, - { 2.150000e-01, -1.877347e-02, 3.222307e-02, -2.174713e-02, -1.809644e-02, 2.997232e-02, -1.806193e-02, -3.889128e+00, 6.427872e+00, -5.570394e+00, 2.521237e+00, -4.753135e-01 }, - { 2.175000e-01, 1.879223e-02, -3.136265e-02, 2.054185e-02, 1.939502e-02, -3.057001e-02, 1.849238e-02, -3.872156e+00, 6.382660e+00, -5.522366e+00, 2.498004e+00, -4.710742e-01 }, - { 2.200000e-01, -1.898487e-02, 3.109918e-02, -2.038745e-02, -1.965261e-02, 3.055183e-02, -1.875616e-02, -3.855041e+00, 6.337266e+00, -5.474250e+00, 2.474755e+00, -4.668318e-01 }, - { 2.225000e-01, -1.998622e-02, 3.371540e-02, -2.524977e-02, -1.488762e-02, 2.760887e-02, -1.820435e-02, -3.837875e+00, 6.291959e+00, -5.426363e+00, 2.451662e+00, -4.626245e-01 }, - { 2.250000e-01, -1.953616e-02, 3.104349e-02, -2.091980e-02, -1.934576e-02, 2.996614e-02, -1.912690e-02, -3.820577e+00, 6.246554e+00, -5.378533e+00, 2.428663e+00, -4.584451e-01 }, - { 2.275000e-01, -1.993634e-02, 3.144058e-02, -2.197981e-02, -1.838341e-02, 2.916658e-02, -1.917183e-02, -3.803316e+00, 6.201503e+00, -5.331263e+00, 2.406016e+00, -4.543469e-01 }, - { 2.300000e-01, -2.062113e-02, 3.290716e-02, -2.497414e-02, -1.550239e-02, 2.728267e-02, -1.894095e-02, -3.785947e+00, 6.156395e+00, -5.284061e+00, 2.383446e+00, -4.502683e-01 }, - { 2.325000e-01, -2.040561e-02, 3.106166e-02, -2.218764e-02, -1.844447e-02, 2.873613e-02, -1.964192e-02, -3.768338e+00, 6.110843e+00, -5.236451e+00, 2.360671e+00, -4.461417e-01 }, - { 2.350000e-01, -2.147974e-02, 3.374553e-02, -2.726145e-02, -1.349154e-02, 2.556251e-02, -1.901871e-02, -3.750842e+00, 6.065879e+00, -5.189689e+00, 2.338420e+00, -4.421382e-01 }, - { 2.375000e-01, -2.076940e-02, 3.024688e-02, -2.177075e-02, -1.916061e-02, 2.861687e-02, -2.022414e-02, -3.733061e+00, 6.020365e+00, -5.142400e+00, 2.315901e+00, -4.380720e-01 }, - { 2.400000e-01, -2.195591e-02, 3.331885e-02, -2.761338e-02, -1.343554e-02, 2.495900e-02, -1.947444e-02, -3.715492e+00, 5.975720e+00, -5.096290e+00, 2.294091e+00, -4.341720e-01 }, - { 2.425000e-01, -2.231199e-02, 3.332121e-02, -2.808680e-02, -1.313027e-02, 2.440305e-02, -1.959056e-02, -3.697675e+00, 5.930614e+00, -5.049733e+00, 2.272043e+00, -4.302134e-01 }, - { 2.450000e-01, -2.228158e-02, 3.210347e-02, -2.658983e-02, -1.483176e-02, 2.507443e-02, -2.011776e-02, -3.679676e+00, 5.885265e+00, -5.003027e+00, 2.249948e+00, -4.262448e-01 }, - { 2.475000e-01, -2.156202e-02, 2.855235e-02, -2.115712e-02, -2.049027e-02, 2.806173e-02, -2.134410e-02, -3.661604e+00, 5.840003e+00, -4.956574e+00, 2.228042e+00, -4.223228e-01 }, - { 2.500000e-01, -2.218616e-02, 2.947092e-02, -2.334593e-02, -1.850965e-02, 2.649985e-02, -2.120181e-02, -3.643553e+00, 5.795039e+00, -4.910570e+00, 2.206402e+00, -4.184580e-01 } + { 2.500000e-03, 9.221512e-13, 4.610756e-12, 9.221512e-12, 9.221512e-12, 4.610756e-12, 9.221512e-13, -4.974584e+00, 9.898659e+00, -9.848470e+00, 4.899299e+00, -9.749042e-01 }, + { 5.000000e-03, 2.913775e-11, 1.456887e-10, 2.913775e-10, 2.913775e-10, 1.456887e-10, 2.913775e-11, -4.949168e+00, 9.797962e+00, -9.698857e+00, 4.800501e+00, -9.504377e-01 }, + { 7.500000e-03, 2.184986e-10, 1.092493e-09, 2.184986e-09, 2.184986e-09, 1.092493e-09, 2.184986e-10, -4.923752e+00, 9.697909e+00, -9.551143e+00, 4.703571e+00, -9.265843e-01 }, + { 1.000000e-02, 9.092865e-10, 4.546433e-09, 9.092865e-09, 9.092865e-09, 4.546433e-09, 9.092865e-10, -4.898337e+00, 9.598497e+00, -9.405308e+00, 4.608476e+00, -9.033283e-01 }, + { 1.250000e-02, 2.740509e-09, 1.370255e-08, 2.740509e-08, 2.740509e-08, 1.370255e-08, 2.740509e-09, -4.872922e+00, 9.499726e+00, -9.261332e+00, 4.515183e+00, -8.806542e-01 }, + { 1.500000e-02, 6.735032e-09, 3.367516e-08, 6.735032e-08, 6.735032e-08, 3.367516e-08, 6.735032e-09, -4.847508e+00, 9.401594e+00, -9.119197e+00, 4.423658e+00, -8.585473e-01 }, + { 1.750000e-02, 1.437797e-08, 7.188984e-08, 1.437797e-07, 1.437797e-07, 7.188984e-08, 1.437797e-08, -4.822094e+00, 9.304100e+00, -8.978882e+00, 4.333870e+00, -8.369928e-01 }, + { 2.000000e-02, 2.768871e-08, 1.384436e-07, 2.768871e-07, 2.768871e-07, 1.384436e-07, 2.768871e-08, -4.796682e+00, 9.207242e+00, -8.840370e+00, 4.245786e+00, -8.159767e-01 }, + { 2.250000e-02, 4.928701e-08, 2.464350e-07, 4.928701e-07, 4.928701e-07, 2.464350e-07, 4.928701e-08, -4.771270e+00, 9.111019e+00, -8.703641e+00, 4.159377e+00, -7.954852e-01 }, + { 2.500000e-02, 8.245336e-08, 4.122668e-07, 8.245336e-07, 8.245336e-07, 4.122668e-07, 8.245336e-08, -4.745859e+00, 9.015430e+00, -8.568676e+00, 4.074612e+00, -7.755049e-01 }, + { 2.750000e-02, 1.311842e-07, 6.559210e-07, 1.311842e-06, 1.311842e-06, 6.559210e-07, 1.311842e-07, -4.720449e+00, 8.920473e+00, -8.435457e+00, 3.991461e+00, -7.560228e-01 }, + { 3.000000e-02, 2.002421e-07, 1.001210e-06, 2.002421e-06, 2.002421e-06, 1.001210e-06, 2.002421e-07, -4.695041e+00, 8.826146e+00, -8.303967e+00, 3.909894e+00, -7.370262e-01 }, + { 3.250000e-02, 2.952011e-07, 1.476005e-06, 2.952011e-06, 2.952011e-06, 1.476005e-06, 2.952011e-07, -4.669634e+00, 8.732448e+00, -8.174185e+00, 3.829883e+00, -7.185027e-01 }, + { 3.500000e-02, 4.224882e-07, 2.112441e-06, 4.224882e-06, 4.224882e-06, 2.112441e-06, 4.224882e-07, -4.644228e+00, 8.639378e+00, -8.046096e+00, 3.751399e+00, -7.004404e-01 }, + { 3.750000e-02, 5.894198e-07, 2.947099e-06, 5.894198e-06, 5.894198e-06, 2.947099e-06, 5.894198e-07, -4.618824e+00, 8.546934e+00, -7.919679e+00, 3.674415e+00, -6.828274e-01 }, + { 4.000000e-02, 8.042356e-07, 4.021178e-06, 8.042356e-06, 8.042356e-06, 4.021178e-06, 8.042356e-07, -4.593421e+00, 8.455115e+00, -7.794918e+00, 3.598903e+00, -6.656525e-01 }, + { 4.250000e-02, 1.076127e-06, 5.380637e-06, 1.076127e-05, 1.076127e-05, 5.380637e-06, 1.076127e-06, -4.568021e+00, 8.363919e+00, -7.671796e+00, 3.524836e+00, -6.489046e-01 }, + { 4.500000e-02, 1.415266e-06, 7.076329e-06, 1.415266e-05, 1.415266e-05, 7.076329e-06, 1.415266e-06, -4.542622e+00, 8.273345e+00, -7.550293e+00, 3.452189e+00, -6.325728e-01 }, + { 4.750000e-02, 1.832823e-06, 9.164113e-06, 1.832823e-05, 1.832823e-05, 9.164113e-06, 1.832823e-06, -4.517225e+00, 8.183391e+00, -7.430394e+00, 3.380934e+00, -6.166466e-01 }, + { 5.000000e-02, 2.340991e-06, 1.170496e-05, 2.340991e-05, 2.340991e-05, 1.170496e-05, 2.340991e-06, -4.491831e+00, 8.094055e+00, -7.312081e+00, 3.311048e+00, -6.011158e-01 }, + { 5.250000e-02, 2.953005e-06, 1.476502e-05, 2.953005e-05, 2.953005e-05, 1.476502e-05, 2.953005e-06, -4.466439e+00, 8.005337e+00, -7.195337e+00, 3.242504e+00, -5.859705e-01 }, + { 5.500000e-02, 3.683149e-06, 1.841574e-05, 3.683149e-05, 3.683149e-05, 1.841574e-05, 3.683149e-06, -4.441049e+00, 7.917234e+00, -7.080145e+00, 3.175278e+00, -5.712009e-01 }, + { 5.750000e-02, 4.546774e-06, 2.273387e-05, 4.546774e-05, 4.546774e-05, 2.273387e-05, 4.546774e-06, -4.415661e+00, 7.829746e+00, -6.966488e+00, 3.109346e+00, -5.567976e-01 }, + { 6.000000e-02, 5.560309e-06, 2.780155e-05, 5.560309e-05, 5.560309e-05, 2.780155e-05, 5.560309e-06, -4.390276e+00, 7.742870e+00, -6.854349e+00, 3.044685e+00, -5.427514e-01 }, + { 6.250000e-02, 6.741264e-06, 3.370632e-05, 6.741264e-05, 6.741264e-05, 3.370632e-05, 6.741264e-06, -4.364894e+00, 7.656604e+00, -6.743713e+00, 2.981271e+00, -5.290533e-01 }, + { 6.500000e-02, 8.108240e-06, 4.054120e-05, 8.108240e-05, 8.108240e-05, 4.054120e-05, 8.108240e-06, -4.339514e+00, 7.570949e+00, -6.634562e+00, 2.919082e+00, -5.156946e-01 }, + { 6.750000e-02, 9.680930e-06, 4.840465e-05, 9.680930e-05, 9.680930e-05, 4.840465e-05, 9.680930e-06, -4.314138e+00, 7.485901e+00, -6.526881e+00, 2.858094e+00, -5.026668e-01 }, + { 7.000000e-02, 1.148013e-05, 5.740063e-05, 1.148013e-04, 1.148013e-04, 5.740063e-05, 1.148013e-05, -4.288764e+00, 7.401459e+00, -6.420653e+00, 2.798287e+00, -4.899617e-01 }, + { 7.250000e-02, 1.352771e-05, 6.763857e-05, 1.352771e-04, 1.352771e-04, 6.763857e-05, 1.352771e-05, -4.263393e+00, 7.317622e+00, -6.315862e+00, 2.739638e+00, -4.775711e-01 }, + { 7.500000e-02, 1.584668e-05, 7.923340e-05, 1.584668e-04, 1.584668e-04, 7.923340e-05, 1.584668e-05, -4.238025e+00, 7.234388e+00, -6.212494e+00, 2.682125e+00, -4.654872e-01 }, + { 7.750000e-02, 1.846110e-05, 9.230551e-05, 1.846110e-04, 1.846110e-04, 9.230551e-05, 1.846110e-05, -4.212661e+00, 7.151755e+00, -6.110531e+00, 2.625730e+00, -4.537024e-01 }, + { 8.000000e-02, 2.139615e-05, 1.069808e-04, 2.139615e-04, 2.139615e-04, 1.069808e-04, 2.139615e-05, -4.187300e+00, 7.069723e+00, -6.009958e+00, 2.570429e+00, -4.422092e-01 }, + { 8.250000e-02, 2.467809e-05, 1.233904e-04, 2.467809e-04, 2.467809e-04, 1.233904e-04, 2.467809e-05, -4.161942e+00, 6.988289e+00, -5.910761e+00, 2.516205e+00, -4.310003e-01 }, + { 8.500000e-02, 2.833425e-05, 1.416713e-04, 2.833425e-04, 2.833425e-04, 1.416713e-04, 2.833425e-05, -4.136588e+00, 6.907451e+00, -5.812923e+00, 2.463036e+00, -4.200687e-01 }, + { 8.750000e-02, 3.239306e-05, 1.619653e-04, 3.239306e-04, 3.239306e-04, 1.619653e-04, 3.239306e-05, -4.111238e+00, 6.827209e+00, -5.716430e+00, 2.410904e+00, -4.094075e-01 }, + { 9.000000e-02, 3.688400e-05, 1.844200e-04, 3.688400e-04, 3.688400e-04, 1.844200e-04, 3.688400e-05, -4.085891e+00, 6.747560e+00, -5.621267e+00, 2.359789e+00, -3.990100e-01 }, + { 9.250000e-02, 4.183762e-05, 2.091881e-04, 4.183762e-04, 4.183762e-04, 2.091881e-04, 4.183762e-05, -4.060548e+00, 6.668503e+00, -5.527418e+00, 2.309672e+00, -3.888696e-01 }, + { 9.500000e-02, 4.728552e-05, 2.364276e-04, 4.728552e-04, 4.728552e-04, 2.364276e-04, 4.728552e-05, -4.035209e+00, 6.590036e+00, -5.434870e+00, 2.260536e+00, -3.789800e-01 }, + { 9.750000e-02, 5.326034e-05, 2.663017e-04, 5.326034e-04, 5.326034e-04, 2.663017e-04, 5.326034e-05, -4.009874e+00, 6.512158e+00, -5.343607e+00, 2.212362e+00, -3.693349e-01 }, + { 1.000000e-01, 5.979578e-05, 2.989789e-04, 5.979578e-04, 5.979578e-04, 2.989789e-04, 5.979578e-05, -3.984543e+00, 6.434867e+00, -5.253615e+00, 2.165133e+00, -3.599282e-01 }, + { 1.025000e-01, 6.692653e-05, 3.346326e-04, 6.692653e-04, 6.692653e-04, 3.346326e-04, 6.692653e-05, -3.959216e+00, 6.358162e+00, -5.164880e+00, 2.118831e+00, -3.507542e-01 }, + { 1.050000e-01, 7.468830e-05, 3.734415e-04, 7.468830e-04, 7.468830e-04, 3.734415e-04, 7.468830e-05, -3.933894e+00, 6.282040e+00, -5.077387e+00, 2.073438e+00, -3.418071e-01 }, + { 1.075000e-01, 8.311783e-05, 4.155892e-04, 8.311783e-04, 8.311783e-04, 4.155892e-04, 8.311783e-05, -3.908575e+00, 6.206500e+00, -4.991123e+00, 2.028939e+00, -3.330811e-01 }, + { 1.100000e-01, 9.225282e-05, 4.612641e-04, 9.225282e-04, 9.225282e-04, 4.612641e-04, 9.225282e-05, -3.883261e+00, 6.131541e+00, -4.906073e+00, 1.985316e+00, -3.245710e-01 }, + { 1.125000e-01, 1.021320e-04, 5.106598e-04, 1.021320e-03, 1.021320e-03, 5.106598e-04, 1.021320e-04, -3.857952e+00, 6.057161e+00, -4.822224e+00, 1.942554e+00, -3.162713e-01 }, + { 1.150000e-01, 1.127949e-04, 5.639746e-04, 1.127949e-03, 1.127949e-03, 5.639746e-04, 1.127949e-04, -3.832647e+00, 5.983358e+00, -4.739562e+00, 1.900636e+00, -3.081769e-01 }, + { 1.175000e-01, 1.242823e-04, 6.214115e-04, 1.242823e-03, 1.242823e-03, 6.214115e-04, 1.242823e-04, -3.807346e+00, 5.910131e+00, -4.658073e+00, 1.859548e+00, -3.002828e-01 }, + { 1.200000e-01, 1.366357e-04, 6.831784e-04, 1.366357e-03, 1.366357e-03, 6.831784e-04, 1.366357e-04, -3.782051e+00, 5.837478e+00, -4.577744e+00, 1.819272e+00, -2.925839e-01 }, + { 1.225000e-01, 1.498975e-04, 7.494876e-04, 1.498975e-03, 1.498975e-03, 7.494876e-04, 1.498975e-04, -3.756760e+00, 5.765398e+00, -4.498561e+00, 1.779796e+00, -2.850755e-01 }, + { 1.250000e-01, 1.641112e-04, 8.205562e-04, 1.641112e-03, 1.641112e-03, 8.205562e-04, 1.641112e-04, -3.731474e+00, 5.693888e+00, -4.420512e+00, 1.741103e+00, -2.777530e-01 }, + { 1.275000e-01, 1.793212e-04, 8.966058e-04, 1.793212e-03, 1.793212e-03, 8.966058e-04, 1.793212e-04, -3.706192e+00, 5.622947e+00, -4.343583e+00, 1.703179e+00, -2.706117e-01 }, + { 1.300000e-01, 1.955725e-04, 9.778624e-04, 1.955725e-03, 1.955725e-03, 9.778624e-04, 1.955725e-04, -3.680916e+00, 5.552574e+00, -4.267762e+00, 1.666010e+00, -2.636472e-01 }, + { 1.325000e-01, 2.129113e-04, 1.064556e-03, 2.129113e-03, 2.129113e-03, 1.064556e-03, 2.129113e-04, -3.655645e+00, 5.482767e+00, -4.193035e+00, 1.629581e+00, -2.568552e-01 }, + { 1.350000e-01, 2.313845e-04, 1.156923e-03, 2.313845e-03, 2.313845e-03, 1.156923e-03, 2.313845e-04, -3.630379e+00, 5.413524e+00, -4.119390e+00, 1.593880e+00, -2.502314e-01 }, + { 1.375000e-01, 2.510400e-04, 1.255200e-03, 2.510400e-03, 2.510400e-03, 1.255200e-03, 2.510400e-04, -3.605118e+00, 5.344844e+00, -4.046813e+00, 1.558893e+00, -2.437717e-01 }, + { 1.400000e-01, 2.719264e-04, 1.359632e-03, 2.719264e-03, 2.719264e-03, 1.359632e-03, 2.719264e-04, -3.579862e+00, 5.276725e+00, -3.975294e+00, 1.524605e+00, -2.374721e-01 }, + { 1.425000e-01, 2.940931e-04, 1.470466e-03, 2.940931e-03, 2.940931e-03, 1.470466e-03, 2.940931e-04, -3.554612e+00, 5.209165e+00, -3.904819e+00, 1.491005e+00, -2.313287e-01 }, + { 1.450000e-01, 3.175906e-04, 1.587953e-03, 3.175906e-03, 3.175906e-03, 1.587953e-03, 3.175906e-04, -3.529367e+00, 5.142163e+00, -3.835375e+00, 1.458080e+00, -2.253377e-01 }, + { 1.475000e-01, 3.424698e-04, 1.712349e-03, 3.424698e-03, 3.424698e-03, 1.712349e-03, 3.424698e-04, -3.504127e+00, 5.075717e+00, -3.766952e+00, 1.425816e+00, -2.194953e-01 }, + { 1.500000e-01, 3.687827e-04, 1.843913e-03, 3.687827e-03, 3.687827e-03, 1.843913e-03, 3.687827e-04, -3.478893e+00, 5.009826e+00, -3.699536e+00, 1.394201e+00, -2.137979e-01 }, + { 1.525000e-01, 3.965819e-04, 1.982910e-03, 3.965819e-03, 3.965819e-03, 1.982910e-03, 3.965819e-04, -3.453664e+00, 4.944488e+00, -3.633116e+00, 1.363224e+00, -2.082419e-01 }, + { 1.550000e-01, 4.259210e-04, 2.129605e-03, 4.259210e-03, 4.259210e-03, 2.129605e-03, 4.259210e-04, -3.428441e+00, 4.879701e+00, -3.567680e+00, 1.332873e+00, -2.028239e-01 }, + { 1.575000e-01, 4.568540e-04, 2.284270e-03, 4.568540e-03, 4.568540e-03, 2.284270e-03, 4.568540e-04, -3.403223e+00, 4.815464e+00, -3.503216e+00, 1.303135e+00, -1.975406e-01 }, + { 1.600000e-01, 4.894361e-04, 2.447180e-03, 4.894361e-03, 4.894361e-03, 2.447180e-03, 4.894361e-04, -3.378011e+00, 4.751775e+00, -3.439713e+00, 1.274000e+00, -1.923886e-01 }, + { 1.625000e-01, 5.237228e-04, 2.618614e-03, 5.237228e-03, 5.237228e-03, 2.618614e-03, 5.237228e-04, -3.352805e+00, 4.688633e+00, -3.377160e+00, 1.245455e+00, -1.873647e-01 }, + { 1.650000e-01, 5.597706e-04, 2.798853e-03, 5.597706e-03, 5.597706e-03, 2.798853e-03, 5.597706e-04, -3.327605e+00, 4.626036e+00, -3.315544e+00, 1.217491e+00, -1.824659e-01 }, + { 1.675000e-01, 5.976365e-04, 2.988183e-03, 5.976365e-03, 5.976365e-03, 2.988183e-03, 5.976365e-04, -3.302410e+00, 4.563982e+00, -3.254854e+00, 1.190095e+00, -1.776890e-01 }, + { 1.700000e-01, 6.373785e-04, 3.186893e-03, 6.373785e-03, 6.373785e-03, 3.186893e-03, 6.373785e-04, -3.277221e+00, 4.502470e+00, -3.195080e+00, 1.163258e+00, -1.730312e-01 }, + { 1.725000e-01, 6.790551e-04, 3.395275e-03, 6.790551e-03, 6.790551e-03, 3.395275e-03, 6.790551e-04, -3.252038e+00, 4.441498e+00, -3.136210e+00, 1.136969e+00, -1.684894e-01 }, + { 1.750000e-01, 7.227254e-04, 3.613627e-03, 7.227254e-03, 7.227254e-03, 3.613627e-03, 7.227254e-04, -3.226861e+00, 4.381065e+00, -3.078233e+00, 1.111218e+00, -1.640609e-01 }, + { 1.775000e-01, 7.684494e-04, 3.842247e-03, 7.684494e-03, 7.684494e-03, 3.842247e-03, 7.684494e-04, -3.201690e+00, 4.321169e+00, -3.021139e+00, 1.085993e+00, -1.597429e-01 }, + { 1.800000e-01, 8.162875e-04, 4.081438e-03, 8.162875e-03, 8.162875e-03, 4.081438e-03, 8.162875e-04, -3.176525e+00, 4.261808e+00, -2.964916e+00, 1.061287e+00, -1.555327e-01 }, + { 1.825000e-01, 8.663011e-04, 4.331505e-03, 8.663011e-03, 8.663011e-03, 4.331505e-03, 8.663011e-04, -3.151366e+00, 4.202982e+00, -2.909555e+00, 1.037088e+00, -1.514277e-01 }, + { 1.850000e-01, 9.185519e-04, 4.592759e-03, 9.185519e-03, 9.185519e-03, 4.592759e-03, 9.185519e-04, -3.126212e+00, 4.144688e+00, -2.855043e+00, 1.013387e+00, -1.474253e-01 }, + { 1.875000e-01, 9.731024e-04, 4.865512e-03, 9.731024e-03, 9.731024e-03, 4.865512e-03, 9.731024e-04, -3.101065e+00, 4.086924e+00, -2.801372e+00, 9.901752e-01, -1.435231e-01 }, + { 1.900000e-01, 1.030016e-03, 5.150079e-03, 1.030016e-02, 1.030016e-02, 5.150079e-03, 1.030016e-03, -3.075924e+00, 4.029691e+00, -2.748530e+00, 9.674428e-01, -1.397185e-01 }, + { 1.925000e-01, 1.089356e-03, 5.446778e-03, 1.089356e-02, 1.089356e-02, 5.446778e-03, 1.089356e-03, -3.050790e+00, 3.972985e+00, -2.696507e+00, 9.451809e-01, -1.360093e-01 }, + { 1.950000e-01, 1.151186e-03, 5.755932e-03, 1.151186e-02, 1.151186e-02, 5.755932e-03, 1.151186e-03, -3.025661e+00, 3.916805e+00, -2.645294e+00, 9.233807e-01, -1.323931e-01 }, + { 1.975000e-01, 1.215573e-03, 6.077865e-03, 1.215573e-02, 1.215573e-02, 6.077865e-03, 1.215573e-03, -3.000538e+00, 3.861150e+00, -2.594879e+00, 9.020332e-01, -1.288676e-01 }, + { 2.000000e-01, 1.282581e-03, 6.412905e-03, 1.282581e-02, 1.282581e-02, 6.412905e-03, 1.282581e-03, -2.975422e+00, 3.806018e+00, -2.545253e+00, 8.811301e-01, -1.254306e-01 }, + { 2.025000e-01, 1.352277e-03, 6.761383e-03, 1.352277e-02, 1.352277e-02, 6.761383e-03, 1.352277e-03, -2.950312e+00, 3.751408e+00, -2.496406e+00, 8.606627e-01, -1.220800e-01 }, + { 2.050000e-01, 1.424727e-03, 7.123633e-03, 1.424727e-02, 1.424727e-02, 7.123633e-03, 1.424727e-03, -2.925208e+00, 3.697318e+00, -2.448328e+00, 8.406230e-01, -1.188137e-01 }, + { 2.075000e-01, 1.499998e-03, 7.499990e-03, 1.499998e-02, 1.499998e-02, 7.499990e-03, 1.499998e-03, -2.900111e+00, 3.643747e+00, -2.401010e+00, 8.210026e-01, -1.156296e-01 }, + { 2.100000e-01, 1.578159e-03, 7.890794e-03, 1.578159e-02, 1.578159e-02, 7.890794e-03, 1.578159e-03, -2.875019e+00, 3.590694e+00, -2.354441e+00, 8.017937e-01, -1.125258e-01 }, + { 2.125000e-01, 1.659277e-03, 8.296387e-03, 1.659277e-02, 1.659277e-02, 8.296387e-03, 1.659277e-03, -2.849934e+00, 3.538156e+00, -2.308613e+00, 7.829884e-01, -1.095002e-01 }, + { 2.150000e-01, 1.743423e-03, 8.717115e-03, 1.743423e-02, 1.743423e-02, 8.717115e-03, 1.743423e-03, -2.824855e+00, 3.486132e+00, -2.263516e+00, 7.645791e-01, -1.065509e-01 }, + { 2.175000e-01, 1.830665e-03, 9.153325e-03, 1.830665e-02, 1.830665e-02, 9.153325e-03, 1.830665e-03, -2.799783e+00, 3.434622e+00, -2.219139e+00, 7.465581e-01, -1.036762e-01 }, + { 2.200000e-01, 1.921074e-03, 9.605368e-03, 1.921074e-02, 1.921074e-02, 9.605368e-03, 1.921074e-03, -2.774717e+00, 3.383623e+00, -2.175475e+00, 7.289181e-01, -1.008741e-01 }, + { 2.225000e-01, 2.014719e-03, 1.007360e-02, 2.014719e-02, 2.014719e-02, 1.007360e-02, 2.014719e-03, -2.749657e+00, 3.333133e+00, -2.132514e+00, 7.116518e-01, -9.814302e-02 }, + { 2.250000e-01, 2.111673e-03, 1.055837e-02, 2.111673e-02, 2.111673e-02, 1.055837e-02, 2.111673e-03, -2.724604e+00, 3.283153e+00, -2.090246e+00, 6.947520e-01, -9.548109e-02 }, + { 2.275000e-01, 2.212008e-03, 1.106004e-02, 2.212008e-02, 2.212008e-02, 1.106004e-02, 2.212008e-03, -2.699556e+00, 3.233679e+00, -2.048664e+00, 6.782117e-01, -9.288667e-02 }, + { 2.300000e-01, 2.315794e-03, 1.157897e-02, 2.315794e-02, 2.315794e-02, 1.157897e-02, 2.315794e-03, -2.674515e+00, 3.184711e+00, -2.007756e+00, 6.620241e-01, -9.035810e-02 }, + { 2.325000e-01, 2.423106e-03, 1.211553e-02, 2.423106e-02, 2.423106e-02, 1.211553e-02, 2.423106e-03, -2.649481e+00, 3.136248e+00, -1.967516e+00, 6.461824e-01, -8.789379e-02 }, + { 2.350000e-01, 2.534016e-03, 1.267008e-02, 2.534016e-02, 2.534016e-02, 1.267008e-02, 2.534016e-03, -2.624453e+00, 3.088287e+00, -1.927933e+00, 6.306799e-01, -8.549216e-02 }, + { 2.375000e-01, 2.648598e-03, 1.324299e-02, 2.648598e-02, 2.648598e-02, 1.324299e-02, 2.648598e-03, -2.599431e+00, 3.040828e+00, -1.889000e+00, 6.155102e-01, -8.315169e-02 }, + { 2.400000e-01, 2.766928e-03, 1.383464e-02, 2.766928e-02, 2.766928e-02, 1.383464e-02, 2.766928e-03, -2.574415e+00, 2.993869e+00, -1.850707e+00, 6.006667e-01, -8.087089e-02 }, + { 2.425000e-01, 2.889078e-03, 1.444539e-02, 2.889078e-02, 2.889078e-02, 1.444539e-02, 2.889078e-03, -2.549406e+00, 2.947408e+00, -1.813047e+00, 5.861434e-01, -7.864829e-02 }, + { 2.450000e-01, 3.015126e-03, 1.507563e-02, 3.015126e-02, 3.015126e-02, 1.507563e-02, 3.015126e-03, -2.524403e+00, 2.901445e+00, -1.776010e+00, 5.719340e-01, -7.648247e-02 }, + { 2.475000e-01, 3.145146e-03, 1.572573e-02, 3.145146e-02, 3.145146e-02, 1.572573e-02, 3.145146e-03, -2.499407e+00, 2.855979e+00, -1.739588e+00, 5.580324e-01, -7.437205e-02 }, + { 2.500000e-01, 3.279216e-03, 1.639608e-02, 3.279216e-02, 3.279216e-02, 1.639608e-02, 3.279216e-03, -2.474416e+00, 2.811006e+00, -1.703772e+00, 5.444327e-01, -7.231567e-02 } }; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c old mode 100644 new mode 100755 index 61694d3a1d..f272cec13f --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -714,28 +714,29 @@ static float lowpass(lowpass_t *filter, float in, int16_t freqIdx ) if (!filter->init) { filter->freqIdx = freqIdx; filter->freq = lowpass_table[filter->freqIdx][0]; - filter->pCoef = &lowpass_table[filter->freqIdx][1]; + 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->in[coefIdx] = in; - filter->out[coefIdx] = in; + filter->x[coefIdx] = in; + filter->y[coefIdx] = in; } filter->init = true; } // Delays for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { - filter->in[coefIdx] = filter->in[coefIdx-1]; - filter->out[coefIdx] = filter->out[coefIdx-1]; + filter->x[coefIdx] = filter->x[coefIdx-1]; + filter->y[coefIdx] = filter->y[coefIdx-1]; } - filter->in[0] = in; + filter->x[0] = in; // Accumulate result - out = filter->in[0] * filter->pCoef[0]; + out = filter->x[0] * filter->b[0]; for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - out += filter->in[coefIdx] * filter->pCoef[coefIdx]; - out -= filter->out[coefIdx] * filter->pCoef[coefIdx + LOWPASS_NUM_COEF - 1]; + out += filter->x[coefIdx] * filter->b[coefIdx]; + out -= filter->y[coefIdx] * filter->a[coefIdx-1]; } - filter->out[0] = out; + filter->y[0] = out; return out; } @@ -747,6 +748,8 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { servo[servoIdx] = (int16_t)lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx); + // Sanity check + servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ea035dcd31..8b4c64bc0d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -97,9 +97,10 @@ typedef struct lowpass_t { bool init; int16_t freqIdx; float freq; - float *pCoef; - float in[LOWPASS_NUM_COEF]; - float out[LOWPASS_NUM_COEF]; + const float *b; + const float *a; + float x[LOWPASS_NUM_COEF]; + float y[LOWPASS_NUM_COEF]; } lowpass_t; extern int16_t motor[MAX_SUPPORTED_MOTORS]; From 0043fb4b12deab78ddebda5e5bec5e2f3355e266 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Fri, 2 Jan 2015 17:01:01 -0500 Subject: [PATCH 07/24] Remove unneded notch filter table --- src/main/flight/notch_table.h | 104 ---------------------------------- 1 file changed, 104 deletions(-) delete mode 100644 src/main/flight/notch_table.h diff --git a/src/main/flight/notch_table.h b/src/main/flight/notch_table.h deleted file mode 100644 index dbe0d36f4c..0000000000 --- a/src/main/flight/notch_table.h +++ /dev/null @@ -1,104 +0,0 @@ -#define NOTCH_FILTER_NUM 100 -#define NOTCH_FILTER_COEFS 3 -static float notch_table[][6] = { - { 1.750088e-03, 9.989016e-01, -1.997682e+00, 9.989016e-01, -1.997682e+00, 9.978032e-01 }, - { 3.500175e-03, 9.978056e-01, -1.995129e+00, 9.978056e-01, -1.995129e+00, 9.956112e-01 }, - { 5.250263e-03, 9.967120e-01, -1.992339e+00, 9.967120e-01, -1.992339e+00, 9.934240e-01 }, - { 7.000350e-03, 9.956208e-01, -1.989316e+00, 9.956208e-01, -1.989316e+00, 9.912416e-01 }, - { 8.750438e-03, 9.945319e-01, -1.986058e+00, 9.945319e-01, -1.986058e+00, 9.890639e-01 }, - { 1.050053e-02, 9.934455e-01, -1.982568e+00, 9.934455e-01, -1.982568e+00, 9.868910e-01 }, - { 1.225061e-02, 9.923614e-01, -1.978846e+00, 9.923614e-01, -1.978846e+00, 9.847227e-01 }, - { 1.400070e-02, 9.912796e-01, -1.974893e+00, 9.912796e-01, -1.974893e+00, 9.825592e-01 }, - { 1.575079e-02, 9.902002e-01, -1.970710e+00, 9.902002e-01, -1.970710e+00, 9.804003e-01 }, - { 1.750088e-02, 9.891230e-01, -1.966298e+00, 9.891230e-01, -1.966298e+00, 9.782461e-01 }, - { 1.925096e-02, 9.880482e-01, -1.961658e+00, 9.880482e-01, -1.961658e+00, 9.760965e-01 }, - { 2.100105e-02, 9.869758e-01, -1.956791e+00, 9.869758e-01, -1.956791e+00, 9.739515e-01 }, - { 2.275114e-02, 9.859056e-01, -1.951699e+00, 9.859056e-01, -1.951699e+00, 9.718111e-01 }, - { 2.450123e-02, 9.848376e-01, -1.946381e+00, 9.848376e-01, -1.946381e+00, 9.696753e-01 }, - { 2.625131e-02, 9.837720e-01, -1.940840e+00, 9.837720e-01, -1.940840e+00, 9.675440e-01 }, - { 2.800140e-02, 9.827086e-01, -1.935077e+00, 9.827086e-01, -1.935077e+00, 9.654173e-01 }, - { 2.975149e-02, 9.816475e-01, -1.929092e+00, 9.816475e-01, -1.929092e+00, 9.632950e-01 }, - { 3.150158e-02, 9.805887e-01, -1.922887e+00, 9.805887e-01, -1.922887e+00, 9.611773e-01 }, - { 3.325166e-02, 9.795320e-01, -1.916463e+00, 9.795320e-01, -1.916463e+00, 9.590640e-01 }, - { 3.500175e-02, 9.784776e-01, -1.909821e+00, 9.784776e-01, -1.909821e+00, 9.569552e-01 }, - { 3.675184e-02, 9.774254e-01, -1.902962e+00, 9.774254e-01, -1.902962e+00, 9.548508e-01 }, - { 3.850193e-02, 9.763754e-01, -1.895889e+00, 9.763754e-01, -1.895889e+00, 9.527509e-01 }, - { 4.025201e-02, 9.753276e-01, -1.888601e+00, 9.753276e-01, -1.888601e+00, 9.506553e-01 }, - { 4.200210e-02, 9.742820e-01, -1.881101e+00, 9.742820e-01, -1.881101e+00, 9.485641e-01 }, - { 4.375219e-02, 9.732386e-01, -1.873390e+00, 9.732386e-01, -1.873390e+00, 9.464773e-01 }, - { 4.550228e-02, 9.721974e-01, -1.865469e+00, 9.721974e-01, -1.865469e+00, 9.443948e-01 }, - { 4.725236e-02, 9.711583e-01, -1.857339e+00, 9.711583e-01, -1.857339e+00, 9.423166e-01 }, - { 4.900245e-02, 9.701213e-01, -1.849002e+00, 9.701213e-01, -1.849002e+00, 9.402427e-01 }, - { 5.075254e-02, 9.690866e-01, -1.840460e+00, 9.690866e-01, -1.840460e+00, 9.381731e-01 }, - { 5.250263e-02, 9.680539e-01, -1.831713e+00, 9.680539e-01, -1.831713e+00, 9.361078e-01 }, - { 5.425271e-02, 9.670233e-01, -1.822763e+00, 9.670233e-01, -1.822763e+00, 9.340467e-01 }, - { 5.600280e-02, 9.659949e-01, -1.813613e+00, 9.659949e-01, -1.813613e+00, 9.319898e-01 }, - { 5.775289e-02, 9.649686e-01, -1.804262e+00, 9.649686e-01, -1.804262e+00, 9.299372e-01 }, - { 5.950298e-02, 9.639444e-01, -1.794713e+00, 9.639444e-01, -1.794713e+00, 9.278888e-01 }, - { 6.125306e-02, 9.629222e-01, -1.784968e+00, 9.629222e-01, -1.784968e+00, 9.258445e-01 }, - { 6.300315e-02, 9.619022e-01, -1.775027e+00, 9.619022e-01, -1.775027e+00, 9.238044e-01 }, - { 6.475324e-02, 9.608842e-01, -1.764893e+00, 9.608842e-01, -1.764893e+00, 9.217684e-01 }, - { 6.650333e-02, 9.598683e-01, -1.754567e+00, 9.598683e-01, -1.754567e+00, 9.197366e-01 }, - { 6.825341e-02, 9.588544e-01, -1.744051e+00, 9.588544e-01, -1.744051e+00, 9.177088e-01 }, - { 7.000350e-02, 9.578426e-01, -1.733346e+00, 9.578426e-01, -1.733346e+00, 9.156852e-01 }, - { 7.175359e-02, 9.568328e-01, -1.722454e+00, 9.568328e-01, -1.722454e+00, 9.136656e-01 }, - { 7.350368e-02, 9.558250e-01, -1.711377e+00, 9.558250e-01, -1.711377e+00, 9.116501e-01 }, - { 7.525376e-02, 9.548193e-01, -1.700116e+00, 9.548193e-01, -1.700116e+00, 9.096386e-01 }, - { 7.700385e-02, 9.538156e-01, -1.688673e+00, 9.538156e-01, -1.688673e+00, 9.076311e-01 }, - { 7.875394e-02, 9.528138e-01, -1.677051e+00, 9.528138e-01, -1.677051e+00, 9.056276e-01 }, - { 8.050403e-02, 9.518141e-01, -1.665249e+00, 9.518141e-01, -1.665249e+00, 9.036282e-01 }, - { 8.225411e-02, 9.508163e-01, -1.653272e+00, 9.508163e-01, -1.653272e+00, 9.016326e-01 }, - { 8.400420e-02, 9.498205e-01, -1.641119e+00, 9.498205e-01, -1.641119e+00, 8.996411e-01 }, - { 8.575429e-02, 9.488267e-01, -1.628794e+00, 9.488267e-01, -1.628794e+00, 8.976535e-01 }, - { 8.750438e-02, 9.478349e-01, -1.616297e+00, 9.478349e-01, -1.616297e+00, 8.956698e-01 }, - { 8.925446e-02, 9.468450e-01, -1.603631e+00, 9.468450e-01, -1.603631e+00, 8.936900e-01 }, - { 9.100455e-02, 9.458570e-01, -1.590797e+00, 9.458570e-01, -1.590797e+00, 8.917140e-01 }, - { 9.275464e-02, 9.448710e-01, -1.577798e+00, 9.448710e-01, -1.577798e+00, 8.897420e-01 }, - { 9.450473e-02, 9.438869e-01, -1.564635e+00, 9.438869e-01, -1.564635e+00, 8.877738e-01 }, - { 9.625481e-02, 9.429047e-01, -1.551311e+00, 9.429047e-01, -1.551311e+00, 8.858095e-01 }, - { 9.800490e-02, 9.419245e-01, -1.537826e+00, 9.419245e-01, -1.537826e+00, 8.838489e-01 }, - { 9.975499e-02, 9.409461e-01, -1.524184e+00, 9.409461e-01, -1.524184e+00, 8.818922e-01 }, - { 1.015051e-01, 9.399697e-01, -1.510385e+00, 9.399697e-01, -1.510385e+00, 8.799393e-01 }, - { 1.032552e-01, 9.389951e-01, -1.496433e+00, 9.389951e-01, -1.496433e+00, 8.779902e-01 }, - { 1.050053e-01, 9.380224e-01, -1.482328e+00, 9.380224e-01, -1.482328e+00, 8.760448e-01 }, - { 1.067553e-01, 9.370516e-01, -1.468074e+00, 9.370516e-01, -1.468074e+00, 8.741031e-01 }, - { 1.085054e-01, 9.360826e-01, -1.453671e+00, 9.360826e-01, -1.453671e+00, 8.721652e-01 }, - { 1.102555e-01, 9.351155e-01, -1.439122e+00, 9.351155e-01, -1.439122e+00, 8.702310e-01 }, - { 1.120056e-01, 9.341502e-01, -1.424429e+00, 9.341502e-01, -1.424429e+00, 8.683005e-01 }, - { 1.137557e-01, 9.331868e-01, -1.409594e+00, 9.331868e-01, -1.409594e+00, 8.663737e-01 }, - { 1.155058e-01, 9.322253e-01, -1.394620e+00, 9.322253e-01, -1.394620e+00, 8.644505e-01 }, - { 1.172559e-01, 9.312655e-01, -1.379507e+00, 9.312655e-01, -1.379507e+00, 8.625310e-01 }, - { 1.190060e-01, 9.303076e-01, -1.364259e+00, 9.303076e-01, -1.364259e+00, 8.606152e-01 }, - { 1.207560e-01, 9.293515e-01, -1.348877e+00, 9.293515e-01, -1.348877e+00, 8.587029e-01 }, - { 1.225061e-01, 9.283971e-01, -1.333363e+00, 9.283971e-01, -1.333363e+00, 8.567943e-01 }, - { 1.242562e-01, 9.274446e-01, -1.317720e+00, 9.274446e-01, -1.317720e+00, 8.548892e-01 }, - { 1.260063e-01, 9.264939e-01, -1.301950e+00, 9.264939e-01, -1.301950e+00, 8.529878e-01 }, - { 1.277564e-01, 9.255449e-01, -1.286054e+00, 9.255449e-01, -1.286054e+00, 8.510899e-01 }, - { 1.295065e-01, 9.245978e-01, -1.270035e+00, 9.245978e-01, -1.270035e+00, 8.491955e-01 }, - { 1.312566e-01, 9.236524e-01, -1.253896e+00, 9.236524e-01, -1.253896e+00, 8.473047e-01 }, - { 1.330067e-01, 9.227087e-01, -1.237638e+00, 9.227087e-01, -1.237638e+00, 8.454174e-01 }, - { 1.347567e-01, 9.217668e-01, -1.221263e+00, 9.217668e-01, -1.221263e+00, 8.435336e-01 }, - { 1.365068e-01, 9.208267e-01, -1.204774e+00, 9.208267e-01, -1.204774e+00, 8.416533e-01 }, - { 1.382569e-01, 9.198883e-01, -1.188172e+00, 9.198883e-01, -1.188172e+00, 8.397765e-01 }, - { 1.400070e-01, 9.189516e-01, -1.171461e+00, 9.189516e-01, -1.171461e+00, 8.379032e-01 }, - { 1.417571e-01, 9.180166e-01, -1.154642e+00, 9.180166e-01, -1.154642e+00, 8.360333e-01 }, - { 1.435072e-01, 9.170834e-01, -1.137718e+00, 9.170834e-01, -1.137718e+00, 8.341668e-01 }, - { 1.452573e-01, 9.161519e-01, -1.120690e+00, 9.161519e-01, -1.120690e+00, 8.323037e-01 }, - { 1.470074e-01, 9.152220e-01, -1.103561e+00, 9.152220e-01, -1.103561e+00, 8.304441e-01 }, - { 1.487574e-01, 9.142939e-01, -1.086334e+00, 9.142939e-01, -1.086334e+00, 8.285878e-01 }, - { 1.505075e-01, 9.133675e-01, -1.069010e+00, 9.133675e-01, -1.069010e+00, 8.267350e-01 }, - { 1.522576e-01, 9.124427e-01, -1.051591e+00, 9.124427e-01, -1.051591e+00, 8.248855e-01 }, - { 1.540077e-01, 9.115196e-01, -1.034081e+00, 9.115196e-01, -1.034081e+00, 8.230393e-01 }, - { 1.557578e-01, 9.105982e-01, -1.016481e+00, 9.105982e-01, -1.016481e+00, 8.211965e-01 }, - { 1.575079e-01, 9.096785e-01, -9.987933e-01, 9.096785e-01, -9.987933e-01, 8.193570e-01 }, - { 1.592580e-01, 9.087604e-01, -9.810205e-01, 9.087604e-01, -9.810205e-01, 8.175208e-01 }, - { 1.610081e-01, 9.078440e-01, -9.631649e-01, 9.078440e-01, -9.631649e-01, 8.156879e-01 }, - { 1.627581e-01, 9.069292e-01, -9.452287e-01, 9.069292e-01, -9.452287e-01, 8.138583e-01 }, - { 1.645082e-01, 9.060160e-01, -9.272142e-01, 9.060160e-01, -9.272142e-01, 8.120320e-01 }, - { 1.662583e-01, 9.051045e-01, -9.091238e-01, 9.051045e-01, -9.091238e-01, 8.102089e-01 }, - { 1.680084e-01, 9.041946e-01, -8.909597e-01, 9.041946e-01, -8.909597e-01, 8.083891e-01 }, - { 1.697585e-01, 9.032863e-01, -8.727243e-01, 9.032863e-01, -8.727243e-01, 8.065725e-01 }, - { 1.715086e-01, 9.023796e-01, -8.544198e-01, 9.023796e-01, -8.544198e-01, 8.047592e-01 }, - { 1.732587e-01, 9.014745e-01, -8.360487e-01, 9.014745e-01, -8.360487e-01, 8.029490e-01 }, - { 1.750088e-01, 9.005710e-01, -8.176131e-01, 9.005710e-01, -8.176131e-01, 8.011420e-01 } -}; From 4bcedab3e9053b7d1bb6f1f5b718b3f5183d651e Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Fri, 2 Jan 2015 17:16:55 -0500 Subject: [PATCH 08/24] Clean up lowpass patch --- src/main/config/config.c | 2 +- src/main/flight/mixer.c | 44 +--------------------------------------- src/main/io/serial_cli.c | 4 ++-- 3 files changed, 4 insertions(+), 46 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 21de5c93b1..c9e1cc25a4 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -428,7 +428,7 @@ static void resetConf(void) currentProfile->mixerConfig.yaw_direction = 1; currentProfile->mixerConfig.tri_unarmed_servo = 1; - currentProfile->mixerConfig.servo_lowpass_freq_idx = 21; + currentProfile->mixerConfig.servo_lowpass_freq_idx = 99; currentProfile->mixerConfig.servo_lowpass_enable = 0; // gimbal diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f272cec13f..5a549e60fd 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -25,13 +25,12 @@ #include "common/axis.h" #include "common/maths.h" -#if 1 + #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" -#endif #include "rx/rx.h" #include "io/gimbal.h" #include "io/escservo.h" @@ -232,7 +231,6 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon gimbalConfig = gimbalConfigToUse; } -#if 1 int16_t determineServoMiddleOrForwardFromChannel(int nr) { @@ -659,46 +657,6 @@ bool isMixerUsingServos(void) { return useServo; } -#endif - -/* -static float notchFilter(notchFilter_t *filter, float in, int16_t freqIdx ) -{ - int16_t coefIdx; - float out; - - if (freqIdx != filter->freqIdx) { - filter->init = false; - } - - if (!filter->init) { - filter->freqIdx = freqIdx; - filter->freq = notch_table[filter->freqIdx][0]; - filter->pCoef = ¬ch_table[filter->freqIdx][1]; - for (coefIdx = 0; coefIdx < NOTCH_FILTER_COEFS; coefIdx++) { - filter->in[coefIdx] = in; - filter->out[coefIdx] = in; - } - filter->init = true; - } - - filter->in[2] = filter->in[1]; - filter->in[1] = filter->in[0]; - filter->in[0] = in; - filter->out[2] = filter->out[1]; - filter->out[1] = filter->out[0]; - - out = filter->in[0] * filter->pCoef[0] + - filter->in[1] * filter->pCoef[1] + - filter->in[2] * filter->pCoef[2] - - (filter->out[1] * filter->pCoef[3] + - filter->out[2] * filter->pCoef[4]); - - filter->out[0] = out; - - return out; -} -*/ static float lowpass(lowpass_t *filter, float in, int16_t freqIdx ) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8ab7281499..776a872d58 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -332,8 +332,8 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, - { "servo_lowpass_freq_idx", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq_idx, 0, 99}, - { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, + { "servo_lowpass_freq_idx", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq_idx, 0, 99}, + { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, From 654c614380ebb0fcc245a82999fe0c193a96af76 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Fri, 2 Jan 2015 17:47:00 -0500 Subject: [PATCH 09/24] Fix rounding; drive unused servos --- src/main/flight/mixer.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5a549e60fd..9ffd32f71a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -52,7 +52,7 @@ static uint8_t motorCount = 0; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; -int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; +int16_t servo[MAX_SUPPORTED_SERVOS]; static int useServo; @@ -511,6 +511,11 @@ void mixTable(void) int16_t maxMotor; uint32_t i; + // paranoia: give all servos a default command; prevents drift on unused servos with lowpass enabled + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = DEFAULT_SERVO_MIDDLE; + } + if (motorCount > 3) { // prevent "yaw jump" during yaw correction axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); @@ -705,7 +710,8 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = (int16_t)lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx); + // Round to nearest + servo[servoIdx] = (int16_t)(lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx) + 0.5f); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } From ad7ec4b915d0773f9fdf716ab90fc8d8c1919b3e Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Fri, 2 Jan 2015 20:03:33 -0500 Subject: [PATCH 10/24] Back out notch filter changes --- src/main/config/config.c | 2 - src/main/flight/mixer.c | 100 +------------------------------- src/main/flight/mixer.h | 13 ----- src/main/flight/notch_table.h | 104 ---------------------------------- src/main/io/serial_cli.c | 2 - src/main/mw.c | 1 - 6 files changed, 1 insertion(+), 221 deletions(-) delete mode 100644 src/main/flight/notch_table.h diff --git a/src/main/config/config.c b/src/main/config/config.c index 21de5c93b1..f1f5695754 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -428,8 +428,6 @@ static void resetConf(void) currentProfile->mixerConfig.yaw_direction = 1; currentProfile->mixerConfig.tri_unarmed_servo = 1; - currentProfile->mixerConfig.servo_lowpass_freq_idx = 21; - currentProfile->mixerConfig.servo_lowpass_enable = 0; // gimbal currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 61694d3a1d..16037d1651 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -25,13 +25,12 @@ #include "common/axis.h" #include "common/maths.h" -#if 1 + #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" -#endif #include "rx/rx.h" #include "io/gimbal.h" #include "io/escservo.h" @@ -43,8 +42,6 @@ #include "config/runtime_config.h" #include "config/config.h" -#include "lowpass_table.h" - #define GIMBAL_SERVO_PITCH 0 #define GIMBAL_SERVO_ROLL 1 @@ -69,7 +66,6 @@ static gimbalConfig_t *gimbalConfig; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static mixerMode_e currentMixerMode; -static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -232,8 +228,6 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon gimbalConfig = gimbalConfigToUse; } -#if 1 - int16_t determineServoMiddleOrForwardFromChannel(int nr) { uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel; @@ -659,95 +653,3 @@ bool isMixerUsingServos(void) { return useServo; } -#endif - -/* -static float notchFilter(notchFilter_t *filter, float in, int16_t freqIdx ) -{ - int16_t coefIdx; - float out; - - if (freqIdx != filter->freqIdx) { - filter->init = false; - } - - if (!filter->init) { - filter->freqIdx = freqIdx; - filter->freq = notch_table[filter->freqIdx][0]; - filter->pCoef = ¬ch_table[filter->freqIdx][1]; - for (coefIdx = 0; coefIdx < NOTCH_FILTER_COEFS; coefIdx++) { - filter->in[coefIdx] = in; - filter->out[coefIdx] = in; - } - filter->init = true; - } - - filter->in[2] = filter->in[1]; - filter->in[1] = filter->in[0]; - filter->in[0] = in; - filter->out[2] = filter->out[1]; - filter->out[1] = filter->out[0]; - - out = filter->in[0] * filter->pCoef[0] + - filter->in[1] * filter->pCoef[1] + - filter->in[2] * filter->pCoef[2] - - (filter->out[1] * filter->pCoef[3] + - filter->out[2] * filter->pCoef[4]); - - filter->out[0] = out; - - return out; -} -*/ - -static float lowpass(lowpass_t *filter, float in, int16_t freqIdx ) -{ - int16_t coefIdx; - float out; - - // Check to see if cutoff frequency changed - if (freqIdx != filter->freqIdx) { - filter->init = false; - } - - // Initialize if needed - if (!filter->init) { - filter->freqIdx = freqIdx; - filter->freq = lowpass_table[filter->freqIdx][0]; - filter->pCoef = &lowpass_table[filter->freqIdx][1]; - for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - filter->in[coefIdx] = in; - filter->out[coefIdx] = in; - } - filter->init = true; - } - - // Delays - for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { - filter->in[coefIdx] = filter->in[coefIdx-1]; - filter->out[coefIdx] = filter->out[coefIdx-1]; - } - filter->in[0] = in; - - // Accumulate result - out = filter->in[0] * filter->pCoef[0]; - for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - out += filter->in[coefIdx] * filter->pCoef[coefIdx]; - out -= filter->out[coefIdx] * filter->pCoef[coefIdx + LOWPASS_NUM_COEF - 1]; - } - filter->out[0] = out; - - return out; -} - -void filterServos(void) -{ - int16_t servoIdx; - - if (mixerConfig->servo_lowpass_enable) { - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = (int16_t)lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx); - } - } -} - diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ea035dcd31..f198fd11dd 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -66,8 +66,6 @@ typedef struct mixer_t { typedef struct mixerConfig_s { int8_t yaw_direction; uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - int16_t servo_lowpass_freq_idx; // lowpass servo filter frequency selection - int8_t servo_lowpass_enable; // enable/disable lowpass filter } mixerConfig_t; typedef struct flight3DConfig_s { @@ -92,16 +90,6 @@ typedef struct servoParam_t { int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED } servoParam_t; -#define LOWPASS_NUM_COEF 6 -typedef struct lowpass_t { - bool init; - int16_t freqIdx; - float freq; - float *pCoef; - float in[LOWPASS_NUM_COEF]; - float out[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]; @@ -111,6 +99,5 @@ void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerResetMotors(void); void mixTable(void); -void filterServos(void); void writeServos(void); void writeMotors(void); diff --git a/src/main/flight/notch_table.h b/src/main/flight/notch_table.h deleted file mode 100644 index dbe0d36f4c..0000000000 --- a/src/main/flight/notch_table.h +++ /dev/null @@ -1,104 +0,0 @@ -#define NOTCH_FILTER_NUM 100 -#define NOTCH_FILTER_COEFS 3 -static float notch_table[][6] = { - { 1.750088e-03, 9.989016e-01, -1.997682e+00, 9.989016e-01, -1.997682e+00, 9.978032e-01 }, - { 3.500175e-03, 9.978056e-01, -1.995129e+00, 9.978056e-01, -1.995129e+00, 9.956112e-01 }, - { 5.250263e-03, 9.967120e-01, -1.992339e+00, 9.967120e-01, -1.992339e+00, 9.934240e-01 }, - { 7.000350e-03, 9.956208e-01, -1.989316e+00, 9.956208e-01, -1.989316e+00, 9.912416e-01 }, - { 8.750438e-03, 9.945319e-01, -1.986058e+00, 9.945319e-01, -1.986058e+00, 9.890639e-01 }, - { 1.050053e-02, 9.934455e-01, -1.982568e+00, 9.934455e-01, -1.982568e+00, 9.868910e-01 }, - { 1.225061e-02, 9.923614e-01, -1.978846e+00, 9.923614e-01, -1.978846e+00, 9.847227e-01 }, - { 1.400070e-02, 9.912796e-01, -1.974893e+00, 9.912796e-01, -1.974893e+00, 9.825592e-01 }, - { 1.575079e-02, 9.902002e-01, -1.970710e+00, 9.902002e-01, -1.970710e+00, 9.804003e-01 }, - { 1.750088e-02, 9.891230e-01, -1.966298e+00, 9.891230e-01, -1.966298e+00, 9.782461e-01 }, - { 1.925096e-02, 9.880482e-01, -1.961658e+00, 9.880482e-01, -1.961658e+00, 9.760965e-01 }, - { 2.100105e-02, 9.869758e-01, -1.956791e+00, 9.869758e-01, -1.956791e+00, 9.739515e-01 }, - { 2.275114e-02, 9.859056e-01, -1.951699e+00, 9.859056e-01, -1.951699e+00, 9.718111e-01 }, - { 2.450123e-02, 9.848376e-01, -1.946381e+00, 9.848376e-01, -1.946381e+00, 9.696753e-01 }, - { 2.625131e-02, 9.837720e-01, -1.940840e+00, 9.837720e-01, -1.940840e+00, 9.675440e-01 }, - { 2.800140e-02, 9.827086e-01, -1.935077e+00, 9.827086e-01, -1.935077e+00, 9.654173e-01 }, - { 2.975149e-02, 9.816475e-01, -1.929092e+00, 9.816475e-01, -1.929092e+00, 9.632950e-01 }, - { 3.150158e-02, 9.805887e-01, -1.922887e+00, 9.805887e-01, -1.922887e+00, 9.611773e-01 }, - { 3.325166e-02, 9.795320e-01, -1.916463e+00, 9.795320e-01, -1.916463e+00, 9.590640e-01 }, - { 3.500175e-02, 9.784776e-01, -1.909821e+00, 9.784776e-01, -1.909821e+00, 9.569552e-01 }, - { 3.675184e-02, 9.774254e-01, -1.902962e+00, 9.774254e-01, -1.902962e+00, 9.548508e-01 }, - { 3.850193e-02, 9.763754e-01, -1.895889e+00, 9.763754e-01, -1.895889e+00, 9.527509e-01 }, - { 4.025201e-02, 9.753276e-01, -1.888601e+00, 9.753276e-01, -1.888601e+00, 9.506553e-01 }, - { 4.200210e-02, 9.742820e-01, -1.881101e+00, 9.742820e-01, -1.881101e+00, 9.485641e-01 }, - { 4.375219e-02, 9.732386e-01, -1.873390e+00, 9.732386e-01, -1.873390e+00, 9.464773e-01 }, - { 4.550228e-02, 9.721974e-01, -1.865469e+00, 9.721974e-01, -1.865469e+00, 9.443948e-01 }, - { 4.725236e-02, 9.711583e-01, -1.857339e+00, 9.711583e-01, -1.857339e+00, 9.423166e-01 }, - { 4.900245e-02, 9.701213e-01, -1.849002e+00, 9.701213e-01, -1.849002e+00, 9.402427e-01 }, - { 5.075254e-02, 9.690866e-01, -1.840460e+00, 9.690866e-01, -1.840460e+00, 9.381731e-01 }, - { 5.250263e-02, 9.680539e-01, -1.831713e+00, 9.680539e-01, -1.831713e+00, 9.361078e-01 }, - { 5.425271e-02, 9.670233e-01, -1.822763e+00, 9.670233e-01, -1.822763e+00, 9.340467e-01 }, - { 5.600280e-02, 9.659949e-01, -1.813613e+00, 9.659949e-01, -1.813613e+00, 9.319898e-01 }, - { 5.775289e-02, 9.649686e-01, -1.804262e+00, 9.649686e-01, -1.804262e+00, 9.299372e-01 }, - { 5.950298e-02, 9.639444e-01, -1.794713e+00, 9.639444e-01, -1.794713e+00, 9.278888e-01 }, - { 6.125306e-02, 9.629222e-01, -1.784968e+00, 9.629222e-01, -1.784968e+00, 9.258445e-01 }, - { 6.300315e-02, 9.619022e-01, -1.775027e+00, 9.619022e-01, -1.775027e+00, 9.238044e-01 }, - { 6.475324e-02, 9.608842e-01, -1.764893e+00, 9.608842e-01, -1.764893e+00, 9.217684e-01 }, - { 6.650333e-02, 9.598683e-01, -1.754567e+00, 9.598683e-01, -1.754567e+00, 9.197366e-01 }, - { 6.825341e-02, 9.588544e-01, -1.744051e+00, 9.588544e-01, -1.744051e+00, 9.177088e-01 }, - { 7.000350e-02, 9.578426e-01, -1.733346e+00, 9.578426e-01, -1.733346e+00, 9.156852e-01 }, - { 7.175359e-02, 9.568328e-01, -1.722454e+00, 9.568328e-01, -1.722454e+00, 9.136656e-01 }, - { 7.350368e-02, 9.558250e-01, -1.711377e+00, 9.558250e-01, -1.711377e+00, 9.116501e-01 }, - { 7.525376e-02, 9.548193e-01, -1.700116e+00, 9.548193e-01, -1.700116e+00, 9.096386e-01 }, - { 7.700385e-02, 9.538156e-01, -1.688673e+00, 9.538156e-01, -1.688673e+00, 9.076311e-01 }, - { 7.875394e-02, 9.528138e-01, -1.677051e+00, 9.528138e-01, -1.677051e+00, 9.056276e-01 }, - { 8.050403e-02, 9.518141e-01, -1.665249e+00, 9.518141e-01, -1.665249e+00, 9.036282e-01 }, - { 8.225411e-02, 9.508163e-01, -1.653272e+00, 9.508163e-01, -1.653272e+00, 9.016326e-01 }, - { 8.400420e-02, 9.498205e-01, -1.641119e+00, 9.498205e-01, -1.641119e+00, 8.996411e-01 }, - { 8.575429e-02, 9.488267e-01, -1.628794e+00, 9.488267e-01, -1.628794e+00, 8.976535e-01 }, - { 8.750438e-02, 9.478349e-01, -1.616297e+00, 9.478349e-01, -1.616297e+00, 8.956698e-01 }, - { 8.925446e-02, 9.468450e-01, -1.603631e+00, 9.468450e-01, -1.603631e+00, 8.936900e-01 }, - { 9.100455e-02, 9.458570e-01, -1.590797e+00, 9.458570e-01, -1.590797e+00, 8.917140e-01 }, - { 9.275464e-02, 9.448710e-01, -1.577798e+00, 9.448710e-01, -1.577798e+00, 8.897420e-01 }, - { 9.450473e-02, 9.438869e-01, -1.564635e+00, 9.438869e-01, -1.564635e+00, 8.877738e-01 }, - { 9.625481e-02, 9.429047e-01, -1.551311e+00, 9.429047e-01, -1.551311e+00, 8.858095e-01 }, - { 9.800490e-02, 9.419245e-01, -1.537826e+00, 9.419245e-01, -1.537826e+00, 8.838489e-01 }, - { 9.975499e-02, 9.409461e-01, -1.524184e+00, 9.409461e-01, -1.524184e+00, 8.818922e-01 }, - { 1.015051e-01, 9.399697e-01, -1.510385e+00, 9.399697e-01, -1.510385e+00, 8.799393e-01 }, - { 1.032552e-01, 9.389951e-01, -1.496433e+00, 9.389951e-01, -1.496433e+00, 8.779902e-01 }, - { 1.050053e-01, 9.380224e-01, -1.482328e+00, 9.380224e-01, -1.482328e+00, 8.760448e-01 }, - { 1.067553e-01, 9.370516e-01, -1.468074e+00, 9.370516e-01, -1.468074e+00, 8.741031e-01 }, - { 1.085054e-01, 9.360826e-01, -1.453671e+00, 9.360826e-01, -1.453671e+00, 8.721652e-01 }, - { 1.102555e-01, 9.351155e-01, -1.439122e+00, 9.351155e-01, -1.439122e+00, 8.702310e-01 }, - { 1.120056e-01, 9.341502e-01, -1.424429e+00, 9.341502e-01, -1.424429e+00, 8.683005e-01 }, - { 1.137557e-01, 9.331868e-01, -1.409594e+00, 9.331868e-01, -1.409594e+00, 8.663737e-01 }, - { 1.155058e-01, 9.322253e-01, -1.394620e+00, 9.322253e-01, -1.394620e+00, 8.644505e-01 }, - { 1.172559e-01, 9.312655e-01, -1.379507e+00, 9.312655e-01, -1.379507e+00, 8.625310e-01 }, - { 1.190060e-01, 9.303076e-01, -1.364259e+00, 9.303076e-01, -1.364259e+00, 8.606152e-01 }, - { 1.207560e-01, 9.293515e-01, -1.348877e+00, 9.293515e-01, -1.348877e+00, 8.587029e-01 }, - { 1.225061e-01, 9.283971e-01, -1.333363e+00, 9.283971e-01, -1.333363e+00, 8.567943e-01 }, - { 1.242562e-01, 9.274446e-01, -1.317720e+00, 9.274446e-01, -1.317720e+00, 8.548892e-01 }, - { 1.260063e-01, 9.264939e-01, -1.301950e+00, 9.264939e-01, -1.301950e+00, 8.529878e-01 }, - { 1.277564e-01, 9.255449e-01, -1.286054e+00, 9.255449e-01, -1.286054e+00, 8.510899e-01 }, - { 1.295065e-01, 9.245978e-01, -1.270035e+00, 9.245978e-01, -1.270035e+00, 8.491955e-01 }, - { 1.312566e-01, 9.236524e-01, -1.253896e+00, 9.236524e-01, -1.253896e+00, 8.473047e-01 }, - { 1.330067e-01, 9.227087e-01, -1.237638e+00, 9.227087e-01, -1.237638e+00, 8.454174e-01 }, - { 1.347567e-01, 9.217668e-01, -1.221263e+00, 9.217668e-01, -1.221263e+00, 8.435336e-01 }, - { 1.365068e-01, 9.208267e-01, -1.204774e+00, 9.208267e-01, -1.204774e+00, 8.416533e-01 }, - { 1.382569e-01, 9.198883e-01, -1.188172e+00, 9.198883e-01, -1.188172e+00, 8.397765e-01 }, - { 1.400070e-01, 9.189516e-01, -1.171461e+00, 9.189516e-01, -1.171461e+00, 8.379032e-01 }, - { 1.417571e-01, 9.180166e-01, -1.154642e+00, 9.180166e-01, -1.154642e+00, 8.360333e-01 }, - { 1.435072e-01, 9.170834e-01, -1.137718e+00, 9.170834e-01, -1.137718e+00, 8.341668e-01 }, - { 1.452573e-01, 9.161519e-01, -1.120690e+00, 9.161519e-01, -1.120690e+00, 8.323037e-01 }, - { 1.470074e-01, 9.152220e-01, -1.103561e+00, 9.152220e-01, -1.103561e+00, 8.304441e-01 }, - { 1.487574e-01, 9.142939e-01, -1.086334e+00, 9.142939e-01, -1.086334e+00, 8.285878e-01 }, - { 1.505075e-01, 9.133675e-01, -1.069010e+00, 9.133675e-01, -1.069010e+00, 8.267350e-01 }, - { 1.522576e-01, 9.124427e-01, -1.051591e+00, 9.124427e-01, -1.051591e+00, 8.248855e-01 }, - { 1.540077e-01, 9.115196e-01, -1.034081e+00, 9.115196e-01, -1.034081e+00, 8.230393e-01 }, - { 1.557578e-01, 9.105982e-01, -1.016481e+00, 9.105982e-01, -1.016481e+00, 8.211965e-01 }, - { 1.575079e-01, 9.096785e-01, -9.987933e-01, 9.096785e-01, -9.987933e-01, 8.193570e-01 }, - { 1.592580e-01, 9.087604e-01, -9.810205e-01, 9.087604e-01, -9.810205e-01, 8.175208e-01 }, - { 1.610081e-01, 9.078440e-01, -9.631649e-01, 9.078440e-01, -9.631649e-01, 8.156879e-01 }, - { 1.627581e-01, 9.069292e-01, -9.452287e-01, 9.069292e-01, -9.452287e-01, 8.138583e-01 }, - { 1.645082e-01, 9.060160e-01, -9.272142e-01, 9.060160e-01, -9.272142e-01, 8.120320e-01 }, - { 1.662583e-01, 9.051045e-01, -9.091238e-01, 9.051045e-01, -9.091238e-01, 8.102089e-01 }, - { 1.680084e-01, 9.041946e-01, -8.909597e-01, 9.041946e-01, -8.909597e-01, 8.083891e-01 }, - { 1.697585e-01, 9.032863e-01, -8.727243e-01, 9.032863e-01, -8.727243e-01, 8.065725e-01 }, - { 1.715086e-01, 9.023796e-01, -8.544198e-01, 9.023796e-01, -8.544198e-01, 8.047592e-01 }, - { 1.732587e-01, 9.014745e-01, -8.360487e-01, 9.014745e-01, -8.360487e-01, 8.029490e-01 }, - { 1.750088e-01, 9.005710e-01, -8.176131e-01, 9.005710e-01, -8.176131e-01, 8.011420e-01 } -}; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8ab7281499..79b3f27c47 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -332,8 +332,6 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, - { "servo_lowpass_freq_idx", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq_idx, 0, 99}, - { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, diff --git a/src/main/mw.c b/src/main/mw.c index af57037a89..54defd191e 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -696,7 +696,6 @@ void loop(void) ); mixTable(); - filterServos(); writeServos(); writeMotors(); } From 64a78f82f87559050821fe83a6f879ff62280e40 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sat, 3 Jan 2015 12:25:46 -0500 Subject: [PATCH 11/24] Interim checkin --- src/main/flight/mixer.c | 24 +++++++++++++++++------- src/main/flight/mixer.h | 16 ++++++++++++---- 2 files changed, 29 insertions(+), 11 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9ffd32f71a..95aeb7d3c6 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8b4c64bc0d..16a1a0cc54 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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]; From d019fa181d0ace79555fe254a84854a063a85ac4 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sat, 3 Jan 2015 13:10:05 -0500 Subject: [PATCH 12/24] time filter execution --- src/main/flight/mixer.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9ffd32f71a..7561250aa0 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -44,11 +44,15 @@ #include "lowpass_table.h" + #define GIMBAL_SERVO_PITCH 0 #define GIMBAL_SERVO_ROLL 1 #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 +#include "drivers/system.h" +extern int16_t debug[4]; + static uint8_t motorCount = 0; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; @@ -708,6 +712,8 @@ void filterServos(void) { int16_t servoIdx; + uint32_t startTime = micros(); + if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { // Round to nearest @@ -716,5 +722,7 @@ void filterServos(void) servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } } + + debug[0] = (int16_t)(micros() - startTime); } From 596da65a87959fd14531dbc19781a21606052f36 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sat, 3 Jan 2015 14:56:50 -0500 Subject: [PATCH 13/24] Interim dev checkin --- src/main/flight/lowpass_table.h | 103 ------------------------------ src/main/flight/mixer.c | 40 ++++++++---- src/main/flight/mixer.h | 11 +--- src/test/Makefile | 24 +++++-- src/test/unit/battery_unittest.cc | 8 +++ 5 files changed, 56 insertions(+), 130 deletions(-) delete mode 100644 src/main/flight/lowpass_table.h diff --git a/src/main/flight/lowpass_table.h b/src/main/flight/lowpass_table.h deleted file mode 100644 index b89680bfb4..0000000000 --- a/src/main/flight/lowpass_table.h +++ /dev/null @@ -1,103 +0,0 @@ -#define LOWPASS_FILTER_NUM 100 -static float lowpass_table[][12] = { - { 2.500000e-03, 9.221512e-13, 4.610756e-12, 9.221512e-12, 9.221512e-12, 4.610756e-12, 9.221512e-13, -4.974584e+00, 9.898659e+00, -9.848470e+00, 4.899299e+00, -9.749042e-01 }, - { 5.000000e-03, 2.913775e-11, 1.456887e-10, 2.913775e-10, 2.913775e-10, 1.456887e-10, 2.913775e-11, -4.949168e+00, 9.797962e+00, -9.698857e+00, 4.800501e+00, -9.504377e-01 }, - { 7.500000e-03, 2.184986e-10, 1.092493e-09, 2.184986e-09, 2.184986e-09, 1.092493e-09, 2.184986e-10, -4.923752e+00, 9.697909e+00, -9.551143e+00, 4.703571e+00, -9.265843e-01 }, - { 1.000000e-02, 9.092865e-10, 4.546433e-09, 9.092865e-09, 9.092865e-09, 4.546433e-09, 9.092865e-10, -4.898337e+00, 9.598497e+00, -9.405308e+00, 4.608476e+00, -9.033283e-01 }, - { 1.250000e-02, 2.740509e-09, 1.370255e-08, 2.740509e-08, 2.740509e-08, 1.370255e-08, 2.740509e-09, -4.872922e+00, 9.499726e+00, -9.261332e+00, 4.515183e+00, -8.806542e-01 }, - { 1.500000e-02, 6.735032e-09, 3.367516e-08, 6.735032e-08, 6.735032e-08, 3.367516e-08, 6.735032e-09, -4.847508e+00, 9.401594e+00, -9.119197e+00, 4.423658e+00, -8.585473e-01 }, - { 1.750000e-02, 1.437797e-08, 7.188984e-08, 1.437797e-07, 1.437797e-07, 7.188984e-08, 1.437797e-08, -4.822094e+00, 9.304100e+00, -8.978882e+00, 4.333870e+00, -8.369928e-01 }, - { 2.000000e-02, 2.768871e-08, 1.384436e-07, 2.768871e-07, 2.768871e-07, 1.384436e-07, 2.768871e-08, -4.796682e+00, 9.207242e+00, -8.840370e+00, 4.245786e+00, -8.159767e-01 }, - { 2.250000e-02, 4.928701e-08, 2.464350e-07, 4.928701e-07, 4.928701e-07, 2.464350e-07, 4.928701e-08, -4.771270e+00, 9.111019e+00, -8.703641e+00, 4.159377e+00, -7.954852e-01 }, - { 2.500000e-02, 8.245336e-08, 4.122668e-07, 8.245336e-07, 8.245336e-07, 4.122668e-07, 8.245336e-08, -4.745859e+00, 9.015430e+00, -8.568676e+00, 4.074612e+00, -7.755049e-01 }, - { 2.750000e-02, 1.311842e-07, 6.559210e-07, 1.311842e-06, 1.311842e-06, 6.559210e-07, 1.311842e-07, -4.720449e+00, 8.920473e+00, -8.435457e+00, 3.991461e+00, -7.560228e-01 }, - { 3.000000e-02, 2.002421e-07, 1.001210e-06, 2.002421e-06, 2.002421e-06, 1.001210e-06, 2.002421e-07, -4.695041e+00, 8.826146e+00, -8.303967e+00, 3.909894e+00, -7.370262e-01 }, - { 3.250000e-02, 2.952011e-07, 1.476005e-06, 2.952011e-06, 2.952011e-06, 1.476005e-06, 2.952011e-07, -4.669634e+00, 8.732448e+00, -8.174185e+00, 3.829883e+00, -7.185027e-01 }, - { 3.500000e-02, 4.224882e-07, 2.112441e-06, 4.224882e-06, 4.224882e-06, 2.112441e-06, 4.224882e-07, -4.644228e+00, 8.639378e+00, -8.046096e+00, 3.751399e+00, -7.004404e-01 }, - { 3.750000e-02, 5.894198e-07, 2.947099e-06, 5.894198e-06, 5.894198e-06, 2.947099e-06, 5.894198e-07, -4.618824e+00, 8.546934e+00, -7.919679e+00, 3.674415e+00, -6.828274e-01 }, - { 4.000000e-02, 8.042356e-07, 4.021178e-06, 8.042356e-06, 8.042356e-06, 4.021178e-06, 8.042356e-07, -4.593421e+00, 8.455115e+00, -7.794918e+00, 3.598903e+00, -6.656525e-01 }, - { 4.250000e-02, 1.076127e-06, 5.380637e-06, 1.076127e-05, 1.076127e-05, 5.380637e-06, 1.076127e-06, -4.568021e+00, 8.363919e+00, -7.671796e+00, 3.524836e+00, -6.489046e-01 }, - { 4.500000e-02, 1.415266e-06, 7.076329e-06, 1.415266e-05, 1.415266e-05, 7.076329e-06, 1.415266e-06, -4.542622e+00, 8.273345e+00, -7.550293e+00, 3.452189e+00, -6.325728e-01 }, - { 4.750000e-02, 1.832823e-06, 9.164113e-06, 1.832823e-05, 1.832823e-05, 9.164113e-06, 1.832823e-06, -4.517225e+00, 8.183391e+00, -7.430394e+00, 3.380934e+00, -6.166466e-01 }, - { 5.000000e-02, 2.340991e-06, 1.170496e-05, 2.340991e-05, 2.340991e-05, 1.170496e-05, 2.340991e-06, -4.491831e+00, 8.094055e+00, -7.312081e+00, 3.311048e+00, -6.011158e-01 }, - { 5.250000e-02, 2.953005e-06, 1.476502e-05, 2.953005e-05, 2.953005e-05, 1.476502e-05, 2.953005e-06, -4.466439e+00, 8.005337e+00, -7.195337e+00, 3.242504e+00, -5.859705e-01 }, - { 5.500000e-02, 3.683149e-06, 1.841574e-05, 3.683149e-05, 3.683149e-05, 1.841574e-05, 3.683149e-06, -4.441049e+00, 7.917234e+00, -7.080145e+00, 3.175278e+00, -5.712009e-01 }, - { 5.750000e-02, 4.546774e-06, 2.273387e-05, 4.546774e-05, 4.546774e-05, 2.273387e-05, 4.546774e-06, -4.415661e+00, 7.829746e+00, -6.966488e+00, 3.109346e+00, -5.567976e-01 }, - { 6.000000e-02, 5.560309e-06, 2.780155e-05, 5.560309e-05, 5.560309e-05, 2.780155e-05, 5.560309e-06, -4.390276e+00, 7.742870e+00, -6.854349e+00, 3.044685e+00, -5.427514e-01 }, - { 6.250000e-02, 6.741264e-06, 3.370632e-05, 6.741264e-05, 6.741264e-05, 3.370632e-05, 6.741264e-06, -4.364894e+00, 7.656604e+00, -6.743713e+00, 2.981271e+00, -5.290533e-01 }, - { 6.500000e-02, 8.108240e-06, 4.054120e-05, 8.108240e-05, 8.108240e-05, 4.054120e-05, 8.108240e-06, -4.339514e+00, 7.570949e+00, -6.634562e+00, 2.919082e+00, -5.156946e-01 }, - { 6.750000e-02, 9.680930e-06, 4.840465e-05, 9.680930e-05, 9.680930e-05, 4.840465e-05, 9.680930e-06, -4.314138e+00, 7.485901e+00, -6.526881e+00, 2.858094e+00, -5.026668e-01 }, - { 7.000000e-02, 1.148013e-05, 5.740063e-05, 1.148013e-04, 1.148013e-04, 5.740063e-05, 1.148013e-05, -4.288764e+00, 7.401459e+00, -6.420653e+00, 2.798287e+00, -4.899617e-01 }, - { 7.250000e-02, 1.352771e-05, 6.763857e-05, 1.352771e-04, 1.352771e-04, 6.763857e-05, 1.352771e-05, -4.263393e+00, 7.317622e+00, -6.315862e+00, 2.739638e+00, -4.775711e-01 }, - { 7.500000e-02, 1.584668e-05, 7.923340e-05, 1.584668e-04, 1.584668e-04, 7.923340e-05, 1.584668e-05, -4.238025e+00, 7.234388e+00, -6.212494e+00, 2.682125e+00, -4.654872e-01 }, - { 7.750000e-02, 1.846110e-05, 9.230551e-05, 1.846110e-04, 1.846110e-04, 9.230551e-05, 1.846110e-05, -4.212661e+00, 7.151755e+00, -6.110531e+00, 2.625730e+00, -4.537024e-01 }, - { 8.000000e-02, 2.139615e-05, 1.069808e-04, 2.139615e-04, 2.139615e-04, 1.069808e-04, 2.139615e-05, -4.187300e+00, 7.069723e+00, -6.009958e+00, 2.570429e+00, -4.422092e-01 }, - { 8.250000e-02, 2.467809e-05, 1.233904e-04, 2.467809e-04, 2.467809e-04, 1.233904e-04, 2.467809e-05, -4.161942e+00, 6.988289e+00, -5.910761e+00, 2.516205e+00, -4.310003e-01 }, - { 8.500000e-02, 2.833425e-05, 1.416713e-04, 2.833425e-04, 2.833425e-04, 1.416713e-04, 2.833425e-05, -4.136588e+00, 6.907451e+00, -5.812923e+00, 2.463036e+00, -4.200687e-01 }, - { 8.750000e-02, 3.239306e-05, 1.619653e-04, 3.239306e-04, 3.239306e-04, 1.619653e-04, 3.239306e-05, -4.111238e+00, 6.827209e+00, -5.716430e+00, 2.410904e+00, -4.094075e-01 }, - { 9.000000e-02, 3.688400e-05, 1.844200e-04, 3.688400e-04, 3.688400e-04, 1.844200e-04, 3.688400e-05, -4.085891e+00, 6.747560e+00, -5.621267e+00, 2.359789e+00, -3.990100e-01 }, - { 9.250000e-02, 4.183762e-05, 2.091881e-04, 4.183762e-04, 4.183762e-04, 2.091881e-04, 4.183762e-05, -4.060548e+00, 6.668503e+00, -5.527418e+00, 2.309672e+00, -3.888696e-01 }, - { 9.500000e-02, 4.728552e-05, 2.364276e-04, 4.728552e-04, 4.728552e-04, 2.364276e-04, 4.728552e-05, -4.035209e+00, 6.590036e+00, -5.434870e+00, 2.260536e+00, -3.789800e-01 }, - { 9.750000e-02, 5.326034e-05, 2.663017e-04, 5.326034e-04, 5.326034e-04, 2.663017e-04, 5.326034e-05, -4.009874e+00, 6.512158e+00, -5.343607e+00, 2.212362e+00, -3.693349e-01 }, - { 1.000000e-01, 5.979578e-05, 2.989789e-04, 5.979578e-04, 5.979578e-04, 2.989789e-04, 5.979578e-05, -3.984543e+00, 6.434867e+00, -5.253615e+00, 2.165133e+00, -3.599282e-01 }, - { 1.025000e-01, 6.692653e-05, 3.346326e-04, 6.692653e-04, 6.692653e-04, 3.346326e-04, 6.692653e-05, -3.959216e+00, 6.358162e+00, -5.164880e+00, 2.118831e+00, -3.507542e-01 }, - { 1.050000e-01, 7.468830e-05, 3.734415e-04, 7.468830e-04, 7.468830e-04, 3.734415e-04, 7.468830e-05, -3.933894e+00, 6.282040e+00, -5.077387e+00, 2.073438e+00, -3.418071e-01 }, - { 1.075000e-01, 8.311783e-05, 4.155892e-04, 8.311783e-04, 8.311783e-04, 4.155892e-04, 8.311783e-05, -3.908575e+00, 6.206500e+00, -4.991123e+00, 2.028939e+00, -3.330811e-01 }, - { 1.100000e-01, 9.225282e-05, 4.612641e-04, 9.225282e-04, 9.225282e-04, 4.612641e-04, 9.225282e-05, -3.883261e+00, 6.131541e+00, -4.906073e+00, 1.985316e+00, -3.245710e-01 }, - { 1.125000e-01, 1.021320e-04, 5.106598e-04, 1.021320e-03, 1.021320e-03, 5.106598e-04, 1.021320e-04, -3.857952e+00, 6.057161e+00, -4.822224e+00, 1.942554e+00, -3.162713e-01 }, - { 1.150000e-01, 1.127949e-04, 5.639746e-04, 1.127949e-03, 1.127949e-03, 5.639746e-04, 1.127949e-04, -3.832647e+00, 5.983358e+00, -4.739562e+00, 1.900636e+00, -3.081769e-01 }, - { 1.175000e-01, 1.242823e-04, 6.214115e-04, 1.242823e-03, 1.242823e-03, 6.214115e-04, 1.242823e-04, -3.807346e+00, 5.910131e+00, -4.658073e+00, 1.859548e+00, -3.002828e-01 }, - { 1.200000e-01, 1.366357e-04, 6.831784e-04, 1.366357e-03, 1.366357e-03, 6.831784e-04, 1.366357e-04, -3.782051e+00, 5.837478e+00, -4.577744e+00, 1.819272e+00, -2.925839e-01 }, - { 1.225000e-01, 1.498975e-04, 7.494876e-04, 1.498975e-03, 1.498975e-03, 7.494876e-04, 1.498975e-04, -3.756760e+00, 5.765398e+00, -4.498561e+00, 1.779796e+00, -2.850755e-01 }, - { 1.250000e-01, 1.641112e-04, 8.205562e-04, 1.641112e-03, 1.641112e-03, 8.205562e-04, 1.641112e-04, -3.731474e+00, 5.693888e+00, -4.420512e+00, 1.741103e+00, -2.777530e-01 }, - { 1.275000e-01, 1.793212e-04, 8.966058e-04, 1.793212e-03, 1.793212e-03, 8.966058e-04, 1.793212e-04, -3.706192e+00, 5.622947e+00, -4.343583e+00, 1.703179e+00, -2.706117e-01 }, - { 1.300000e-01, 1.955725e-04, 9.778624e-04, 1.955725e-03, 1.955725e-03, 9.778624e-04, 1.955725e-04, -3.680916e+00, 5.552574e+00, -4.267762e+00, 1.666010e+00, -2.636472e-01 }, - { 1.325000e-01, 2.129113e-04, 1.064556e-03, 2.129113e-03, 2.129113e-03, 1.064556e-03, 2.129113e-04, -3.655645e+00, 5.482767e+00, -4.193035e+00, 1.629581e+00, -2.568552e-01 }, - { 1.350000e-01, 2.313845e-04, 1.156923e-03, 2.313845e-03, 2.313845e-03, 1.156923e-03, 2.313845e-04, -3.630379e+00, 5.413524e+00, -4.119390e+00, 1.593880e+00, -2.502314e-01 }, - { 1.375000e-01, 2.510400e-04, 1.255200e-03, 2.510400e-03, 2.510400e-03, 1.255200e-03, 2.510400e-04, -3.605118e+00, 5.344844e+00, -4.046813e+00, 1.558893e+00, -2.437717e-01 }, - { 1.400000e-01, 2.719264e-04, 1.359632e-03, 2.719264e-03, 2.719264e-03, 1.359632e-03, 2.719264e-04, -3.579862e+00, 5.276725e+00, -3.975294e+00, 1.524605e+00, -2.374721e-01 }, - { 1.425000e-01, 2.940931e-04, 1.470466e-03, 2.940931e-03, 2.940931e-03, 1.470466e-03, 2.940931e-04, -3.554612e+00, 5.209165e+00, -3.904819e+00, 1.491005e+00, -2.313287e-01 }, - { 1.450000e-01, 3.175906e-04, 1.587953e-03, 3.175906e-03, 3.175906e-03, 1.587953e-03, 3.175906e-04, -3.529367e+00, 5.142163e+00, -3.835375e+00, 1.458080e+00, -2.253377e-01 }, - { 1.475000e-01, 3.424698e-04, 1.712349e-03, 3.424698e-03, 3.424698e-03, 1.712349e-03, 3.424698e-04, -3.504127e+00, 5.075717e+00, -3.766952e+00, 1.425816e+00, -2.194953e-01 }, - { 1.500000e-01, 3.687827e-04, 1.843913e-03, 3.687827e-03, 3.687827e-03, 1.843913e-03, 3.687827e-04, -3.478893e+00, 5.009826e+00, -3.699536e+00, 1.394201e+00, -2.137979e-01 }, - { 1.525000e-01, 3.965819e-04, 1.982910e-03, 3.965819e-03, 3.965819e-03, 1.982910e-03, 3.965819e-04, -3.453664e+00, 4.944488e+00, -3.633116e+00, 1.363224e+00, -2.082419e-01 }, - { 1.550000e-01, 4.259210e-04, 2.129605e-03, 4.259210e-03, 4.259210e-03, 2.129605e-03, 4.259210e-04, -3.428441e+00, 4.879701e+00, -3.567680e+00, 1.332873e+00, -2.028239e-01 }, - { 1.575000e-01, 4.568540e-04, 2.284270e-03, 4.568540e-03, 4.568540e-03, 2.284270e-03, 4.568540e-04, -3.403223e+00, 4.815464e+00, -3.503216e+00, 1.303135e+00, -1.975406e-01 }, - { 1.600000e-01, 4.894361e-04, 2.447180e-03, 4.894361e-03, 4.894361e-03, 2.447180e-03, 4.894361e-04, -3.378011e+00, 4.751775e+00, -3.439713e+00, 1.274000e+00, -1.923886e-01 }, - { 1.625000e-01, 5.237228e-04, 2.618614e-03, 5.237228e-03, 5.237228e-03, 2.618614e-03, 5.237228e-04, -3.352805e+00, 4.688633e+00, -3.377160e+00, 1.245455e+00, -1.873647e-01 }, - { 1.650000e-01, 5.597706e-04, 2.798853e-03, 5.597706e-03, 5.597706e-03, 2.798853e-03, 5.597706e-04, -3.327605e+00, 4.626036e+00, -3.315544e+00, 1.217491e+00, -1.824659e-01 }, - { 1.675000e-01, 5.976365e-04, 2.988183e-03, 5.976365e-03, 5.976365e-03, 2.988183e-03, 5.976365e-04, -3.302410e+00, 4.563982e+00, -3.254854e+00, 1.190095e+00, -1.776890e-01 }, - { 1.700000e-01, 6.373785e-04, 3.186893e-03, 6.373785e-03, 6.373785e-03, 3.186893e-03, 6.373785e-04, -3.277221e+00, 4.502470e+00, -3.195080e+00, 1.163258e+00, -1.730312e-01 }, - { 1.725000e-01, 6.790551e-04, 3.395275e-03, 6.790551e-03, 6.790551e-03, 3.395275e-03, 6.790551e-04, -3.252038e+00, 4.441498e+00, -3.136210e+00, 1.136969e+00, -1.684894e-01 }, - { 1.750000e-01, 7.227254e-04, 3.613627e-03, 7.227254e-03, 7.227254e-03, 3.613627e-03, 7.227254e-04, -3.226861e+00, 4.381065e+00, -3.078233e+00, 1.111218e+00, -1.640609e-01 }, - { 1.775000e-01, 7.684494e-04, 3.842247e-03, 7.684494e-03, 7.684494e-03, 3.842247e-03, 7.684494e-04, -3.201690e+00, 4.321169e+00, -3.021139e+00, 1.085993e+00, -1.597429e-01 }, - { 1.800000e-01, 8.162875e-04, 4.081438e-03, 8.162875e-03, 8.162875e-03, 4.081438e-03, 8.162875e-04, -3.176525e+00, 4.261808e+00, -2.964916e+00, 1.061287e+00, -1.555327e-01 }, - { 1.825000e-01, 8.663011e-04, 4.331505e-03, 8.663011e-03, 8.663011e-03, 4.331505e-03, 8.663011e-04, -3.151366e+00, 4.202982e+00, -2.909555e+00, 1.037088e+00, -1.514277e-01 }, - { 1.850000e-01, 9.185519e-04, 4.592759e-03, 9.185519e-03, 9.185519e-03, 4.592759e-03, 9.185519e-04, -3.126212e+00, 4.144688e+00, -2.855043e+00, 1.013387e+00, -1.474253e-01 }, - { 1.875000e-01, 9.731024e-04, 4.865512e-03, 9.731024e-03, 9.731024e-03, 4.865512e-03, 9.731024e-04, -3.101065e+00, 4.086924e+00, -2.801372e+00, 9.901752e-01, -1.435231e-01 }, - { 1.900000e-01, 1.030016e-03, 5.150079e-03, 1.030016e-02, 1.030016e-02, 5.150079e-03, 1.030016e-03, -3.075924e+00, 4.029691e+00, -2.748530e+00, 9.674428e-01, -1.397185e-01 }, - { 1.925000e-01, 1.089356e-03, 5.446778e-03, 1.089356e-02, 1.089356e-02, 5.446778e-03, 1.089356e-03, -3.050790e+00, 3.972985e+00, -2.696507e+00, 9.451809e-01, -1.360093e-01 }, - { 1.950000e-01, 1.151186e-03, 5.755932e-03, 1.151186e-02, 1.151186e-02, 5.755932e-03, 1.151186e-03, -3.025661e+00, 3.916805e+00, -2.645294e+00, 9.233807e-01, -1.323931e-01 }, - { 1.975000e-01, 1.215573e-03, 6.077865e-03, 1.215573e-02, 1.215573e-02, 6.077865e-03, 1.215573e-03, -3.000538e+00, 3.861150e+00, -2.594879e+00, 9.020332e-01, -1.288676e-01 }, - { 2.000000e-01, 1.282581e-03, 6.412905e-03, 1.282581e-02, 1.282581e-02, 6.412905e-03, 1.282581e-03, -2.975422e+00, 3.806018e+00, -2.545253e+00, 8.811301e-01, -1.254306e-01 }, - { 2.025000e-01, 1.352277e-03, 6.761383e-03, 1.352277e-02, 1.352277e-02, 6.761383e-03, 1.352277e-03, -2.950312e+00, 3.751408e+00, -2.496406e+00, 8.606627e-01, -1.220800e-01 }, - { 2.050000e-01, 1.424727e-03, 7.123633e-03, 1.424727e-02, 1.424727e-02, 7.123633e-03, 1.424727e-03, -2.925208e+00, 3.697318e+00, -2.448328e+00, 8.406230e-01, -1.188137e-01 }, - { 2.075000e-01, 1.499998e-03, 7.499990e-03, 1.499998e-02, 1.499998e-02, 7.499990e-03, 1.499998e-03, -2.900111e+00, 3.643747e+00, -2.401010e+00, 8.210026e-01, -1.156296e-01 }, - { 2.100000e-01, 1.578159e-03, 7.890794e-03, 1.578159e-02, 1.578159e-02, 7.890794e-03, 1.578159e-03, -2.875019e+00, 3.590694e+00, -2.354441e+00, 8.017937e-01, -1.125258e-01 }, - { 2.125000e-01, 1.659277e-03, 8.296387e-03, 1.659277e-02, 1.659277e-02, 8.296387e-03, 1.659277e-03, -2.849934e+00, 3.538156e+00, -2.308613e+00, 7.829884e-01, -1.095002e-01 }, - { 2.150000e-01, 1.743423e-03, 8.717115e-03, 1.743423e-02, 1.743423e-02, 8.717115e-03, 1.743423e-03, -2.824855e+00, 3.486132e+00, -2.263516e+00, 7.645791e-01, -1.065509e-01 }, - { 2.175000e-01, 1.830665e-03, 9.153325e-03, 1.830665e-02, 1.830665e-02, 9.153325e-03, 1.830665e-03, -2.799783e+00, 3.434622e+00, -2.219139e+00, 7.465581e-01, -1.036762e-01 }, - { 2.200000e-01, 1.921074e-03, 9.605368e-03, 1.921074e-02, 1.921074e-02, 9.605368e-03, 1.921074e-03, -2.774717e+00, 3.383623e+00, -2.175475e+00, 7.289181e-01, -1.008741e-01 }, - { 2.225000e-01, 2.014719e-03, 1.007360e-02, 2.014719e-02, 2.014719e-02, 1.007360e-02, 2.014719e-03, -2.749657e+00, 3.333133e+00, -2.132514e+00, 7.116518e-01, -9.814302e-02 }, - { 2.250000e-01, 2.111673e-03, 1.055837e-02, 2.111673e-02, 2.111673e-02, 1.055837e-02, 2.111673e-03, -2.724604e+00, 3.283153e+00, -2.090246e+00, 6.947520e-01, -9.548109e-02 }, - { 2.275000e-01, 2.212008e-03, 1.106004e-02, 2.212008e-02, 2.212008e-02, 1.106004e-02, 2.212008e-03, -2.699556e+00, 3.233679e+00, -2.048664e+00, 6.782117e-01, -9.288667e-02 }, - { 2.300000e-01, 2.315794e-03, 1.157897e-02, 2.315794e-02, 2.315794e-02, 1.157897e-02, 2.315794e-03, -2.674515e+00, 3.184711e+00, -2.007756e+00, 6.620241e-01, -9.035810e-02 }, - { 2.325000e-01, 2.423106e-03, 1.211553e-02, 2.423106e-02, 2.423106e-02, 1.211553e-02, 2.423106e-03, -2.649481e+00, 3.136248e+00, -1.967516e+00, 6.461824e-01, -8.789379e-02 }, - { 2.350000e-01, 2.534016e-03, 1.267008e-02, 2.534016e-02, 2.534016e-02, 1.267008e-02, 2.534016e-03, -2.624453e+00, 3.088287e+00, -1.927933e+00, 6.306799e-01, -8.549216e-02 }, - { 2.375000e-01, 2.648598e-03, 1.324299e-02, 2.648598e-02, 2.648598e-02, 1.324299e-02, 2.648598e-03, -2.599431e+00, 3.040828e+00, -1.889000e+00, 6.155102e-01, -8.315169e-02 }, - { 2.400000e-01, 2.766928e-03, 1.383464e-02, 2.766928e-02, 2.766928e-02, 1.383464e-02, 2.766928e-03, -2.574415e+00, 2.993869e+00, -1.850707e+00, 6.006667e-01, -8.087089e-02 }, - { 2.425000e-01, 2.889078e-03, 1.444539e-02, 2.889078e-02, 2.889078e-02, 1.444539e-02, 2.889078e-03, -2.549406e+00, 2.947408e+00, -1.813047e+00, 5.861434e-01, -7.864829e-02 }, - { 2.450000e-01, 3.015126e-03, 1.507563e-02, 3.015126e-02, 3.015126e-02, 1.507563e-02, 3.015126e-03, -2.524403e+00, 2.901445e+00, -1.776010e+00, 5.719340e-01, -7.648247e-02 }, - { 2.475000e-01, 3.145146e-03, 1.572573e-02, 3.145146e-02, 3.145146e-02, 1.572573e-02, 3.145146e-03, -2.499407e+00, 2.855979e+00, -1.739588e+00, 5.580324e-01, -7.437205e-02 }, - { 2.500000e-01, 3.279216e-03, 1.639608e-02, 3.279216e-02, 3.279216e-02, 1.639608e-02, 3.279216e-03, -2.474416e+00, 2.811006e+00, -1.703772e+00, 5.444327e-01, -7.231567e-02 } -}; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 95aeb7d3c6..e78050dd68 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -18,6 +18,11 @@ #include #include #include +#include + +#ifdef UNIT_TEST +#include +#endif #include "platform.h" @@ -25,11 +30,12 @@ #include "common/axis.h" #include "common/maths.h" - +#ifndef UNIT_TEST #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" +#endif #include "rx/rx.h" #include "io/gimbal.h" @@ -42,8 +48,6 @@ #include "config/runtime_config.h" #include "config/config.h" -#include "lowpass_table.h" - #define GIMBAL_SERVO_PITCH 0 #define GIMBAL_SERVO_ROLL 1 @@ -70,9 +74,6 @@ 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 @@ -234,7 +235,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon gimbalConfig = gimbalConfigToUse; } - +#ifndef UNIT_TEST int16_t determineServoMiddleOrForwardFromChannel(int nr) { uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel; @@ -666,12 +667,26 @@ bool isMixerUsingServos(void) return useServo; } -static void generate_lowpass_coeffs5(int16_t freq) +#endif + +static void generate_lowpass_coeffs2(int16_t freq, float *b, float *a) { - const float lowpassNormalizedButter5[] = { 3.2361f, 5.2361f, 5.2361f, 3.2361f, 1.0f }; - + float freqf = (float)freq*0.001f; + float omega = tanf(M_PI*freqf/2.0f); + float scaling = 1.0f / (omega*omega + 1.4142f*omega + 1.0f); + + b[0] = scaling * omega*omega; + b[1] = 2.0f * b[0]; + b[2] = b[0]; + // First assumed to be 1.0 + a[0] = scaling * (2.0f * omega*omega - 2.0f); + a[1] = scaling * (omega*omega - 1.4142f * omega + 1.0f); +#ifdef UNIT_TEST + printf("lowpass cutoff: %f, omega: %f\n", freqf, omega); + printf("b: %f %f %f, a: %f %f\n", b[2], b[1], b[0], a[1], a[0]); +#endif } static float lowpass(lowpass_t *filter, float in, int16_t freq) @@ -686,7 +701,8 @@ static float lowpass(lowpass_t *filter, float in, int16_t freq) // Initialize if needed if (!filter->init) { - generate_lowpass_coeffs5(freq, filter->b, filter->a); + generate_lowpass_coeffs2(freq, filter->b, filter->a); + filter->freq = freq; //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++) { @@ -721,7 +737,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) + 0.5f); + servo[servoIdx] = (int16_t)(lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx) + 0.5f); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 16a1a0cc54..54911f991d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -92,20 +92,13 @@ typedef struct servoParam_t { int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED } 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; -*/ +#define LOWPASS_NUM_COEF 3 typedef struct lowpass_t { bool init; int16_t freq; // Normalized freq in 1/1000ths float b[LOWPASS_NUM_COEF]; - float a[LOWPASS_NUM_COEF]; + float a[LOWPASS_NUM_COEF-1]; float x[LOWPASS_NUM_COEF]; float y[LOWPASS_NUM_COEF]; } lowpass_t; diff --git a/src/test/Makefile b/src/test/Makefile index 4dd0cf1ab0..6e6d29906b 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -33,15 +33,16 @@ CXXFLAGS += -g -Wall -Wextra -pthread -ggdb3 -O0 -DUNIT_TEST # All tests produced by this Makefile. Remember to add new tests you # created to the list. +#flight_imu_unittest +#altitude_hold_unittest + #gps_conversion_unittest + #ledstrip_unittest + #ws2811_unittest TESTS = \ battery_unittest \ - flight_imu_unittest \ - altitude_hold_unittest \ - gps_conversion_unittest \ telemetry_hott_unittest \ - rc_controls_unittest \ - ledstrip_unittest \ - ws2811_unittest + mixer_unittest \ + rc_controls_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -147,6 +148,17 @@ gps_conversion_unittest : $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gp $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(USER_DIR)/flight/lowpass_table.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@ + +$(OBJECT_DIR)/mixer_unittest.o : $(TEST_DIR)/mixer_unittest.cc $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/mixer_unittest.cc -o $@ + +mixer_unittest : $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/mixer_unittest.o $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(OBJECT_DIR)/telemetry/hott.o : $(USER_DIR)/telemetry/hott.c $(USER_DIR)/telemetry/hott.h $(GTEST_HEADERS) @mkdir -p $(dir $@) diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 0d30723eca..4e42f9eac6 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -84,4 +84,12 @@ void delay(uint32_t ms) UNUSED(ms); return; } + +int constrain(int amt, int low, int high) +{ + UNUSED(amt); + UNUSED(low); + UNUSED(high); + return 0; +} } From 11a9725605e660b5766d4e2ed7866fb8e9f4c25d Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sun, 4 Jan 2015 21:11:28 -0500 Subject: [PATCH 14/24] Fixed point passes initial unit test --- src/test/unit/mixer_unittest.cc | 114 ++++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) create mode 100644 src/test/unit/mixer_unittest.cc diff --git a/src/test/unit/mixer_unittest.cc b/src/test/unit/mixer_unittest.cc new file mode 100644 index 0000000000..29d23c7bc9 --- /dev/null +++ b/src/test/unit/mixer_unittest.cc @@ -0,0 +1,114 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ +#include + +#include + +extern "C" { + #include "flight/mixer.h" + #include "rx/rx.h" + #include "io/gimbal.h" + #include "io/escservo.h" + extern void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse); +} +uint32_t debug[4]; + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +TEST(MixerTest, ServoLowpassFilter) +{ + int16_t servoCmds[3000]; + int16_t expectedOut[3000]; + const int16_t margin = 0; + uint8_t servoIdx; + uint16_t sampleIdx; + static mixerConfig_t mixerConfig; + static servoParam_t servoConfig[MAX_SUPPORTED_SERVOS]; + + + uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t); + + // generate inputs and expecteds + for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) { + if (sampleIdx < 1000) { + servoCmds[sampleIdx] = 501; + } else if (sampleIdx >= 1000 && sampleIdx < 2000) { + servoCmds[sampleIdx] = 2500; + } else { + servoCmds[sampleIdx] = 1500; + } + + if ((sampleIdx >= 900 && sampleIdx < 1000) || + (sampleIdx >= 1900 && sampleIdx < 2000)|| + (sampleIdx >= 2900 && sampleIdx < 3000)) { + expectedOut[sampleIdx] = servoCmds[sampleIdx]; + } else { + expectedOut[sampleIdx] = -1; + } + + } + + // Set mixer configuration + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + servoConfig[servoIdx].min = 0; + servoConfig[servoIdx].max = 2500; + } + + mixerConfig.servo_lowpass_enable = 1; + mixerConfig.servo_lowpass_freq_idx = 400; // 10 to 400 + mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL); + + // Run tests + for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) { + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + servo[servoIdx] = servoCmds[sampleIdx]; + } + + filterServos(); + + if (expectedOut[sampleIdx] >= 0) { + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + EXPECT_LE(servo[servoIdx], expectedOut[sampleIdx] + margin); + EXPECT_GE(servo[servoIdx], expectedOut[sampleIdx] - margin); + } + } + } +} + +// STUBS + +extern "C" { + +void delay(uint32_t ms) +{ + UNUSED(ms); + return; +} + +int constrain(int amt, int low, int high) +{ + return (amt > high ? high : (amt < low ? low : amt)); +} + +uint32_t micros() +{ + return 0; +} +} + + From de7fa3f31a7f9591617d6ae55e8d1c14fdbefabd Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sun, 4 Jan 2015 21:11:40 -0500 Subject: [PATCH 15/24] Fixed point passes initial unit test --- src/main/flight/mixer.c | 126 ++++++++++++++++++++++++++++++++-------- src/main/flight/mixer.h | 16 +++-- src/test/Makefile | 2 +- 3 files changed, 114 insertions(+), 30 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 1f8df7b660..fc5ab7d1c6 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -53,6 +53,8 @@ #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 +#define MIXER_DEBUG + #include "drivers/system.h" extern int16_t debug[4]; @@ -672,24 +674,95 @@ bool isMixerUsingServos(void) #endif -static void generate_lowpass_coeffs2(int16_t freq, float *b, float *a) +static void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter) { + float fixedScaler; + int i; + + // generates coefficients for a 2nd-order butterworth low-pass filter float freqf = (float)freq*0.001f; float omega = tanf(M_PI*freqf/2.0f); - float scaling = 1.0f / (omega*omega + 1.4142f*omega + 1.0f); - - b[0] = scaling * omega*omega; - b[1] = 2.0f * b[0]; - b[2] = b[0]; + float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f); - // First assumed to be 1.0 - a[0] = scaling * (2.0f * omega*omega - 2.0f); - a[1] = scaling * (omega*omega - 1.4142f * omega + 1.0f); #ifdef UNIT_TEST printf("lowpass cutoff: %f, omega: %f\n", freqf, omega); - printf("b: %f %f %f, a: %f %f\n", b[2], b[1], b[0], a[1], a[0]); #endif + + filter->bf[0] = scaling * omega*omega; + filter->bf[1] = 2.0f * filter->bf[0]; + filter->bf[2] = filter->bf[0]; + + filter->af[0] = 1.0f; + filter->af[1] = scaling * (2.0f * omega*omega - 2.0f); + filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f); + + // Scale for fixed-point + filter->input_bias = 1500; // Typical servo range is 1500 +/- 500 + filter->input_shift = 16; + filter->coeff_shift = 24; + fixedScaler = (float)(1ULL << filter->coeff_shift); + for (i = 0; i < LOWPASS_NUM_COEF; i++) { + filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler); + filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler); +#ifdef UNIT_TEST + printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i, + filter->bf[i], filter->af[i], filter->b[i], filter->a[i]); +#endif + } + + filter->freq = freq; +} + +static int32_t lowpass_fixed(lowpass_t *filter, int32_t in, int16_t freq) +{ + int16_t coefIdx; + int64_t out; + int32_t in_s; + + // Check to see if cutoff frequency changed + if (freq != filter->freq) { + filter->init = false; + } + + // Initialize if needed + if (!filter->init) { + generate_lowpass_coeffs2(freq, filter); + for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift; + filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift; + } + filter->init = true; + } + + // Unbias input and scale + in_s = (in - filter->input_bias) << filter->input_shift; + + // Delays + for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { + filter->x[coefIdx] = filter->x[coefIdx-1]; + filter->y[coefIdx] = filter->y[coefIdx-1]; + } + filter->x[0] = in_s; + + // Accumulate result + out = filter->x[0] * filter->b[0]; + for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + out -= filter->y[coefIdx] * filter->a[coefIdx]; + out += filter->x[coefIdx] * filter->b[coefIdx]; + } + + // Scale output by coefficient shift + out >>= filter->coeff_shift; + filter->y[0] = (int32_t)out; + + // Scale output by input shift and round + out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift; + + // Reapply bias + out += filter->input_bias; + + return (int32_t)out; } static float lowpass(lowpass_t *filter, float in, int16_t freq) @@ -704,50 +777,53 @@ static float lowpass(lowpass_t *filter, float in, int16_t freq) // Initialize if needed if (!filter->init) { - generate_lowpass_coeffs2(freq, filter->b, filter->a); - filter->freq = freq; - //filter->b = &lowpass_table[filter->freqIdx][1]; - //filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF]; + generate_lowpass_coeffs2(freq, filter); for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - filter->x[coefIdx] = in; - filter->y[coefIdx] = in; + filter->xf[coefIdx] = in; + filter->yf[coefIdx] = in; } filter->init = true; } // Delays for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { - filter->x[coefIdx] = filter->x[coefIdx-1]; - filter->y[coefIdx] = filter->y[coefIdx-1]; + filter->xf[coefIdx] = filter->xf[coefIdx-1]; + filter->yf[coefIdx] = filter->yf[coefIdx-1]; } - filter->x[0] = in; + filter->xf[0] = in; // Accumulate result - out = filter->x[0] * filter->b[0]; + out = filter->xf[0] * filter->b[0]; for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - out += filter->x[coefIdx] * filter->b[coefIdx]; - out -= filter->y[coefIdx] * filter->a[coefIdx-1]; + out += filter->xf[coefIdx] * filter->bf[coefIdx]; + out -= filter->yf[coefIdx] * filter->af[coefIdx]; } - filter->y[0] = out; + filter->yf[0] = out; return out; } + void filterServos(void) { int16_t servoIdx; +#if defined(MIXER_DEBUG) uint32_t startTime = micros(); +#endif 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_idx) + 0.5f); + servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx); + // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } } - +#if defined(MIXER_DEBUG) debug[0] = (int16_t)(micros() - startTime); +#endif } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 54911f991d..92748c2e13 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -93,14 +93,22 @@ typedef struct servoParam_t { } servoParam_t; #define LOWPASS_NUM_COEF 3 +#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f)) typedef struct lowpass_t { bool init; int16_t freq; // Normalized freq in 1/1000ths - float b[LOWPASS_NUM_COEF]; - float a[LOWPASS_NUM_COEF-1]; - float x[LOWPASS_NUM_COEF]; - float y[LOWPASS_NUM_COEF]; + float bf[LOWPASS_NUM_COEF]; + float af[LOWPASS_NUM_COEF]; + int64_t b[LOWPASS_NUM_COEF]; + int64_t a[LOWPASS_NUM_COEF]; + int16_t coeff_shift; + int16_t input_shift; + int32_t input_bias; + float xf[LOWPASS_NUM_COEF]; + float yf[LOWPASS_NUM_COEF]; + int32_t x[LOWPASS_NUM_COEF]; + int32_t y[LOWPASS_NUM_COEF]; } lowpass_t; diff --git a/src/test/Makefile b/src/test/Makefile index 6e6d29906b..c46ba1d505 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -148,7 +148,7 @@ gps_conversion_unittest : $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gp $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(USER_DIR)/flight/lowpass_table.h $(GTEST_HEADERS) +$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@ From dc3271255c9e1e25705886c261a0db9d2163cf13 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sun, 4 Jan 2015 22:10:20 -0500 Subject: [PATCH 16/24] More unit tests --- src/main/flight/mixer.c | 43 +------------ src/test/unit/mixer_unittest.cc | 103 +++++++++++++++++++++++++------- 2 files changed, 83 insertions(+), 63 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index fc5ab7d1c6..460cca91a5 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -674,7 +674,7 @@ bool isMixerUsingServos(void) #endif -static void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter) +void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter) { float fixedScaler; int i; @@ -765,45 +765,6 @@ static int32_t lowpass_fixed(lowpass_t *filter, int32_t in, int16_t freq) return (int32_t)out; } -static float lowpass(lowpass_t *filter, float in, int16_t freq) -{ - int16_t coefIdx; - float out; - - // Check to see if cutoff frequency changed - if (freq != filter->freq) { - filter->init = false; - } - - // Initialize if needed - if (!filter->init) { - generate_lowpass_coeffs2(freq, filter); - for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - filter->xf[coefIdx] = in; - filter->yf[coefIdx] = in; - } - filter->init = true; - } - - // Delays - for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { - filter->xf[coefIdx] = filter->xf[coefIdx-1]; - filter->yf[coefIdx] = filter->yf[coefIdx-1]; - } - filter->xf[0] = in; - - // Accumulate result - out = filter->xf[0] * filter->b[0]; - for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - out += filter->xf[coefIdx] * filter->bf[coefIdx]; - out -= filter->yf[coefIdx] * filter->af[coefIdx]; - } - filter->yf[0] = out; - - return out; -} - - void filterServos(void) { int16_t servoIdx; @@ -814,8 +775,6 @@ 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_fixed(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx); // Sanity check diff --git a/src/test/unit/mixer_unittest.cc b/src/test/unit/mixer_unittest.cc index 29d23c7bc9..dd9ca7cf9c 100644 --- a/src/test/unit/mixer_unittest.cc +++ b/src/test/unit/mixer_unittest.cc @@ -24,29 +24,82 @@ extern "C" { #include "io/gimbal.h" #include "io/escservo.h" extern void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse); + extern void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter); } + uint32_t debug[4]; +static int16_t servoRef[MAX_SUPPORTED_SERVOS]; +static int16_t referenceOut[MAX_SUPPORTED_SERVOS]; +static uint16_t freq; +static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; +static servoParam_t servoConfig[MAX_SUPPORTED_SERVOS]; #include "unittest_macros.h" #include "gtest/gtest.h" +static float lowpass_ref(lowpass_t *filter, float in, int16_t freq) +{ + int16_t coefIdx; + float out; + + // Check to see if cutoff frequency changed + if (freq != filter->freq) { + filter->init = false; + } + + // Initialize if needed + if (!filter->init) { + generate_lowpass_coeffs2(freq, filter); + for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + filter->xf[coefIdx] = in; + filter->yf[coefIdx] = in; + } + filter->init = true; + } + + // Delays + for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { + filter->xf[coefIdx] = filter->xf[coefIdx-1]; + filter->yf[coefIdx] = filter->yf[coefIdx-1]; + } + filter->xf[0] = in; + + // Accumulate result + out = filter->xf[0] * filter->bf[0]; + for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + out += filter->xf[coefIdx] * filter->bf[coefIdx]; + out -= filter->yf[coefIdx] * filter->af[coefIdx]; + } + filter->yf[0] = out; + + return out; +} + +static void filterServosReference(void) +{ + int16_t servoIdx; + + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + // Round to nearest + referenceOut[servoIdx] = (int16_t)(lowpass_ref(&lowpassFilters[servoIdx], (float)servoRef[servoIdx], freq) + 0.5f); + } +} + + TEST(MixerTest, ServoLowpassFilter) { int16_t servoCmds[3000]; int16_t expectedOut[3000]; - const int16_t margin = 0; uint8_t servoIdx; uint16_t sampleIdx; static mixerConfig_t mixerConfig; - static servoParam_t servoConfig[MAX_SUPPORTED_SERVOS]; - uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t); // generate inputs and expecteds for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) { if (sampleIdx < 1000) { - servoCmds[sampleIdx] = 501; + servoCmds[sampleIdx] = 500; } else if (sampleIdx >= 1000 && sampleIdx < 2000) { servoCmds[sampleIdx] = 2500; } else { @@ -60,34 +113,42 @@ TEST(MixerTest, ServoLowpassFilter) } else { expectedOut[sampleIdx] = -1; } - } // Set mixer configuration for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { servoConfig[servoIdx].min = 0; - servoConfig[servoIdx].max = 2500; + servoConfig[servoIdx].max = 3000; } - mixerConfig.servo_lowpass_enable = 1; - mixerConfig.servo_lowpass_freq_idx = 400; // 10 to 400 - mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL); + // Test all frequencies + for (freq = 10; freq <= 400; freq++) + { + printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f)); - // Run tests - for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) { - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = servoCmds[sampleIdx]; - } + mixerConfig.servo_lowpass_enable = 1; + mixerConfig.servo_lowpass_freq_idx = freq; + mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL); - filterServos(); - - if (expectedOut[sampleIdx] >= 0) { + // Run tests + for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - EXPECT_LE(servo[servoIdx], expectedOut[sampleIdx] + margin); - EXPECT_GE(servo[servoIdx], expectedOut[sampleIdx] - margin); + servo[servoIdx] = servoCmds[sampleIdx]; + servoRef[servoIdx] = servoCmds[sampleIdx]; } - } - } + + filterServos(); + filterServosReference(); + + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + if (expectedOut[sampleIdx] >= 0) { + EXPECT_EQ(servo[servoIdx], expectedOut[sampleIdx]); + } + EXPECT_LE(servo[servoIdx], referenceOut[servoIdx] + 1); + EXPECT_GE(servo[servoIdx], referenceOut[servoIdx] - 1); + } + } // for each sample + } // for each freq } // STUBS From 06ce23951b062d67f94587f1e048280f7ae24f21 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sun, 4 Jan 2015 22:15:09 -0500 Subject: [PATCH 17/24] Rename freq parameter; remove UT warnings --- src/main/config/config.c | 2 +- src/main/flight/mixer.c | 12 ++++++++---- src/main/flight/mixer.h | 2 +- src/main/io/serial_cli.c | 2 +- src/test/unit/mixer_unittest.cc | 2 +- 5 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index c9e1cc25a4..14f9788543 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -428,7 +428,7 @@ static void resetConf(void) currentProfile->mixerConfig.yaw_direction = 1; currentProfile->mixerConfig.tri_unarmed_servo = 1; - currentProfile->mixerConfig.servo_lowpass_freq_idx = 99; + currentProfile->mixerConfig.servo_lowpass_freq = 400; currentProfile->mixerConfig.servo_lowpass_enable = 0; // gimbal diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 460cca91a5..5f5dd6e6eb 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -58,14 +58,16 @@ #include "drivers/system.h" extern int16_t debug[4]; +#ifndef UNIT_TEST static uint8_t motorCount = 0; +static int useServo; +static uint8_t servoCount; +#endif + int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t servo[MAX_SUPPORTED_SERVOS]; -static int useServo; - -static uint8_t servoCount; static servoParam_t *servoConf; static mixerConfig_t *mixerConfig; @@ -75,8 +77,10 @@ static airplaneConfig_t *airplaneConfig; static rxConfig_t *rxConfig; static gimbalConfig_t *gimbalConfig; +#ifndef UNIT_TEST static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static mixerMode_e currentMixerMode; +#endif static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; static const motorMixer_t mixerQuadX[] = { @@ -775,7 +779,7 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx); + servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 92748c2e13..33bdb49f0e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -66,7 +66,7 @@ typedef struct mixer_t { typedef struct mixerConfig_s { int8_t yaw_direction; uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - int16_t servo_lowpass_freq_idx; // lowpass servo filter frequency selection + int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq int8_t servo_lowpass_enable; // enable/disable lowpass filter } mixerConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 776a872d58..662f1430e9 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -332,7 +332,7 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, - { "servo_lowpass_freq_idx", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq_idx, 0, 99}, + { "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400}, { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, diff --git a/src/test/unit/mixer_unittest.cc b/src/test/unit/mixer_unittest.cc index dd9ca7cf9c..4879aaa896 100644 --- a/src/test/unit/mixer_unittest.cc +++ b/src/test/unit/mixer_unittest.cc @@ -127,7 +127,7 @@ TEST(MixerTest, ServoLowpassFilter) printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f)); mixerConfig.servo_lowpass_enable = 1; - mixerConfig.servo_lowpass_freq_idx = freq; + mixerConfig.servo_lowpass_freq = freq; mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL); // Run tests From c853f20d751cac54e6496901275fdb62a016ae27 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sun, 4 Jan 2015 22:25:13 -0500 Subject: [PATCH 18/24] Disable mixer debug --- src/main/flight/mixer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5f5dd6e6eb..5d9e3e64f0 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -53,7 +53,7 @@ #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 -#define MIXER_DEBUG +//#define MIXER_DEBUG #include "drivers/system.h" extern int16_t debug[4]; From fd71e1ee931d7d683fe141a0332a605d4d35eb89 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sun, 4 Jan 2015 22:32:11 -0500 Subject: [PATCH 19/24] Limit UT changes to those mixer-related --- src/test/Makefile | 35 +++++++++++++++---------------- src/test/unit/battery_unittest.cc | 8 ------- 2 files changed, 17 insertions(+), 26 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index c46ba1d505..22981097e6 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -33,16 +33,16 @@ CXXFLAGS += -g -Wall -Wextra -pthread -ggdb3 -O0 -DUNIT_TEST # All tests produced by this Makefile. Remember to add new tests you # created to the list. -#flight_imu_unittest -#altitude_hold_unittest - #gps_conversion_unittest - #ledstrip_unittest - #ws2811_unittest TESTS = \ battery_unittest \ + flight_imu_unittest \ + altitude_hold_unittest \ + gps_conversion_unittest \ telemetry_hott_unittest \ - mixer_unittest \ - rc_controls_unittest + rc_controls_unittest \ + ledstrip_unittest \ + ws2811_unittest \ + mixer_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -148,17 +148,6 @@ gps_conversion_unittest : $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gp $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) - @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@ - -$(OBJECT_DIR)/mixer_unittest.o : $(TEST_DIR)/mixer_unittest.cc $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) - @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/mixer_unittest.cc -o $@ - -mixer_unittest : $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/mixer_unittest.o $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ - $(OBJECT_DIR)/telemetry/hott.o : $(USER_DIR)/telemetry/hott.c $(USER_DIR)/telemetry/hott.h $(GTEST_HEADERS) @mkdir -p $(dir $@) @@ -213,4 +202,14 @@ $(OBJECT_DIR)/ws2811_unittest.o : $(TEST_DIR)/ws2811_unittest.cc \ ws2811_unittest :$(OBJECT_DIR)/drivers/light_ws2811strip.o $(OBJECT_DIR)/ws2811_unittest.o $(OBJECT_DIR)/gtest_main.a $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@ + +$(OBJECT_DIR)/mixer_unittest.o : $(TEST_DIR)/mixer_unittest.cc $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/mixer_unittest.cc -o $@ + +mixer_unittest : $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/mixer_unittest.o $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 4e42f9eac6..0d30723eca 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -84,12 +84,4 @@ void delay(uint32_t ms) UNUSED(ms); return; } - -int constrain(int amt, int low, int high) -{ - UNUSED(amt); - UNUSED(low); - UNUSED(high); - return 0; -} } From 9d5a8e8e86f031f9eff4831dbb31be5a8d49e156 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Mon, 5 Jan 2015 21:32:49 -0500 Subject: [PATCH 20/24] Pull in mixer docs --- docs/Mixer.md | 56 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 docs/Mixer.md diff --git a/docs/Mixer.md b/docs/Mixer.md new file mode 100644 index 0000000000..c662b4342a --- /dev/null +++ b/docs/Mixer.md @@ -0,0 +1,56 @@ +# Mixer + +Cleanflight supports a number of mixing configurations as well as custom mixing. Mixer configurations determine how the servos and motors work together to control the aircraft. + +## Configuration + +To use a built-in mixing configuration, you can use the Chrome configuration GUI. It includes images of the various mixer types to assist in making the proper connections. See the Configuration section of the documentation for more information on the GUI. + +You can also use the Command Line Interface (CLI) to set the mixer type: + +1. Use `mixer list` to see a list of supported mixes +2. Select a mixer. For example, to select TRI, use `mixer TRI` +3. You must use `save` to preserve your changes + +## Supported Mixer Types + +| Name | Description | Motors | Servos | +| ------------- | ------------------------- | -------------- | ---------------- | +| TRI | Tricopter | M1-M3 | S1 | +| QUADP | Quadcopter-Plus | M1-M4 | None | +| QUADX | Quadcopter-X | M1-M4 | None | +| BI | Bicopter (left/right) | M1-M2 | S1, S2 | +| GIMBAL | Gimbal control | N/A | S1, S2 | +| Y6 | Y6-copter | M1-M6 | None | +| HEX6 | Hexacopter-Plus | M1-M6 | None | +| FLYING_WING | Fixed wing; elevons | M1 | S1, S2 | +| Y4 | Y4-copter | M1-M4 | None | +| HEX6X | Hexacopter-X | M1-M6 | None | +| OCTOX8 | Octocopter-X (over/under) | M1-M8 | None | +| OCTOFLATP | Octocopter-FlatPlus | M1-M8 | None | +| OCTOFLATX | Octocopter-FlatX | M1-M8 | None | +| AIRPLANE | Fixed wing; Ax2, R, E | M1 | S1, S2, S3, S4 | +| HELI_120_CCPM | | | | +| HELI_90_DEG | | | | +| VTAIL4 | Quadcopter with V-Tail | M1-M4 | N/A | +| HEX6H | Hexacopter-H | M1-M6 | None | +| PPM_TO_SERVO | | | | +| DUALCOPTER | Dualcopter | M1-M2 | S1, S2 | +| SINGLECOPTER | Conventional helicopter | M1 | S1 | +| ATAIL4 | Quadcopter with A-Tail | M1-M4 | N/A | +| CUSTOM | User-defined | | | + + +## Servo filtering + +A low-pass filter can be enabled for the servos. It may be useful for avoiding structural modes in the airframe, for example. Currently it can only be configured via the CLI: + +1. Use `set servo_lowpass_freq = nnn` to select the cutoff frequency. Valid values range from 10 to 400. This is a fraction of the loop frequency in 1/1000ths. For example, `40` means `0.040`. +2. Use `set servo_lowpass_enable = 1` to enable filtering. + +The cutoff frequency can be determined by the following formula: +`Frequency = 1000 * servo_lowpass_freq / looptime` + +For example, if `servo_lowpass_freq` is set to 40, and looptime is set to the default of 3500 us, the cutoff frequency will be 11.43 Hz. + + From 62147736b2e31e3c4c45789eb8547adfa49a342d Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Tue, 6 Jan 2015 20:26:09 -0500 Subject: [PATCH 21/24] Remove unnecessary cast --- src/main/flight/mixer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5d9e3e64f0..91de60ea67 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -779,7 +779,7 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq); + servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); From f5afb74126d84998c05ff3d140556ccde7331c91 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sat, 10 Jan 2015 14:33:52 -0500 Subject: [PATCH 22/24] motorCount needs to be global --- src/main/flight/mixer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 91de60ea67..ebe74cda32 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -59,7 +59,7 @@ extern int16_t debug[4]; #ifndef UNIT_TEST -static uint8_t motorCount = 0; +uint8_t motorCount = 0; static int useServo; static uint8_t servoCount; #endif From ec946ea7d581290d4e4bc0c168aa257729a2bb18 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sat, 31 Jan 2015 11:46:44 -0500 Subject: [PATCH 23/24] Split filter into separate file for future reuse --- Makefile | 1 + src/main/flight/lowpass.c | 115 +++++++++++++++++ src/main/flight/lowpass.h | 41 ++++++ src/main/flight/mixer.c | 118 ++---------------- src/main/flight/mixer.h | 20 --- src/main/mw.c | 1 + src/test/Makefile | 32 +++-- ...{mixer_unittest.cc => lowpass_unittest.cc} | 91 ++++---------- 8 files changed, 210 insertions(+), 209 deletions(-) create mode 100755 src/main/flight/lowpass.c create mode 100644 src/main/flight/lowpass.h rename src/test/unit/{mixer_unittest.cc => lowpass_unittest.cc} (53%) diff --git a/Makefile b/Makefile index aadb9ab50c..51cb6585d9 100644 --- a/Makefile +++ b/Makefile @@ -206,6 +206,7 @@ COMMON_SRC = build_config.c \ flight/flight.c \ flight/imu.c \ flight/mixer.c \ + flight/lowpass.c \ drivers/bus_i2c_soft.c \ drivers/serial.c \ drivers/sound_beeper.c \ diff --git a/src/main/flight/lowpass.c b/src/main/flight/lowpass.c new file mode 100755 index 0000000000..4b53060c09 --- /dev/null +++ b/src/main/flight/lowpass.c @@ -0,0 +1,115 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "flight/lowpass.h" + +void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter) +{ + float fixedScaler; + int i; + + // generates coefficients for a 2nd-order butterworth low-pass filter + float freqf = (float)freq*0.001f; + float omega = tanf((float)M_PI*freqf/2.0f); + float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f); + + +#ifdef UNIT_TEST + printf("lowpass cutoff: %f, omega: %f\n", freqf, omega); +#endif + + filter->bf[0] = scaling * omega*omega; + filter->bf[1] = 2.0f * filter->bf[0]; + filter->bf[2] = filter->bf[0]; + + filter->af[0] = 1.0f; + filter->af[1] = scaling * (2.0f * omega*omega - 2.0f); + filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f); + + // Scale for fixed-point + filter->input_bias = 1500; // Typical servo range is 1500 +/- 500 + filter->input_shift = 16; + filter->coeff_shift = 24; + fixedScaler = (float)(1ULL << filter->coeff_shift); + for (i = 0; i < LOWPASS_NUM_COEF; i++) { + filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler); + filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler); +#ifdef UNIT_TEST + printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i, + filter->bf[i], filter->af[i], filter->b[i], filter->a[i]); +#endif + } + + filter->freq = freq; +} + +int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq) +{ + int16_t coefIdx; + int64_t out; + int32_t in_s; + + // Check to see if cutoff frequency changed + if (freq != filter->freq) { + filter->init = false; + } + + // Initialize if needed + if (!filter->init) { + generateLowpassCoeffs2(freq, filter); + for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift; + filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift; + } + filter->init = true; + } + + // Unbias input and scale + in_s = (in - filter->input_bias) << filter->input_shift; + + // Delays + for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { + filter->x[coefIdx] = filter->x[coefIdx-1]; + filter->y[coefIdx] = filter->y[coefIdx-1]; + } + filter->x[0] = in_s; + + // Accumulate result + out = filter->x[0] * filter->b[0]; + for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { + out -= filter->y[coefIdx] * filter->a[coefIdx]; + out += filter->x[coefIdx] * filter->b[coefIdx]; + } + + // Scale output by coefficient shift + out >>= filter->coeff_shift; + filter->y[0] = (int32_t)out; + + // Scale output by input shift and round + out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift; + + // Reapply bias + out += filter->input_bias; + + return (int32_t)out; +} + diff --git a/src/main/flight/lowpass.h b/src/main/flight/lowpass.h new file mode 100644 index 0000000000..bbd20f4b93 --- /dev/null +++ b/src/main/flight/lowpass.h @@ -0,0 +1,41 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + + +#define LOWPASS_NUM_COEF 3 +#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f)) + +typedef struct lowpass_t { + bool init; + int16_t freq; // Normalized freq in 1/1000ths + float bf[LOWPASS_NUM_COEF]; + float af[LOWPASS_NUM_COEF]; + int64_t b[LOWPASS_NUM_COEF]; + int64_t a[LOWPASS_NUM_COEF]; + int16_t coeff_shift; + int16_t input_shift; + int32_t input_bias; + float xf[LOWPASS_NUM_COEF]; + float yf[LOWPASS_NUM_COEF]; + int32_t x[LOWPASS_NUM_COEF]; + int32_t y[LOWPASS_NUM_COEF]; +} lowpass_t; + +void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter); +int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 7143a96341..84ac033855 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -18,11 +18,6 @@ #include #include #include -#include - -#ifdef UNIT_TEST -#include -#endif #include "platform.h" @@ -30,12 +25,11 @@ #include "common/axis.h" #include "common/maths.h" -#ifndef UNIT_TEST + #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" -#endif #include "rx/rx.h" #include "io/gimbal.h" @@ -44,6 +38,7 @@ #include "flight/mixer.h" #include "flight/flight.h" +#include "flight/lowpass.h" #include "config/runtime_config.h" #include "config/config.h" @@ -55,19 +50,16 @@ //#define MIXER_DEBUG -#include "drivers/system.h" extern int16_t debug[4]; -#ifndef UNIT_TEST uint8_t motorCount = 0; -static int useServo; -static uint8_t servoCount; -#endif - int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t servo[MAX_SUPPORTED_SERVOS]; +static int useServo; + +static uint8_t servoCount; static servoParam_t *servoConf; static mixerConfig_t *mixerConfig; @@ -77,10 +69,8 @@ static airplaneConfig_t *airplaneConfig; static rxConfig_t *rxConfig; static gimbalConfig_t *gimbalConfig; -#ifndef UNIT_TEST static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static mixerMode_e currentMixerMode; -#endif static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; static const motorMixer_t mixerQuadX[] = { @@ -244,7 +234,6 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon gimbalConfig = gimbalConfigToUse; } -#ifndef UNIT_TEST int16_t determineServoMiddleOrForwardFromChannel(int nr) { uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel; @@ -524,7 +513,7 @@ void mixTable(void) int16_t maxMotor; uint32_t i; - // paranoia: give all servos a default command; prevents drift on unused servos with lowpass enabled + // paranoia: give all servos a default command for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = DEFAULT_SERVO_MIDDLE; } @@ -676,99 +665,6 @@ bool isMixerUsingServos(void) return useServo; } -#endif - -void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter) -{ - float fixedScaler; - int i; - - // generates coefficients for a 2nd-order butterworth low-pass filter - float freqf = (float)freq*0.001f; - float omega = tanf(M_PI*freqf/2.0f); - float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f); - - -#ifdef UNIT_TEST - printf("lowpass cutoff: %f, omega: %f\n", freqf, omega); -#endif - - filter->bf[0] = scaling * omega*omega; - filter->bf[1] = 2.0f * filter->bf[0]; - filter->bf[2] = filter->bf[0]; - - filter->af[0] = 1.0f; - filter->af[1] = scaling * (2.0f * omega*omega - 2.0f); - filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f); - - // Scale for fixed-point - filter->input_bias = 1500; // Typical servo range is 1500 +/- 500 - filter->input_shift = 16; - filter->coeff_shift = 24; - fixedScaler = (float)(1ULL << filter->coeff_shift); - for (i = 0; i < LOWPASS_NUM_COEF; i++) { - filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler); - filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler); -#ifdef UNIT_TEST - printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i, - filter->bf[i], filter->af[i], filter->b[i], filter->a[i]); -#endif - } - - filter->freq = freq; -} - -static int32_t lowpass_fixed(lowpass_t *filter, int32_t in, int16_t freq) -{ - int16_t coefIdx; - int64_t out; - int32_t in_s; - - // Check to see if cutoff frequency changed - if (freq != filter->freq) { - filter->init = false; - } - - // Initialize if needed - if (!filter->init) { - generate_lowpass_coeffs2(freq, filter); - for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift; - filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift; - } - filter->init = true; - } - - // Unbias input and scale - in_s = (in - filter->input_bias) << filter->input_shift; - - // Delays - for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) { - filter->x[coefIdx] = filter->x[coefIdx-1]; - filter->y[coefIdx] = filter->y[coefIdx-1]; - } - filter->x[0] = in_s; - - // Accumulate result - out = filter->x[0] * filter->b[0]; - for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { - out -= filter->y[coefIdx] * filter->a[coefIdx]; - out += filter->x[coefIdx] * filter->b[coefIdx]; - } - - // Scale output by coefficient shift - out >>= filter->coeff_shift; - filter->y[0] = (int32_t)out; - - // Scale output by input shift and round - out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift; - - // Reapply bias - out += filter->input_bias; - - return (int32_t)out; -} - void filterServos(void) { int16_t servoIdx; @@ -779,7 +675,7 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq); + servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 607ec52abd..6a0859a986 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -92,26 +92,6 @@ typedef struct servoParam_t { int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED } servoParam_t; -#define LOWPASS_NUM_COEF 3 -#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f)) - -typedef struct lowpass_t { - bool init; - int16_t freq; // Normalized freq in 1/1000ths - float bf[LOWPASS_NUM_COEF]; - float af[LOWPASS_NUM_COEF]; - int64_t b[LOWPASS_NUM_COEF]; - int64_t a[LOWPASS_NUM_COEF]; - int16_t coeff_shift; - int16_t input_shift; - int32_t input_bias; - float xf[LOWPASS_NUM_COEF]; - float yf[LOWPASS_NUM_COEF]; - int32_t x[LOWPASS_NUM_COEF]; - int32_t 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]; diff --git a/src/main/mw.c b/src/main/mw.c index 028177515a..5fdc6e5339 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -715,6 +715,7 @@ void loop(void) ); mixTable(); + filterServos(); writeServos(); writeMotors(); diff --git a/src/test/Makefile b/src/test/Makefile index c2fff7a1c9..5ea38fe9f0 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -53,7 +53,7 @@ TESTS = \ rc_controls_unittest \ ledstrip_unittest \ ws2811_unittest \ - mixer_unittest + lowpass_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -320,16 +320,30 @@ ws2811_unittest : \ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) - @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@ -$(OBJECT_DIR)/mixer_unittest.o : $(TEST_DIR)/mixer_unittest.cc $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS) - @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/mixer_unittest.cc -o $@ +$(OBJECT_DIR)/flight/lowpass.o : \ + $(USER_DIR)/flight/lowpass.c \ + $(USER_DIR)/flight/lowpass.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/lowpass.c -o $@ + +$(OBJECT_DIR)/lowpass_unittest.o : \ + $(TEST_DIR)/lowpass_unittest.cc \ + $(USER_DIR)/flight/lowpass.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/lowpass_unittest.cc -o $@ + +lowpass_unittest : \ + $(OBJECT_DIR)/flight/lowpass.o \ + $(OBJECT_DIR)/lowpass_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -mixer_unittest : $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/mixer_unittest.o $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ test: $(TESTS) set -e && for test in $(TESTS) ; do \ diff --git a/src/test/unit/mixer_unittest.cc b/src/test/unit/lowpass_unittest.cc similarity index 53% rename from src/test/unit/mixer_unittest.cc rename to src/test/unit/lowpass_unittest.cc index 4879aaa896..1b07987b79 100644 --- a/src/test/unit/mixer_unittest.cc +++ b/src/test/unit/lowpass_unittest.cc @@ -15,29 +15,19 @@ * along with Cleanflight. If not, see . */ #include - #include extern "C" { - #include "flight/mixer.h" - #include "rx/rx.h" - #include "io/gimbal.h" - #include "io/escservo.h" - extern void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse); - extern void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter); + #include "flight/lowpass.h" } -uint32_t debug[4]; -static int16_t servoRef[MAX_SUPPORTED_SERVOS]; -static int16_t referenceOut[MAX_SUPPORTED_SERVOS]; -static uint16_t freq; -static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; -static servoParam_t servoConfig[MAX_SUPPORTED_SERVOS]; +static lowpass_t lowpassFilterReference; +static lowpass_t lowpassFilter; #include "unittest_macros.h" #include "gtest/gtest.h" -static float lowpass_ref(lowpass_t *filter, float in, int16_t freq) +static float lowpassRef(lowpass_t *filter, float in, int16_t freq) { int16_t coefIdx; float out; @@ -49,7 +39,7 @@ static float lowpass_ref(lowpass_t *filter, float in, int16_t freq) // Initialize if needed if (!filter->init) { - generate_lowpass_coeffs2(freq, filter); + generateLowpassCoeffs2(freq, filter); for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) { filter->xf[coefIdx] = in; filter->yf[coefIdx] = in; @@ -75,24 +65,14 @@ static float lowpass_ref(lowpass_t *filter, float in, int16_t freq) return out; } -static void filterServosReference(void) -{ - int16_t servoIdx; - - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - // Round to nearest - referenceOut[servoIdx] = (int16_t)(lowpass_ref(&lowpassFilters[servoIdx], (float)servoRef[servoIdx], freq) + 0.5f); - } -} - - -TEST(MixerTest, ServoLowpassFilter) +TEST(LowpassTest, Lowpass) { int16_t servoCmds[3000]; int16_t expectedOut[3000]; - uint8_t servoIdx; + int16_t referenceOut; + int16_t filterOut; uint16_t sampleIdx; - static mixerConfig_t mixerConfig; + int16_t freq; uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t); @@ -115,38 +95,26 @@ TEST(MixerTest, ServoLowpassFilter) } } - // Set mixer configuration - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servoConfig[servoIdx].min = 0; - servoConfig[servoIdx].max = 3000; - } - // Test all frequencies - for (freq = 10; freq <= 400; freq++) - { + for (freq = 10; freq <= 400; freq++) { printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f)); - mixerConfig.servo_lowpass_enable = 1; - mixerConfig.servo_lowpass_freq = freq; - mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL); - // Run tests - for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) { - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = servoCmds[sampleIdx]; - servoRef[servoIdx] = servoCmds[sampleIdx]; - } + for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) + { + // Filter under test + filterOut = (int16_t)lowpassFixed(&lowpassFilter, servoCmds[sampleIdx], freq); - filterServos(); - filterServosReference(); + // Floating-point reference + referenceOut = (int16_t)(lowpassRef(&lowpassFilterReference, (float)servoCmds[sampleIdx], freq) + 0.5f); - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - if (expectedOut[sampleIdx] >= 0) { - EXPECT_EQ(servo[servoIdx], expectedOut[sampleIdx]); - } - EXPECT_LE(servo[servoIdx], referenceOut[servoIdx] + 1); - EXPECT_GE(servo[servoIdx], referenceOut[servoIdx] - 1); + if (expectedOut[sampleIdx] >= 0) { + EXPECT_EQ(filterOut, expectedOut[sampleIdx]); } + // Some tolerance + // TODO adjust precision to remove the need for this? + EXPECT_LE(filterOut, referenceOut + 1); + EXPECT_GE(filterOut, referenceOut - 1); } // for each sample } // for each freq } @@ -155,21 +123,6 @@ TEST(MixerTest, ServoLowpassFilter) extern "C" { -void delay(uint32_t ms) -{ - UNUSED(ms); - return; -} - -int constrain(int amt, int low, int high) -{ - return (amt > high ? high : (amt < low ? low : amt)); -} - -uint32_t micros() -{ - return 0; -} } From 302701772c170372ac369492f70c85558e770fd5 Mon Sep 17 00:00:00 2001 From: Joel Fuster Date: Sat, 31 Jan 2015 20:47:41 -0500 Subject: [PATCH 24/24] Add missing function declaration --- src/main/flight/mixer.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 6a0859a986..14945d80eb 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -103,3 +103,4 @@ void mixerResetMotors(void); void mixTable(void); void writeServos(void); void writeMotors(void); +void filterServos(void);