Lowpass testing
This commit is contained in:
Joel Fuster
parent
c5b822ecf1
commit
3eee9eb079
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 }
|
||||
};
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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 },
|
||||
|
|
|
|||
Loading…
Reference in New Issue