File content as of revision 28:5905886c76ee:

#include "mbed.h"
//#include "MODSERIAL.h"
#include "PinDetect.h"
#include "IMUfilter.h"
#include "ADXL345_I2C.h"
#include "ITG3200.h"
#include "Point.h"
#include <vector>
#include "Line.h"
#include "stringUtils.h"
#include "base.h"
#include "Config.h"

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

int checksumm;

int dont = 0;
#define g0 9.812865328//Gravity at Earth's surface in m/s/s
#define SAMPLES 8//Number of samples to average.
#define CALIBRATION_SAMPLES 256//Number of samples to be averaged for a null bias calculation during calibration.
#define toDegrees(x) (x * 57.2957795)//Convert from radians to degrees.
#define toRadians(x) (x * 0.01745329252)//Convert from degrees to radians.
#define GYROSCOPE_GAIN (1 / 14.375)//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
#define ACCELEROMETER_GAIN (0.004 * g0)//Full scale resolution on the ADXL345 is 4mg/LSB.
#define GYRO_RATE   0.005//Sampling gyroscope at 200Hz.
#define ACC_RATE    0.005//Sampling accelerometer at 200Hz.
#define FILTER_RATE 0.1//Updating filter at 40Hz.
double distance_from_line;
double cm_per_deg_lon;
double cm_per_deg_lat;
//all timing objects
Timer gps_connecting;
Timer autosteer_time;
Timeout 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;
Ticker  debug_leds;

//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(p22);
PwmOut pwm2(p21);


//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.
//DigitalIn boom1(p20);
//DigitalIn boom2(p19);
//DigitalIn boom3(p18);
//DigitalIn boom4(p17);
char boom18; //1 byte
char lastboom18; //1 byte
char boomstate[8]={'$','F','B','S',0,13,10,0 };

//gyro
IMUfilter imuFilter(FILTER_RATE, 0.3);
ADXL345_I2C accelerometer(p28, p27);
ITG3200 gyroscope(p28,p27);

Point position;
Point looked_ahead;
Point line_start;
Point line_end;
Point tilt_compensated_position;
Point yaw_compensated_position;

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;

int antenna_active = 0;//do we have an antenna connected?
char msg[256]; //GPS line buffer
char msg2[256];//PC line buffer
int printing;
int num_of_gps_sats;
int print_vtg = 0; //FGPS asked for VTG?
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 print_gsa = 0;//FGPS request GSA printing
int print_gsv = 1;//FGPS request GSV printing.
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;

volatile double a_xAccumulator = 0;
volatile double a_yAccumulator = 0;
volatile double a_zAccumulator = 0;
volatile double w_xAccumulator = 0;
volatile double w_yAccumulator = 0;
volatile double w_zAccumulator = 0;

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

int readings[3];
int accelerometerSamples = 0;
int gyroscopeSamples = 0;
int print_euler = 1;

double Freepilot_lat;
double Freepilot_lon;
double Freepilot_lat1;
double Freepilot_lon1;
double Freepilot_bearing;
//double Freepilot_lon_meter;
//double Freepilot_lat_meter;

void initializeAcceleromter(void);
void calibrateAccelerometer(void);
void sampleAccelerometer(void);
void initializeGyroscope(void);
void calibrateGyroscope(void);
void sampleGyroscope(void);
void filter(void);

volatile bool newline_detected = false;
char tx_line[80];
char rx_line[80];

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());
}

#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

double dist_Point_to_Line( Point P, Point line_start, Point line_end)
{
    //Point v = point_sub(L->point1,L.point0);
    // Point w = point_sub(P,L.point0);
     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);
}

void initializeAccelerometer(void)
{
    accelerometer.setPowerControl(0x00);    //Go into standby mode to configure the device.
    accelerometer.setDataFormatControl(0x0B);    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataRate(ADXL345_200HZ);    //200Hz data rate.
    accelerometer.setPowerControl(0x08);    //Measurement mode.
    wait_ms(22);    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
}

void sampleAccelerometer(void)
{
    if (accelerometerSamples == SAMPLES) 
    {
        //Average the samples, remove the bias, and calculate the acceleration
        //in m/s/s.
        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
        a_xAccumulator = 0;
        a_yAccumulator = 0;
        a_zAccumulator = 0;
        accelerometerSamples = 0;
    } 
    else 
    {
        //Take another sample.
        accelerometer.getOutput(readings);
        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];
        accelerometerSamples++;
    }
}

