File content as of revision 54:2405c62c1049:

#ifndef PI
#define PI 3.14159265359
#endif

double m_Tcenter[5] = {0,0,0,0,0};
double mphaseadv[5]= {0,0,0,0,0};
double morder[5]= {0,0,0,0,0};
int order;
double B0[5]= {0,0,0,0,0};
double B1[5]= {0,0,0,0,0};
double B2[5]= {0,0,0,0,0};
double B3[5]= {0,0,0,0,0};
double A_1[5]= {0,0,0,0,0};
double A_2[5]= {0,0,0,0,0};
double A_3[5]= {0,0,0,0,0};

double mx[5]= {0,0,0,0,0};
double my[5]= {0,0,0,0,0};
double mz[5]= {0,0,0,0,0};
int Err_aPort = 0;

double OutputValue = 0;
double OutputTime = 0;
double LastOutputValue = 0;

double SpeedN = 1;
int porder = 0;

void SetDigitalFilter(double phaseadv, double _Tcenter, int filternumber)
{
    m_Tcenter[filternumber] = _Tcenter;
    mphaseadv[filternumber] = phaseadv;
    morder[filternumber] = porder;
    _Tcenter = _Tcenter / 2 / PI;
    order = porder;

    double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
    double _T1 = sqrt(T1T2ratio) * _Tcenter;
    double _T2 =_T1 / T1T2ratio;
    double _T = 0.2;
    double _K = (1 + 2 * _T1 / _T);
    double _L = (1 - 2 * _T1 / _T);
    double _M = (1 + 2 * _T2 / _T);
    double _N = (1 - 2 * _T2 / _T);
//   order = 2;
    //version 1,
    switch (order) {
        case 3:
            B0[filternumber] = pow(_K, 3) / pow(_M, 3);
            B1[filternumber] = 3 * pow(_K, 2) * _L / pow(_M, 3);
            B2[filternumber] = 3 * _K * pow(_L, 2) / pow(_M, 3);
            B3[filternumber] = pow(_L, 3) / pow(_M, 3);

            A_1[filternumber] = 3 * _N / _M;
            A_2[filternumber] = 3 * pow(_N, 2) / pow(_M, 2);
            A_3[filternumber] = pow(_N, 3) / pow(_M, 3);
            break;
        case 2:
            B0[filternumber] = pow(_K, 2) / pow(_M, 2);
            B1[filternumber] = 2 * _K * _L / pow(_M, 2);
            B2[filternumber] = pow(_L, 2) / pow(_M, 2);
            B3[filternumber] = 0;

            A_1[filternumber] = 2 * _N / _M;
            A_2[filternumber] = pow(_N, 2) / pow(_M, 2);
            A_3[filternumber] = 0;
            break;
        case 1:
        case 0:
            B0[filternumber] = _K / _M;
            B1[filternumber] = _L / _M;
            B2[filternumber] = 0;
            B3[filternumber] = 0;

            A_1[filternumber] = _N / _M;
            A_2[filternumber] = 0;
            A_3[filternumber] = 0;
            break;
    }
}
//double d = 0;

string Steer(int ActiveTime,int value)
{
    string sRet = "";
    if (value < 0) value = 0;
    if ((value > 255)) value = 255;
    OutputValue = value;
    OutputTime = ActiveTime;
    sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
    LastOutputValue = OutputValue;

    return (sRet);
}

string ControlSteerFilter(int ActiveTime,  double distAUTOL, double speed, double FilterGain, double min, double max,double SCALE)
{
    string sRet = "";
    int N = 3;
    double y = 0;

    mx[N - 3] = mx[N - 2];
    mx[N - 2] = mx[N - 1];
    mx[N - 1] = mx[N];
    if ( FilterGain > 0 ) {
        if ( abs(distAUTOL) > 0 ) {
            mx[N] = distAUTOL * FilterGain;

            my[N] = -A_1[0] * (double)my[N - 1] - A_2[0] * (double)my[N - 2] - A_3[0] * (double)my[N - 3] + B0[0] * (double)mx[N] + B1[0] * (double)mx[N - 1] + B2[0] * (double)mx[N - 2] + B3[0] * (double)mx[N - 3];
            mz[N] = my[N];

            my[N - 3] = my[N - 2];
            my[N - 2] = my[N - 1];
            my[N - 1] = my[N];

            mz[N - 3] = mz[N - 2];
            mz[N - 2] = mz[N - 1];
            mz[N - 1] = mz[N];
        }
    }
    double y1 = (double)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
    //modify scale depending on distance!
    double mscale = 0.1 * abs(distAUTOL) + SCALE;
    y = (double)(y1 * mscale);      // scale it here   if neccesary
    y = (double)(y * pow((10.0 / speed), SpeedN)); //SpeedN varies the gain factor with speed  n=0 to 1 or more. n=0 should give  y=

    y = y * mscale;

    if ( y < -max ) {
        y = -max;
    } else if ( y > max) {
        y = max;
    }
    y = 127 + y;
    if (y <= 127) y = y - (min / 2.0);
    if (y >= 127) y = y + (min / 2.0);

    if (y >= 255) y = 255;
    if (y <= 0) y = 0;

    int value = (int)y;
    if (speed > 1.0 ) {
        sRet= Steer( ActiveTime, value);
    } else {
        sRet = Steer( ActiveTime, 127 );
    }

    return (sRet);
}