File content as of revision 41:a26acd346c2f:

#include "mbed.h"
#include "PinDetect.h"
#include "Point.h"
#include <vector>
#include "Line.h"
#include "stringUtils.h"
#include "base.h"
#include "Config.h"
#include "imu_functions.h"
#include "atoh.h"
#include "checksum.h"
#include <string.h>

#define dot(u,v)   ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
#define norm(v)     sqrt(dot(v,v))     // norm = length of  vector
#define d(u,v)      norm(point_sub(u,v))          // distance = norm of difference

char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
long lastsend_version=0;
Timer vTimer; //this timer is int based! Max is 30 minutes

int checksumm;
double distance_from_line;
double cm_per_deg_lon;
double cm_per_deg_lat;
//all timing objects
Timer gps_connecting;
Timer autosteer_time;
Timer autosteer_timeout; //timeout work near as timer, but they don't give timing. they just trigger an interrupt once the time we assigned it is passed.
Ticker accelerometerTicker;
Ticker gyroscopeTicker;
Ticker filterTicker;
Ticker  angle_print;

//Motor
PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
DigitalOut enable_motor(p7);

PwmOut pwm1(p21);
PwmOut pwm2(p22);

//equipment switches
PinDetect  boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect  boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect  boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect  boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.

char boom18; //1 byte
char lastboom18; //1 byte
char boomstate[8]= {'$','F','B','S',0,13,10,0 };

double filterg = 100;
Point position;
Point looked_ahead;
Point line_start;
Point line_end;
Point tilt_compensated_position;
Point yaw_compensated_position;

extern int gyro_pos;
double distance_to_line;

//FreePilot variables
int timer_enabled;
double motorspeed;
int enable_time;
char* motor_enable_state = 0;
int motor_enable = 0;
int lastmotor_enable = 1;
double pwm1_speed;
double pwm2_speed;
long lastsend_motorstate=0;
Timer motTimer; //this timer is int based! Max is 30 minutes
Timer btTimer; //measure time for Bluetooth communication
long lastgetBT=0;

int msg2_changed = 1;
char* buffer;
double meter_lat = 0;
double meter_lon = 0;

char msg[256]; //GPS line buffer
char msg2[256];//PC line buffer
int printing;
int num_of_gps_sats;

double decimal_lon;
float longitude;
float latitude;
char ns, ew;
int lock;
int flag_gga;
int reading;
double decimal_latitude;
int  gps_satellite_quality;
int  day;
int  hour;
int  minute;
int  second;
int  tenths;
int  hundreths;
char status;
double track;    // track made good . angle
char magvar_dir;
double magvar;
int  year;
int  month;
double speed_km;
double speed_m_s = 0;
double velocity; // speed in knot
int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
int connecting = 0; //are we still in phase of connecting? based on the connect_time value.

int angle_send = 0;
int correct_rmc = 1;
double m_lat = 0;
double m_lon = 0;
char* degminsec;
double m_per_deg_lon;
double m_per_deg_lat;
double look_ahead_lon;
double look_ahead_lat;
int active_AB = 0;
double compensation_vector;
char output[256];

double yaw;
double pitch;
double roll;

double a_x;
double a_y;
double a_z;
double w_x;
double w_y;
double w_z;

int readings[3];
double Freepilot_bearing;

int time_till_stop = 200;
volatile bool newline_detected = false;

Point point_add(Point a, Point b)
{
    return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
}

Point point_sub(Point a , Point b)
{
    return Point(a.GetX() - b.GetX(),  a.GetY() - b.GetY());
}

double get_yaw()
{
    double yaw_angle = imuFilter.getYaw() * -1 ; // *-1 to reverse polarity as gyro is up-side-down
    return yaw_angle;
}