void calibrateAccelerometer(void)
{
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    //Take a number of readings and average them
    //to calculate the zero g offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) 
    {
        accelerometer.getOutput(readings);
        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];
        wait(ACC_RATE);
    }

    a_xAccumulator /= CALIBRATION_SAMPLES;
    a_yAccumulator /= CALIBRATION_SAMPLES;
    a_zAccumulator /= CALIBRATION_SAMPLES;

    //At 4mg/LSB, 250 LSBs is 1g.
    a_xBias = a_xAccumulator;
    a_yBias = a_yAccumulator;
    a_zBias = (a_zAccumulator - 250);

    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
}

void initializeGyroscope(void)
{
    gyroscope.setLpBandwidth(LPFBW_42HZ);
    gyroscope.setSampleRateDivider(4);
}

void calibrateGyroscope(void)
{

    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;

    //Take a number of readings and average them
    //to calculate the gyroscope bias offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) 
    {
        w_xAccumulator += gyroscope.getGyroX();
        w_yAccumulator += gyroscope.getGyroY();
        w_zAccumulator += gyroscope.getGyroZ();
        wait(GYRO_RATE);
    }
    //Average the samples.
    w_xAccumulator /= CALIBRATION_SAMPLES;
    w_yAccumulator /= CALIBRATION_SAMPLES;
    w_zAccumulator /= CALIBRATION_SAMPLES;

    w_xBias = w_xAccumulator;
    w_yBias = w_yAccumulator;
    w_zBias = w_zAccumulator;

    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
}

void sampleGyroscope(void)
{
    if (gyroscopeSamples == SAMPLES) 
    {
        //Average the samples, remove the bias, and calculate the angular
        //velocity in rad/s.
        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);

        w_xAccumulator = 0;
        w_yAccumulator = 0;
        w_zAccumulator = 0;
        gyroscopeSamples = 0;
    } 
    else 
    {
        w_xAccumulator += gyroscope.getGyroX();
        w_yAccumulator += gyroscope.getGyroY();
        w_zAccumulator += gyroscope.getGyroZ();
        gyroscopeSamples++;
    }
}

void filter(void)
{
    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);    //Update the filter variables.
    imuFilter.computeEuler();    //Calculate the new Euler angles.
}

void activate_antenna()
{
    gps.baud(gps_baud);
    antenna_active = 1;
}

float Round_digits(float x, int numdigits)
{
    //  return ceil(x * pow(10,numdigits))/pow(10,numdigits);
    return ceil(x);
}

char* dec_deg_to_degminsec(double d_lat)
{
    double coord = d_lat;
    int sec = (int)Round_digits(coord * 3600,0);
    int deg = sec / 3600;
    sec = abs (sec % 3600);
    int min = sec / 60;
    sec %= 60;

    sprintf(degminsec,"%i%i%i",deg,min,sec);
    return degminsec;
}

double decimal_to_meters_lat(double lat)
{
    m_per_deg_lat = 111.111;
    m_lat = m_per_deg_lat * lat;
    return m_lat;
}

double decimal_to_meters_lon(double lon, double lat)
{
    m_per_deg_lon = 111.111 * cos(lat);
    double m_lon = m_per_deg_lon * lon;
    return m_lon;
}

double m_lat_to_dec_deg(double lat)
{
    m_per_deg_lon = 111.111;
    decimal_latitude =  decimal_latitude / m_per_deg_lat;
    return decimal_latitude;
}

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;
    }
    decimal_latitude = val;
    return val;
}

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;
    }
    decimal_lon = val;
    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;
    }
}

//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)));
    return;
}

Point compensation;
double compensation_angle;
//antenna compensation in cm
void tilt_compensate()
{
    roll = imuFilter.getRoll();
    compensation_vector = antennaheight * sin(roll);
    compensation.SetX(compensation_vector * cos((toDegrees(imuFilter.getYaw()) *  -1 - 90)/57.295779513));
    compensation.SetY(compensation_vector * sin((toDegrees(imuFilter.getYaw()) *  -1 - 90)/57.295779513));
}

void yaw_compensate()
{
    yaw = imuFilter.getYaw();
    
}

void modify_rmc()
{
    
}

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);
    }
}

