Diff: main.cpp

Revision:

26:dc00998140af

Child:

27:9ac59b261d87

Child:

31:c40f16ff3a2f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jan 16 17:26:07 2015 +0000
@@ -0,0 +1,1186 @@
+#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"
+
+// Connect the TX of the GPS module to p10 RX input
+MODSERIAL b(p9, p10);
+MODSERIAL pc(USBTX, USBRX);
+MODSERIAL bluetooth(p13, p14);
+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;
+
+PinDetect  motor_switch(p24); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
+DigitalOut enable_motor(p21);
+DigitalOut led1(p11);
+DigitalOut led2(p12);
+PwmOut pwm1(p22);
+PwmOut pwm2(p23);
+
+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 parameters
+double look_ahead_time = 2;
+double look_ahead_distance = 5;
+double scale = 1;
+double phaseadv = 50;
+double _Tcenter = 5;
+double filter_gain = 125;
+double avg_pos = -4;
+
+//FreePilot variables
+int timer_enabled;
+int motorspeed;
+int enable_time;
+char* motor_enable_state = 0;
+int motor_enable_tosend = 0;
+double pwm1_speed;
+double pwm2_speed;
+
+// in prevision of PID addition to FreePilot
+double kp = 0;
+double ki = 0;
+double kd = 0;
+
+int msg2_changed = 1;
+char* buffer;
+double meter_lat = 0;
+double meter_lon = 0;
+double antenna_height = 200;
+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 gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate.
+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];
+//offsets
+double w_xBias;
+double w_yBias;
+double w_zBias;
+double a_xBias;
+double a_yBias;
+double a_zBias;
+
+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()
+{
+b.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 = antenna_height * 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 ) 
+    {
+        antenna_height = 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();
+        look_ahead_distance = look_ahead_time * speed_m_s;
+        
+        get_latlon_byangle(position.GetX(),position.GetY(),look_ahead_distance,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 ) 
+    {
+        look_ahead_time = atof(ahead);
+        scale = atof(scl);
+        phaseadv = atof(phase);
+        avg_pos = atof(avg);
+        _Tcenter = atof(center);
+         sprintf(output, "$SETTINGS:%f\r\n", look_ahead_time);
+            pc.puts(output);
+    }
+}
+
+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(autosteer_done,(double)enable_time / (double)1000);
+        if ( motorspeed > 127 ) 
+        {
+            pwm2_speed = 0;
+            pwm1_speed = ((double)motorspeed - (double)127) / 127;
+            enable_motor = 1;
+        } 
+        else if ( motorspeed < 127 ) 
+        {
+            pwm2_speed = ((double)motorspeed / 127 );
+            pwm1_speed = 0;
+            enable_motor = 1;
+        } 
+        else 
+        {
+            pwm1_speed = 0;
+            pwm2_speed = 0;
+            enable_motor = 0;
+        }
+        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(!b.writeable()) {}   //disable uart3 interrupt (p9,p10)
+             sprintf(output,"$%s",init_string);
+            b.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)) 
+    {
+        process_ASTEER(pc_string);
+    } 
+    else if (!strncmp(pc_string, "$GPS-BAUD",9)) 
+    {
+        process_GPSBAUD(pc_string);
+        sprintf(output,"%s\r\n",pc_string);
+        bluetooth.puts(output);
+    } 
+    else if (!strncmp(pc_string, "$FGPS,",6)) 
+    {
+       process_initstring(pc_string);
+       sprintf(output,"%s\r\n",pc_string);
+       bluetooth.puts(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;
+    }
+}*/
+
+void getline2()
+{
+    for (int i = 0; i<126; i++) 
+    {
+        msg2[i] = bluetooth.getc();
+        if (msg2[i] == '\n') 
+        {
+            msg2[i+1] = 0;
+            dont = 0;
+            return;
+        }
+    }
+}
+
+void getline()
+{
+    while (b.getc() != '$');
+    msg[0] = '$';  // wait for the start of a line
+    for (int i=1; i<256; i++) 
+    {
+        msg[i] = b.getc();
+        if (msg[i] == '\n') 
+        {
+            msg[i+1] = 0;
+            return;
+        }
+    }
+}
+
+int sample()
+{
+    while (1) 
+    {
+        getline();
+        return 1;
+    }
+}
+
+int samplepc()
+{
+    while (1) 
+    {
+        getline2();
+        return 1;
+    }
+}
+
+void keyPressedHeld( void )
+{
+    motor_enable_state = "$ENABLE=1\r\n";
+    motor_enable_tosend = 1;
+}
+
+void keyReleasedHeld( void )
+{
+    motor_enable_state = "$ENABLE=0\r\n";
+    motor_enable_tosend = 1;
+}
+
+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()
+{
+    bluetooth.baud(38400);
+    b.baud(38400);
+pc.baud(38400);
+    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) 
+    {
+        if ( antenna_active == 1 ) 
+        {
+            if ( b.readable()) 
+            {
+                if (sample()) 
+                {
+                    checksumm = getCheckSum(msg);
+                    if ( pc.writeable()) 
+                    {
+                        sprintf(output,"%s\r\n\0",msg);
+                        pc.puts(output);
+                    }
+                    gps_analyse(msg);
+                }
+            }
+        }
+
+        if ( bluetooth.readable()) 
+        {
+            if (samplepc()) 
+            {
+                trim(msg2,"\r");
+                trim(msg2,"\n");
+                trim(msg2,"\0");
+                trim(msg2," ");
+                sprintf(output,"%s\0",msg2);
+                pc.puts(output);
+                pc_analyse(msg2);
+            }
+        }
+        if ( motor_enable_tosend == 1 && reading == 0 ) 
+        {
+            bluetooth.puts(motor_enable_state);
+            motor_enable_tosend = 0;
+        }
+     /*  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;
+        }*/
+    }
+}
\ No newline at end of file