double get_roll()
{
    double roll_angle = 0;
    if ( gyro_pos == 0 ) {
        roll_angle = imuFilter.getRoll();
    } else if ( gyro_pos == 1 ) {
        roll_angle = imuFilter.getRoll() * -1;
    } else if( gyro_pos == 2 ) {
        roll_angle = imuFilter.getPitch() * -1;
    } else if ( gyro_pos == 3 ) {
        roll_angle = imuFilter.getPitch();
    }
    return roll_angle;
}

double get_pitch()
{
    double pitch_angle = 0;
    if ( gyro_pos == 0 ) {
        pitch_angle = imuFilter.getPitch();
    } else if ( gyro_pos == 1 ) {
        pitch_angle = imuFilter.getPitch() * -1;
    } else if( gyro_pos == 2 ) {
        pitch_angle = imuFilter.getRoll();
    } else if ( gyro_pos == 3 ) {
        pitch_angle = imuFilter.getRoll() * -1;
    }
    return pitch_angle;
}

double dist_Point_to_Line( Point P, Point line_start, Point line_end)
{
    Point v = point_sub(line_end,line_start);
    Point w = point_sub(P,line_start);

    double c1 = dot(w,v);
    double c2 = dot(v,v);
    double b = c1 / c2;

    Point resulting(b * v.GetX(),b*v.GetY());
    Point Pb = point_add(line_start, resulting);

    return d(P, Pb);
}

double lat_to_deg(char *s, char north_south)
{
    int deg, min, sec;
    double fsec, val;

    deg  = ( (s[0] - '0') * 10) + s[1] - '0';
    min  = ( (s[2] - '0') * 10) + s[3] - '0';
    sec  = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
    fsec = (double)((double)sec /10000.0);
    val  = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
    if (north_south == 'S') {
        val *= -1.0;
    }
    return val;
}

// isLeft(): test if a point is Left|On|Right of an infinite 2D line.
//    Input:  three points P0, P1, and P2
//    Return: >0 for P2 left of the line through P0 to P1
//          =0 for P2 on the line
//          <0 for P2 right of the line
int isLeft( Point P0, Point P1, Point P2 )
{
    double isleft = ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
    if ( isleft > 0 ) {
        isleft = 1;
    } else {
        isleft = -1;
    }
    return (int)isleft;
}

double lon_to_deg(char *s, char east_west)
{
    int deg, min, sec;
    double fsec, val;
    deg  = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
    min  = ( (s[3] - '0') * 10) + s[4] - '0';
    sec  = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
    fsec = (double)((double)sec /10000.0);
    val  = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);

    if (east_west == 'W') {
        val *= -1.0;
    }
    return val;
}