void process_cs(char* cs_string)
{
                sprintf(output, "$cs: %s , %02X\r\n",cs_string, checksumm);
                pc.puts(output);
    char *token;
    int  token_counter = 0;
   // char *cs   = (char *)NULL;
    token = strtok(cs_string, "*");
    while (token) 
    {
        switch (token_counter) 
        {
            case 1:
                sprintf(output, "$cs:%s , %02X\r\n",token, checksumm);
                pc.puts(token);
                break;
        }
    }
}

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 *latitude  = (char *)NULL;
    char *longitude = (char *)NULL;
    char *lat_dir   = (char *)NULL;
    char *lon_dir   = (char *)NULL;

    token = strtok(s, ",*");
    while (token) 
    {
        switch (token_counter) 
        {
            case 9:
                date   = token;
                break;
            case 1:
                time   = token;
                break;
            case 2:
                stat   = token;
                break;
            case 7:
                vel    = token;
                break;
            case 8:
                trk    = token;
                break;
            case 10:
                magv   = token;
                break;
            case 11:
                magd   = token;
                break;
            case 3:
                latitude  = token;
                break;
            case 5:
                longitude = token;
                break;
            case 4:
                lat_dir   = token;
                break;
            case 6:
                lon_dir   = token;
                break;
           /* case 11:
                process_cs(token);*/
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if (stat && date && time) 
    {
        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;
        //speed_m_s = 5;
        track      = atof(trk);
        magvar     = atof(magv);
        magvar_dir = magd[0];
    }
        decimal_latitude = lat_to_deg(latitude,  lat_dir[0]);
        decimal_lon = lon_to_deg(longitude, lon_dir[0]);
        position.SetX(decimal_latitude);
        position.SetY(decimal_lon);
        cm_per_deg_lat = 111111;
        cm_per_deg_lon = 111111 * cos(decimal_latitude);
        tilt_compensate(); //in centimeters
        compensation.SetY(compensation.GetY() / cm_per_deg_lon);
        compensation.SetX(compensation.GetX() / cm_per_deg_lat);
        
       // yaw_compensate();
        position = point_add(position,compensation);
        modify_rmc();
        double lookaheaddistance = lookaheadtime * speed_m_s;
        
        get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,track,look_ahead_lon,look_ahead_lat);
        looked_ahead.SetX(look_ahead_lat);
        looked_ahead.SetY(look_ahead_lon);
        double filtering = 111.111 / 2.0 + 111.111 * cos(decimal_latitude)/2.0;
        distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end);
       
     //   sprintf(output,"$DIST_TO_LINE: % .12f\r\n\0",distance_to_line * filtering);
     //   pc.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;
        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:
                    bearing = token;
                    break;
            }
            token = strtok((char *)NULL, ",");
            token_counter++;
        }
            Freepilot_lon  = atof(line_lon);
            Freepilot_lat = atof(line_lat);
            Freepilot_lon1  = atof(line_lon1);
            Freepilot_lat1 = atof(line_lat1);
            Freepilot_bearing = atof(bearing);
            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\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY());
            pc.puts(output);
}

void autosteer_done()
{
    //kill the autosteer once the timeout reech
    enable_motor = 0;
}

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 *_kd = (char *)NULL;

    token = strtok(FGPSAUTO, ",");
    while (token) 
    {
        switch (token_counter) 
        {
            case 1:
                phase = token;
                break;
            case 2:
                center = 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);
         sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
            pc.puts(output);
    }
}
//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.attach_us(autosteer_done,(double)enable_time *  (double)1000.0);
        if ( motorspeed > 127.0 ) 
        {
            pwm2_speed = 0.0;
            pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0;
            enable_motor = 1;
        } 
        else if ( motorspeed < 127.0 ) 
        {
            pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 );
            pwm1_speed = 0.0;
            enable_motor = 1;
        } 
        else 
        {
            pwm1_speed = 0;
            pwm2_speed = 0;
            enable_motor = 0;
        }
        sprintf(output,"STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);     
        pc.puts(output);
        pwm1 = pwm1_speed;
        pwm2 = pwm2_speed;
    }
}
            

