File content as of revision 40:a68cc1a1a1e7:

//Maybe you rather use the routine I currently use in FarmerGPS? It uses a lookahead and simply distance to AB-line.
//No heading error at all.

//ControlSteerFilter is the main routine. ActiveTime in ms, typically under 200ms, distAUTOL is distance to AB line at lookahead position
#include "mbed.h"
#include <string.h>
#include <math.h>
#include <stdlib.h>
#define PI 3.14159265359


double m_Tcenter[10];
double mphaseadv[10];
double morder[10];
int order;
double B0[10];
double B1[10];
double B2[10];
double B3[10];
double A_1[10];
double A_2[10];
double A_3[10];

double mx[10];
double my[10];
double mz[10];
int Err_aPort = 0;

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

double SpeedN = 1;

std::string itos(int n)
{
    const int max_size = std::numeric_limits<int>::digits10 + 1 /*sign*/ + 1 /*0-terminator*/;
    char buffer[max_size] = {0};
    sprintf(buffer, "%d", n);
    return std::string(buffer);
}

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

    //alogfile = "AUTO-" + equipment.fieldfilename + ".txt";

    // double phaseadv = 47;
    // double _Tcenter = 3.6;
    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);
  //  int iFilterCount = 1;

    //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 ((Err_aPort == 0)) {
        //  if (ActiveTime > 300) ActiveTime = 300;
        if (value < 0) value = 0;
        if ((value > 255)) value = 255;
        OutputValue = value;
        OutputTime = ActiveTime;

        // d =  //= DateTime.Now - autosteer.LastCommunication;

        //no need to send repeated 127=do nothing
        if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
            sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
            LastOutputValue = OutputValue;
            //  autosteer.Timer1counter = 0;
            //  autosteer.LastCommunication = DateTime.Now;
        } else {
            //no data expected
        }
    }
    return (sRet);
}

string ControlSteerFilter( int ActiveTime,  double distAUTOL, double lddist, double speed, bool steer, double FilterGain)
{
    string sRet = "";

    int N = 3;
    float y = 0;
    if (B0[0] == 9999.0) {
        SetDigitalFilter(2, 47, 4.2 / 2 / PI,0);
    }
    if (distAUTOL == 5000) distAUTOL = 0; //not set

    if (speed < 1) steer=false;

    mx[N - 3] = mx[N - 2];
    mx[N - 2] = mx[N - 1];
    mx[N - 1] = mx[N];
    mx[N] = distAUTOL * FilterGain;

    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];

    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];

    //if (iFilterCount > 1) {
   //     mz[N] = -A_1[1] * (double)mz[N - 1] - A_2[1] * (double)mz[N - 2] - A_3[1] * (double)mz[N - 3] + B0[1] * (double)my[N] + B1[1] * (double)my[N - 1] + B2[1] * (double)mz[N - 2] + B3[1] * (double)mz[N - 3];
  //  } else {
        mz[N] = my[N];
  //  }

    //done calculating, shift array values. now done at loop start
    float y1 = (float)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
    // if (morder[1] == 0) y1 = (float)my[N];

    //modify scale depending on distance!
    double mscale = scale;
    if (abs(distAUTOL) > 0.5) mscale = scale * 1.5;
    if (abs(distAUTOL) > 1.5) mscale = scale * 1.5;

    y = (float)(y1 * mscale);      // scale it here   if neccesary
    // y = (float) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
    y = (float)(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 = 127 + y;
    if (y >= 255) y = 255;
    if (y <= 0) y = 0;
    int value = (int)y;
    if (steer) {
        sRet= Steer( ActiveTime, value);
    } else {
        //   autosteer.LastCommunication = DateTime.Now;
        //   return (sRet.c_str());
    }
    return (sRet);
}