void nmea_gga(char *s)
{
    char *token;
    int  token_counter = 0;
    char *latitude  = (char *)NULL;
    char *longitude = (char *)NULL;
    char *lat_dir   = (char *)NULL;
    char *lon_dir   = (char *)NULL;
    char *qual      = (char *)NULL;
    char *altitude  = (char *)NULL;
    char *sats      = (char *)NULL;

    token = strtok(s, ",");
    while (token) {
        switch (token_counter) {
            case 2:
                latitude  = token;
                break;
            case 4:
                longitude = token;
                break;
            case 3:
                lat_dir   = token;
                break;
            case 5:
                lon_dir   = token;
                break;
            case 6:
                qual      = token;
                break;
            case 7:
                sats      = token;
                break;
            case 9:
                altitude  = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if (latitude && longitude && altitude && sats) {
        decimal_latitude = lat_to_deg(latitude,  lat_dir[0]);
        decimal_lon = lon_to_deg(longitude, lon_dir[0]);
        num_of_gps_sats = atoi(sats);
        gps_satellite_quality = atoi(qual);
    } else {
        gps_satellite_quality = 0;
    }
}

void autosteer_done()
{
    enable_motor = 0;
}

//from farmerGPS code
void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
{
    double ydist = 0;
    double xdist = 0;
    angle = angle + 180;
    double radiant = angle * 3.14159265359 / 180;
    double sinr = sin(radiant);
    double cosr = cos(radiant);
    xdist = cosr * distance;
    ydist = sinr * distance;
    lat2 = lat1 + (ydist / (69.09 * -1609.344));
    lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
}

Point compensation;

void yaw_compensate()
{
    yaw = get_yaw();
}

void pitch_and_roll(double real_bearing)
{
    pitch = get_pitch();
    roll = get_roll();
    compensation.SetX(antennaheight * tan(roll) * sin(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * cos(real_bearing/ 57.295779513));
    compensation.SetY(antennaheight * tan(roll) * cos(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * sin(real_bearing/ 57.295779513));
}

void process_GPSHEIGHT(char* height_string)
{
    char *token;
    int  token_counter = 0;
    char *height  = (char *)NULL;
    token = strtok(height_string, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                height = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( height ) {
        antennaheight = atof(height);
        Config_Save();
    }
}

char dms[128];
char* To_DMS(double dec_deg)
{
    dec_deg = abs(dec_deg);
    int d = (int)(dec_deg);
    sprintf(dms,"%0.2i\0",d);
    double m = (double)(((double)dec_deg - (double)d) * 60.0);
    if (m < 10 ) {
        sprintf(dms,"%s0%0.9f\0",dms,m);
    } else {
        sprintf(dms,"%s%0.9f\0",dms,m);
    }
    return dms;
}

char* To_DMS_lon(double dec_deg)
{
    dec_deg = abs(dec_deg);
    int d = (int)(dec_deg);
    sprintf(dms,"%0.3i\0",d);
    double m = (double)(((double)dec_deg - (double)d) * 60.0);
    if (m < 10 ) {
        sprintf(dms,"%s0%0.9f\0",dms,m);
    } else {
        sprintf(dms,"%s%0.9f\0",dms,m);
    }
    return dms;
}

//sets pwm1 and pwm2 and enable_motor
void process_ASTEER(char* asteer)
{
    char *token;
    int  token_counter = 0;
    char *asteer_speed  = (char *)NULL;
    char *asteer_time = (char *)NULL;
    token = strtok(asteer, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                asteer_speed  = token;
                break;
            case 2:
                asteer_time = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( asteer_speed && asteer_time ) {
        motorspeed = atof(asteer_speed);
        enable_time = atof(asteer_time);
        autosteer_timeout.reset();
        time_till_stop = atoi(asteer_time);
        if ( motor_enable == 0 ) {
        } else {
            if ( motorspeed > 127.0 ) {
                pwm2_speed = 0.0;
                pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;

            } else if ( motorspeed < 127.0 ) {
                pwm1_speed = 0.0;
                pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
            } else {
                pwm1_speed = 0;
                pwm2_speed = 0;
                enable_motor = 0;
            }
            //  if(Authenticated)
            // {
            pwm1 = pwm1_speed;
            pwm2 = pwm2_speed;
            enable_motor = 1;
            //}
        }
    }
}

double lastval = 0;
//gets the motor value between -255 and 255 (- means left, positive means right)
//distance in meters, always positive
//angle in degrees
//Points in lat/lon format
int freepilot(Point current, Point target, double heading_err, double dist_line, double scale, double filter_g, double phase_adv, double center, double filtering)//dist in meters
{
    dist_line = abs(dist_line);
    double error = 1;
    int position_sign = isLeft( line_start, line_end, current);
    int forward_sign = isLeft(line_start,line_end, target);
    double position_dist = abs(dist_Point_to_Line( current,line_start,line_end) * filtering);

    if ( forward_sign == -1 ) {
        error = error * -1;
    } else if ( forward_sign == 1 ) {
        error = error;
    }
    //if ( abs(position_dist) < 0.5 ) {
    if ( forward_sign == position_sign ) {
        if ( position_dist > dist_line ) { // && abs(position_dist) <
            // error = error * (dist_line * filter_g - (position_dist * phase_adv));
            // error = 0;
        } else {
            // error = error * (dist_line * filter_g - (position_dist * phase_adv)*0.8);
            error = error * dist_line * filter_g;
        }
    } else { //
        error = (error * ((dist_line * filter_g) - (position_dist * phase_adv)))*0.8;//.8 in order to come back less on line than we came on it
    }
    //} else { //target coming back at 15-20 degrees on the line
    error = error;// + heading_err  * 2 ;
    //}

    error = error * scale;
    if ( error > 0 ) {
        error = error + center;
    } else if (error < 0 ) {
        error = error - center;
    }
    if ( error > 255 ) {
        error = 255;
    } else if ( error < -255 ) {
        error = -255;
    }

    error = error + 255;
    error = (int)(error / 2);

    return (int)error;
}

char *strsep(char **stringp, char *delim)
{
    char *s;
    const char *spanp;
    int c, sc;
    char *tok;
    if ((s = *stringp) == NULL)
        return (NULL);
    for (tok = s;;) {
        c = *s++;
        spanp = delim;
        do {
            if ((sc = *spanp++) == c) {
                if (c == 0)
                    s = NULL;
                else
                    s[-1] = 0;
                *stringp = s;
                return (tok);
            }
        } while (sc != 0);
    }
    /* NOTREACHED */
}

//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>*/

#ifndef PI
#define PI 3.14159265359
#endif

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;
int porder = 0;

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(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 = "";
    //f ((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;
        //}
 //  == }
    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;
  //  if (B0[0] == 9999.0) {
   //     SetDigitalFilter(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];
    mz[N] = my[N];

    double y1 = (double)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
    pc.printf("%f\r\n",y1);

    //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 = (double)(y1 * mscale);      // scale it here   if neccesary
    pc.printf("%f\r\n",y);

    // y = (double) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
    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 * SCALE;
/*
            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);
*/

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

            int value = (int)y;
    pc.printf("%f\r\n",y);

    //if (steer) {
        sRet= Steer( ActiveTime, value);
    //}

    return (sRet);
}

Point old_position;

//char rmc_cpy[256];
void nmea_rmc(char *s)
{
    char *token;
    int  token_counter = 0;
    char *time   = (char *)NULL;
    char *date   = (char *)NULL;
    char *stat   = (char *)NULL;
    char *vel    = (char *)NULL;
    char *trk    = (char *)NULL;
    char *magv   = (char *)NULL;
    char *magd   = (char *)NULL;
    char *latit  = "";
    char *longit = "";
    char *latitude  = (char *)NULL;
    char *longitude = (char *)NULL;
    char *lat_dir   = (char *)NULL;
    char *lon_dir   = (char *)NULL;

    while ((token = strsep(&s, ",")) != NULL) {
        switch (token_counter) {
            case 1:
                time   = token;
                break;
            case 2:
                stat   = token;
                break;
            case 3:
                if ( token ) {
                    latit  = token;
                    latitude  = token;
                }
                break;
            case 4:
                lat_dir   = token;
                break;
            case 5:
                longit = token;
                longitude = token;
                break;
            case 6:
                lon_dir   = token;
                break;
            case 7:
                vel    = token;
                break;
            case 8:
                trk    = token;
                break;
            case 9:
                date   = token;
                break;
            case 10:
                magv   = token;
                break;
            case 11:
                magd = token;
                break;
        }
        token_counter++;
    }
    if (stat!= '\0' && date!= '\0' && time!= '\0') {
        hour       = (char)((time[0] - '0') * 10) + (time[1] - '0');
        minute     = (char)((time[2] - '0') * 10) + (time[3] - '0');
        second     = (char)((time[4] - '0') * 10) + (time[5] - '0');
        day        = (char)((date[0] - '0') * 10) + (date[1] - '0');
        month      = (char)((date[2] - '0') * 10) + (date[3] - '0');
        year       =  (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
        status     = stat[0];
        velocity   = atof(vel);
        speed_km = velocity * 1.852;
        speed_m_s = speed_km * 3600.0 / 1000.0;
        track      = atof(trk);
        magvar     = atof(magv);
    }
    double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
    double diff_angle = Freepilot_bearing - angle;
    diff_angle = ((int)diff_angle + 180) % 360 - 180;

    // pc.printf("%f  %f  %f\r\n",diff_angle, Freepilot_bearing, (track - 90) * -1);
    if ( abs(diff_angle) > 90 ) {
        if ( (abs(360 - diff_angle)) > 90 ) {
            Point temp = line_end;
            line_end = line_start;
            line_start = temp;
            Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
        }
    }
    if ( longit != '\0' && latit != '\0' ) {
        old_position = position;
        position.SetX(lat_to_deg(latitude,  lat_dir[0]));
        position.SetY(lon_to_deg(longitude, lon_dir[0]));
        cm_per_deg_lat = 11054000;
        cm_per_deg_lon = 11132000 * cos(decimal_latitude);

//        pitch_and_roll((track-90)*-1);// as to be real bearing

        compensation.SetY(compensation.GetY() / cm_per_deg_lon);
        compensation.SetX(compensation.GetX() / cm_per_deg_lat);

        position = point_add(position,compensation);

        double lookaheaddistance = lookaheadtime * speed_m_s;
        get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
        looked_ahead.SetX(look_ahead_lat);
        looked_ahead.SetY(look_ahead_lon);
        double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
        distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;/////////////////////////////////////////////////
             SetDigitalFilter(phaseadv,tcenter, 0 );

        //  int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale,  filterg , phaseadv,tcenter ,filtering);
        string steering = ControlSteerFilter(225, distance_to_line, speed_km , filterg, 20.0, 100.0, scale);
//pc.printf("%f\r\n",distance_to_line);


        char command[128];
        sprintf(command,"%s\r\n\0",steering.c_str());    //(int)((((val/2)-127)/scale)+127),500);
        pc.puts(command);

        process_ASTEER(command);
    }
    string rmc = "";
    if(time!= '\0') {
        rmc = "$GPRMC,";
        rmc +=  string(time) + ",";
    } else {
        rmc = "$GPRMC,,";
    }
    if(stat!= '\0') {
        rmc +=(string(stat) + ",");
    } else {
        rmc += ",";
    }
    if ( latit != '\0' && lat_dir!= '\0') {
        rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
    } else {
        rmc += ",,";
    }
    if ( longit != '\0' && lon_dir!= '\0' ) {
        rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
    } else {
        rmc += ",";
        rmc += ",";
    }
    if (vel!= '\0') {
        rmc += (string(vel) + ",");
    } else {
        rmc += ",";
    }
    if ((trk)!= '\0') {
        rmc += string(trk) + ",";
    } else {
        rmc += ",";
    }
    if (date!= '\0') {
        rmc += string(date) + ",";
    } else {
        rmc += ",";
    }
    if (magv!= '\0') {
        rmc += string(magv) + ",";
    } else {
        rmc += ",";
    }
    if (magd!= '\0') {
        rmc += string(magd) + ",W";
    } else {
        rmc += ",W";
    }

    char test[256];
    sprintf(test,"%s*\0",rmc.c_str());
    sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));

    bluetooth.puts(output);
}

void process_FGPSAB(char* ab)
{
    char *token;
    int  token_counter = 0;
    char *line_lat  = (char *)NULL;
    char *line_lon  = (char *)NULL;
    char *line_lat1  = (char *)NULL;
    char *line_lon1  = (char *)NULL;
    //char *bearing  = (char *)NULL;
    string bearing = "";
    token = strtok(ab, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                line_lat = token;
                break;
            case 2:
                line_lon = token;
                break;
            case 3:
                line_lat1 = token;
                break;
            case 4:
                line_lon1 = token;
                break;
            case 5:
                for (int n=0; n < sizeof(token); n++) {
                    if ( token[n] == '*' ) {
                        break;
                    } else {
                        bearing += token[n];
                    }
                }
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    double Freepilot_lon  = atof(line_lon);
    double Freepilot_lat = atof(line_lat);
    double Freepilot_lon1  = atof(line_lon1);
    double Freepilot_lat1 = atof(line_lat1);
    Freepilot_bearing = atof(bearing.c_str()) + 360;
    Freepilot_bearing = ((int)Freepilot_bearing+ 180) % 360 - 180;
    line_start.SetX(Freepilot_lat);
    line_start.SetY(Freepilot_lon);
    line_end.SetX(Freepilot_lat1);
    line_end.SetY(Freepilot_lon1);

    active_AB = 1;

    sprintf(output, "$ABLINE:%f , %f, %f, %f,  %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY(),Freepilot_bearing);
    pc.puts(output);
}

void process_FGPSAUTO(char* FGPSAUTO)
{
    char *token;
    int  token_counter = 0;
    char *ahead  = (char *)NULL;
    char *center  = (char *)NULL;
    char *phase  = (char *)NULL;
    char *scl  = (char *)NULL;
    char *avg  = (char *)NULL;
    char *_kp = (char *)NULL;
    char *_ki  = (char *)NULL;
    char *fg  = (char *)NULL;
    char *_kd = (char *)NULL;

    token = strtok(FGPSAUTO, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                phase = token;
                break;
            case 2:
                center = token;
                break;
            case 3:
                fg = token;
                break;
            case 4:
                scl = token;
                break;
            case 5:
                ahead = token;
                break;
            case 6:
                avg = token;
                break;
            case 7:
                _kp = token;
                break;
            case 8:
                _ki = token;
                break;
            case 9:
                _kd = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( _kp && _ki && _kd ) {
        kp = atof(_kp);
        ki = atof(_ki);
        kd = atof(_kd);
    }
    if ( phase && center && scl && avg && ahead ) {
        lookaheadtime = atof(ahead);
        scale = atof(scl);
        phaseadv = atof(phase);
        avgpos = atof(avg);
        tcenter = atof(center);
        filterg = atof(fg);

        // scale = scale * -1;
        SetDigitalFilter(phaseadv,tcenter, 0 );
    }
}

void pc_analyse(char* pc_string)
{
    pc.puts(pc_string);
    if (!strncmp(pc_string, "$ASTEER", 7)) {
        //stop sending when already exists
    } else if (!strncmp(pc_string, "$BANY",5)) {
        _ID = Config_GetID();
        Config_Save();
    } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
        string mystring = pc_string;
        string baud = pc_string;
        if ( mystring.find('*') > 0 ) {
            string baud = mystring.substr(0,mystring.find('*'));
        }
        process_GPSBAUD((char*)baud.c_str());
        Config_Save();
        sprintf(output,"%s %d\r\n",pc_string,gps_baud);
        //   pc.puts(output);
    } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
        process_FGPSAUTO(pc_string);
        sprintf(output,"%s\r\n",pc_string);
        bluetooth.puts(output);
        Config_Save();
    } else if (!strncmp(pc_string, "$FGPS,",6)) {
        int i=5;
        char c=pc_string[i];
        while (c!=0) {
            i++;
            if (i>255) break; //protect msg buffer!
            c=pc_string[i];
            gps.putc(c);
            pc.putc(c);
        }
    } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
        process_GPSHEIGHT(pc_string);
        sprintf(output,"%s\r\n",pc_string);
        bluetooth.puts(output);
        Config_Save();
    } else if (!strncmp(pc_string, "$FGPSAB",7)) {
        process_FGPSAB(pc_string);
    } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
        calibrateGyroscope();
        calibrateAccelerometer();
        Config_Save();
    } else if (!strncmp(pc_string, "$POSITION",9)) {
        char* pointer;
        char* Data[5];
        int index = 0;
        //Split data at commas
        pointer = strtok(pc_string, ",");
        if(pointer == NULL)
            Data[0] = pc_string;
        while(pointer != NULL) {
            Data[index] = pointer;
            pointer = strtok(NULL, ",");
            index++;
        }
        gyro_pos = atoi(Data[1]);
        Config_Save();
    } else {
    }
}

void gps_analyse(char* gps_string)
{
    if (!strncmp(gps_string, "$GPRMC", 6)) {
        nmea_rmc(gps_string); //analyse and decompose the rmc string
    } else {
        bluetooth.puts(gps_string);
    }
}

int i2 = 0;
bool end2 = false;
bool start2 = false;

bool getline2()
{
    int gotstring=false;
    while (1) {
        if( !bluetooth.readable() ) {
            break;
        }
        char c = bluetooth.getc();
        if (c == 36 ) {
            start2=true;
            end2 = false;
            i2 = 0;
        }
        if ((start2) && (c == 10)) {
            end2=true;
            start2 = false;
        }
        if (start2) {
            msg2[i2]=c;
            i2++;
            if (i2>255) break; //protect msg buffer!
        }
        if (end2) {
            msg2[i2]=c;
            msg2[i2+1] = 0;
            start2 = false;
            gotstring = true;
            end2=false;
            i2=0;
            break;
        }
    }
    return gotstring;
}


int i=0;
bool start=false;
bool end=false;

bool getline(bool forward)
{
    while (1) {
        if( !gps.readable() ) {
            break;
        }
        char c = gps.getc();
        if (forward) { //simply forward all to Bluetooth
            bluetooth.putc(c);
        }
        if (c == 36 ) {
            start=true;
            end = false;
            i = 0;
        }
        if ((start) && (c == 10)) {
            end=true;
            start = false;
        }
        if (start) {
            msg[i]=c;
            i++;
            if (i>255) break; //protect msg buffer!
        }
        if (end) {
            msg[i]=c;
            msg[i+1] = 0;
            i=0;
            start = false;
            end = true;
            break;
        }
    }
    return end;
}

void keyPressedHeld( void )
{
    motor_enable_state = "$ENABLE,0\r\n";
    motor_enable = 0;
    pwm1 = 0.0;
    pwm2 = 0.0;
    ledGREEN=1; //show green for being ready to steer
}

void keyReleasedHeld( void )
{
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    motor_enable_state = "$ENABLE,1\r\n";
    motor_enable = 1;
    pwm1 = 0.0;
    pwm2 = 0.0;
    ledGREEN=0;
}

void boom1PressedHeld( void )
{
    // ledGREEN=1;
    boom18=boom18 & 0xFE;
}

void boom1ReleasedHeld( void )
{
    //ledGREEN=0;
    boom18=boom18 | 0x01;
}

void boom2PressedHeld( void )
{
    boom18=boom18 & 0xFD;
}

void boom2ReleasedHeld( void )
{
    boom18=boom18 | 0x02;
}
void boom3PressedHeld( void )
{
    boom18=boom18 & 0xFB;
}

void boom3ReleasedHeld( void )
{
    boom18=boom18 | 0x04;
}

void boom4PressedHeld( void )
{
    boom18=boom18 & 0xF7;
}

void boom4ReleasedHeld( void )
{
    boom18=boom18 | 0x08;
}

void toprint()
{
    angle_send = 1;
}

double last_yaw = 0;
int counter = 0;
bool bear = false;
double lastroll = 0;
double lastpitch = 0;

int main()
{
    bluetooth.baud(115200);
    gps.baud(38400);
    pc.baud(38400);

//JH prepare and send version info
    vTimer.start();
    vTimer.reset();

    motTimer.start();
    motTimer.reset();
    lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
    motor_enable_state = "$ENABLE,1\r\n";

    btTimer.start();
    btTimer.reset();
    lastgetBT= btTimer.read_ms();

    bluetooth.puts(version);

    lastsend_version=vTimer.read_ms()-18000;
    //  pc.puts("test\r\n");
    /* Config_Startup();
     _ID = Config_GetID();
     Config_Save();
    */
    boom1.attach_asserted_held( &boom1PressedHeld );
    boom1.attach_deasserted_held( &boom1ReleasedHeld );
    boom1.setSampleFrequency(); //default = 20 ms
    boom1.setSamplesTillAssert(5);
    boom1.setSamplesTillHeld(5);
    boom2.attach_asserted_held( &boom2PressedHeld );
    boom2.attach_deasserted_held( &boom2ReleasedHeld );
    boom2.setSamplesTillAssert(5);
    boom2.setSamplesTillHeld(5);
    boom2.setSampleFrequency();
    boom3.attach_asserted_held( &boom3PressedHeld );
    boom3.attach_deasserted_held( &boom3ReleasedHeld );
    boom3.setSamplesTillAssert(5);
    boom3.setSamplesTillHeld(5);
    boom3.setSampleFrequency();
    boom4.attach_asserted_held( &boom4PressedHeld );
    boom4.attach_deasserted_held( &boom4ReleasedHeld );
    boom4.setSamplesTillAssert(5);
    boom4.setSamplesTillHeld(5);
    boom4.setSampleFrequency();

    motor_switch.setSampleFrequency(10000);
    motor_switch.attach_asserted_held( &keyPressedHeld );
    motor_switch.attach_deasserted_held( &keyReleasedHeld );

    initializeAccelerometer();
    calibrateAccelerometer();
    initializeGyroscope();
    calibrateGyroscope();

    accelerometerTicker.attach(&sampleAccelerometer, 0.005);
    gyroscopeTicker.attach(&sampleGyroscope, 0.005);
    filterTicker.attach(&filter, FILTER_RATE);
    angle_print.attach(&toprint,0.2);
    activate_antenna();
    autosteer_timeout.start();

    //setTunings(0.25, 5, 1);
    SetDigitalFilter(phaseadv,tcenter, 0);

    while(1) {
        //JH send version information every 10 seconds to keep Bluetooth alive
        if ((vTimer.read_ms()-lastsend_version)>25000) {
            pc.puts(version);
            bluetooth.puts(version);
            vTimer.reset();
            lastsend_version=vTimer.read_ms();
        }

        if ( autosteer_timeout.read_ms() > 5000 || autosteer_timeout.read_ms() > time_till_stop ) {
            autosteer_timeout.reset();
            enable_motor = 0;
        }
        if ( antenna_active == 1 && gps.readable()) {
            if (getline(false)) {
                if ( validate_checksum(msg)) {
                    //pc.puts(msg);
                    gps_analyse(msg);
                } else {
                    pc.puts("INVALID!!!!\r\n");
                }
            }
        }
        if ( bluetooth.readable()) {
            if (getline2()) {
                btTimer.reset();
                lastgetBT=  btTimer.read_ms();
                //     pc.puts(msg2);
                pc_analyse(msg2);
            }
        }
        if (  btTimer.read_ms()-lastgetBT>1000) {
            //we did not get any commands over BT
            ledRED=1; //turn red
        } else ledRED=0;

        if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
            bluetooth.puts(motor_enable_state);
            // pc.puts(motor_enable_state);
            motTimer.reset();
            lastsend_motorstate=motTimer.read_ms();
            lastmotor_enable=motor_enable;
        }
        if (boom18!=lastboom18) {
            boomstate[4]=boom18 | 0x80; //
            bluetooth.puts(boomstate);
            // pc.puts(boomstate);
            lastboom18=boom18;
        }
        if ( print_euler == 1 && angle_send == 1 ) { //&& reading == 0){
            lastpitch = 0.8*lastpitch + 0.2 * (toDegrees(get_pitch()*3.5));
            lastroll = 0.8 * lastroll + 0.2 * toDegrees(get_roll()*3.5);

            double dps = - last_yaw;
            last_yaw =toDegrees( imuFilter.getYaw()) * -1;
            dps = (dps + last_yaw) * 5; // update every 200 ms, so for dps need *5

            sprintf(output,"$EULER,%f,%f,%f\r\n",lastroll,lastpitch,dps);
            bluetooth.puts(output);
            angle_send = 0;
            counter++;
            if ( bear == false  && counter > 3 ) { //reinitialise the gyro after  600ms for the reference to be changed.
                imuFilter.reset();
                bear = true;
            }
        }
    }
}