void process_initstring(char* init)
{
    char *token;
    int  token_counter = 0;
    char *init_string  = (char *)NULL;
    token = strtok(init, "$");
    while (token) 
    {
        switch (token_counter) 
        {
            case 1:
                init_string = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( init_string ) 
    {
        if ( antenna_active == 1 ) 
        {
            while(!gps.writeable()) {}   //disable uart3 interrupt (p9,p10)
             sprintf(output,"$%s",init_string);
            gps.puts(output);
            gps_connecting.start();
            gps_connecting.reset();
            connecting = 1;
        }
    }
}

void process_GPSBAUD(char* gpsbaud)
{
    char *token;
    int  token_counter = 0;
    char *baud  = (char *)NULL;
    token = strtok(gpsbaud, ",");
    while (token) 
    {
        switch (token_counter) 
        {
            case 1:
                baud = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( baud ) 
    {
        gps_baud = atoi(baud);
    }
    activate_antenna();
}

void pc_analyse(char* pc_string)
{
    if (!strncmp(pc_string, "$ASTEER", 7)) 
    {
        //sets pwm1 and pwm2 and enable_motor
        process_ASTEER(pc_string);
    } 
    else if (!strncmp(pc_string, "$FGPS-BAUD",10)) 
    {
        process_GPSBAUD(pc_string);
        sprintf(output,"%s %d\r\n",pc_string,gps_baud);
        pc.puts(output);
        
    } 
    else if (!strncmp(pc_string, "$FGPS,",6)) 
    {
       
       //process_initstring(pc_string);
       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);   
       }
      // sprintf(output,"%s\r\n",pc_string);
      // pc.puts(output);
      // gpsputs(output);
    } 
    else if (!strncmp(pc_string, "$PRINT_VTG=0",12)) 
    {
        print_vtg = 0;
    } 
    else if (!strncmp(pc_string, "$PRINT_VTG=1",12)) 
    {
        print_vtg = 1;
    } 
    else if (!strncmp(pc_string, "$PRINT_GSV=0",12))
    {
        print_gsv = 0;
    } 
    else if (!strncmp(pc_string, "$PRINT_GSV=1",12)) 
    {
        print_gsv = 1;
    } 
    else if (!strncmp(pc_string, "$PRINT_GSA=0",12)) 
    {
        print_gsa = 0;
    } 
    else if (!strncmp(pc_string, "$PRINT_GSA=1",12)) 
    {
        print_gsa = 1;
    } 
    else if (!strncmp(pc_string, "$PRINT_EULER=1",14))
    {
        print_euler = 1;
    } 
    else if (!strncmp(pc_string, "$PRINT_EULER=0",14)) 
    {
        print_euler = 0;
    } 
    else if (!strncmp(pc_string, "$GPS-HEIGHT",11)) 
    {
        process_GPSHEIGHT(pc_string);
        sprintf(output,"%s\r\n",pc_string);
        bluetooth.puts(output);
    } 
    else if (!strncmp(pc_string, "$CORRECT_RMC=1",14)) 
    {
        correct_rmc = 1;
    } 
    else if (!strncmp(pc_string, "$CORRECT_RMC=0",14)) 
    {
        correct_rmc = 0;
    } 
    else if (!strncmp(pc_string, "$FGPSAUTO",9)) 
    {
        process_FGPSAUTO(pc_string);
        sprintf(output,"%s\r\n",pc_string);
        bluetooth.puts(output);
    } 
    else if (!strncmp(pc_string, "$FGPSAB",7)) 
    {
        sprintf(output,"%s\r\n",pc_string);
      //  bluetooth.puts(output);
        pc.puts(output);
        process_FGPSAB(pc_string);

    } 
    else 
    {
    }
}

void gps_analyse(char* gps_string)
{
    if ( connecting == 1 ) 
    {
        if ( gps_connecting.read_ms() < connect_time && reading == 0 ) 
        {
        if ( bluetooth.writeable() > 0 )
        {
            bluetooth.puts(gps_string);
        } 
        }
        else 
        {
            connecting = 0;
            gps_connecting.stop();
        }
    }
    if (!strncmp(gps_string, "$GPRMC", 6)) 
    {
       // if ( connecting == 0 )// && correct_rmc == 1 ) 
     //   {
            bluetooth.puts(gps_string);
       // }
        nmea_rmc(gps_string); //analyse and decompose the rmc string
    } 
    else if (!strncmp(gps_string, "$GPGGA", 6)) 
    {
      //  if ( connecting == 0 ) 
      //  {
            bluetooth.puts(gps_string);
     //   }
        //nmea_gga(gps_string);//analyse and decompose the gga string
    } 
    else if (!strncmp(gps_string, "$GPVTG", 6)) 
    {
       // if ( print_vtg == 1 && connecting == 0 ) 
       // {
            bluetooth.puts(gps_string);
      //  }
    } 
    else if (!strncmp(gps_string, "$GPGSV", 6)) 
    {
        if ( print_gsv == 1 && connecting == 0 ) 
        {
            bluetooth.puts(gps_string);
        }
    } 
    else if (!strncmp(gps_string, "$GPGSA", 6)) 
    {
        if ( print_gsa == 1 && connecting == 0 ) 
        {
            bluetooth.puts(gps_string);
        }
    } 
    else 
    {
    }
}

/*void rxCallback(MODSERIAL_IRQ_INFO *q)
{
    if ( bluetooth.rxGetLastChar() == '\n') 
    {
        newline_detected = true;
    }
}*/




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 (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 (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;
            start = false;
            end = true;
            break;
        }
    }
    return end;
}


int sample()
{
    while (1) 
    {
        getline(false);
        return 1;
    }
}

int samplepc()
{
    while (1) 
    {
        getline2();
        return 1;
    }
}

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

void keyReleasedHeld( void )
{
    motor_enable_state = "$ENABLE,0\r\n";
    motor_enable = 0;
    ledGREEN=0; 
}

void boom1PressedHeld( void )
{
  
   // ledGREEN=1;
 boom18=boom18 & 0xFE;
   //  sprintf(output,"Boom1 Pressed %d",boom18);     
   //  pc.puts(output);
}

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

void debugging_leds( void )
{
}

int getCheckSum(char *string) {
int i;
int XOR;
int c;
// Calculate checksum ignoring any $'s in the string
for (XOR = 0, i = 0; i < strlen(string); i++) {
c = (unsigned char)string[i];
if (c == '*') break;
if (c != '$') XOR ^= c;
}
return XOR;
}

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

//JH prepare and send version info
    vTimer.start();
    vTimer.reset();
    motTimer.start();
    motTimer.reset();
    btTimer.start();
    btTimer.reset();
    lastgetBT=  btTimer.read_ms();
    
    pc.puts(version);
    bluetooth.puts(version);
    lastsend_version=vTimer.read_ms();
    
  
    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();
       
    Config_Startup();
  //  overide_config(gyro_position,gps_baud,antenna_height, a_zBias, a_yBias, a_zBias,w_xBias, w_yBias,w_zBias, copy_FREEPILOT);
   // int _ID=Config_SetID();
  //  Config_Save(_ID,_btMode,phaseadv,tcenter,filter_gain,scale,avg_pos,gyro_position,gps_baud,antenna_height, a_zBias, a_yBias, a_zBias,w_xBias, w_yBias,w_zBias);
    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);
//debug_leds.atatch(&debugging_leds,2);
gyroscopeTicker.attach(&sampleGyroscope, 0.005);
filterTicker.attach(&filter, FILTER_RATE);
angle_print.attach(&toprint,0.2);
    activate_antenna();
    while(1) 
    {
    
        //JH send version information every 10 seconds to keep Bluetooth alive
        if ((vTimer.read_ms()-lastsend_version)>15000) 
        {
       
            pc.puts(version);
            bluetooth.puts(version);
            vTimer.reset();
            lastsend_version=vTimer.read_ms();
          //  sprintf(output,"GPS Baudrate %d \r\n",gps_baud);     
          //  pc.puts(output);
          //  sprintf(output,"Boom18 %d \r\n",boom18);     
          //  pc.puts(output);
        } 
        
        if ( antenna_active == 1 && gps.readable()) 
        {
            if (getline(true)) 
            {
               // checksumm = getCheckSum(msg);
               // gps_analyse(msg);
            }
        }


        if ( bluetooth.readable()) 
        {
           if (getline2()) 
           {
               btTimer.reset();
               lastgetBT=  btTimer.read_ms();
               x++;
              //  trim(msg2,"\r");
             //   trim(msg2,"\n");
              //  trim(msg2,"\0");
                trim(msg2," ");
                sprintf(output,"%d %s",x,msg2);
                pc.puts(output);
             //pc.puts(output);
                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)>5000) || (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;
        }

        //bounces too much
        //if (boom1) boom18=boom18 & 0xFE;
        //else boom18=boom18 | 0x01;
  
        if (boom18!=lastboom18)
        {
           //  sprintf(output,"Change boom18= %d\r\n",boom18);
           //  pc.puts(output);
          
             boomstate[4]=boom18 | 0x80; //
             bluetooth.puts(boomstate);
             pc.puts(boomstate);
             lastboom18=boom18;
        }

   /*     if ( print_euler == 1 && angle_send == 1 ) //&& reading == 0) 
        {
            sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw()));
            pc.puts(output);
           bluetooth.puts(output);
            angle_send = 0;
        }
*/
    }
}