Diff: main.cpp

Revision:

30:3afafa1ef16b

Parent:

29:23ccb2a50b6f

Child:

32:c57bc701d65c

diff -r 23ccb2a50b6f -r 3afafa1ef16b main.cpp
--- a/main.cpp	Thu Jan 22 20:35:42 2015 +0000
+++ b/main.cpp	Tue Jan 27 12:22:10 2015 +0000
@@ -1,33 +1,18 @@
 #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"
+#include "imu_functions.h"
 
-char *version="FreePilot V2.11 Jan 20, 2015\r\n";
+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;
-
-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;
@@ -39,7 +24,6 @@
 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.
@@ -48,25 +32,16 @@
 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;
@@ -90,18 +65,16 @@
 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;
@@ -128,8 +101,7 @@
 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;
@@ -147,44 +119,22 @@
 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;
+double a_x;
+double a_y;
+double a_z;
+double w_x;
+double w_y;
+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)
 {
@@ -216,181 +166,11 @@
      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)
+/*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)
 {
@@ -410,6 +190,17 @@
     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
+double isLeft( Point P0, Point P1, Point P2 )
+{
+    return ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX())
+           - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
+}
+
 double lon_to_deg(char *s, char east_west)
 {
     int deg, min, sec;
@@ -513,12 +304,6 @@
 void yaw_compensate()
 {
     yaw = imuFilter.getYaw();
-    
-}
-
-void modify_rmc()
-{
-    
 }
 
 void process_GPSHEIGHT(char* height_string)
@@ -542,26 +327,7 @@
     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;
-        }
+        Config_Save();
     }
 }
 
@@ -646,26 +412,34 @@
         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);
+        cm_per_deg_lat = 11054000;
+        cm_per_deg_lon = 11132000 * 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();
+        //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;
+        double filtering = 111111 / 2.0 + 111111 * 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);
+        double sign = isLeft( line_start, line_end, looked_ahead);
+        if ( sign < 0 )
+        {
+            distance_to_line =  distance_to_line; 
+        }
+        else if ( sign > 0 )
+        {
+            distance_to_line = -distance_to_line; 
+        }
 
+        sprintf(output,"$DIST_TO_LINE: % .12f   %f\r\n\0",distance_to_line * filtering, sign);
+        pc.puts(output);
 }
 
 void process_FGPSAB(char* ab)
@@ -734,7 +508,6 @@
     char *_kp = (char *)NULL;
     char *_ki  = (char *)NULL;
     char *_kd = (char *)NULL;
-
     token = strtok(FGPSAUTO, ",");
     while (token) 
     {
@@ -781,10 +554,11 @@
         phaseadv = atof(phase);
         avgpos = atof(avg);
         tcenter = atof(center);
-         sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
-            pc.puts(output);
+        sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
+        pc.puts(output);
     }
 }
+
 //sets pwm1 and pwm2 and enable_motor
 void process_ASTEER(char* asteer)
 {
@@ -792,8 +566,6 @@
     int  token_counter = 0;
     char *asteer_speed  = (char *)NULL;
     char *asteer_time = (char *)NULL;
-
-
     token = strtok(asteer, ",");
     while (token) 
     {
@@ -847,60 +619,7 @@
 }
             
 
-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)
 {
@@ -911,20 +630,18 @@
     } 
     else if (!strncmp(pc_string, "$BANY",5)) 
     {
-        
         _ID = Config_GetID();
         Config_Save();
     }
-    else if (!strncmp(pc_string, "$FGPS-BAUD",10)) 
+    else if (!strncmp(pc_string, "$GPSBAUD",8)) 
     {
         process_GPSBAUD(pc_string);
+        Config_Save();
         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];
@@ -936,61 +653,21 @@
           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)) 
+
+    else if (!strncmp(pc_string, "$GPSHEIGHT",10)) 
     {
         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;
+        Config_Save();
     } 
     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, "$FGPSAB",7)) 
     {
@@ -998,7 +675,11 @@
       //  bluetooth.puts(output);
       //  pc.puts(output);
         process_FGPSAB(pc_string);
-
+    } 
+    else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) 
+    {
+        calibrateAccelerometer();
+        Config_Save();
     } 
     else 
     {
@@ -1007,74 +688,14 @@
 
 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();
-        }
-    }
+    pc.puts(gps_string);
+    bluetooth.puts(gps_string);
     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;
@@ -1146,8 +767,7 @@
             if (i>255) break; //protect msg buffer!
         }
         if (end)
-        {
-            
+        { 
             msg[i]=c;   
             msg[i+1] = 0;
             i=0;
@@ -1159,25 +779,6 @@
     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";
@@ -1194,34 +795,27 @@
 
 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;
 }
 
@@ -1243,28 +837,24 @@
   boom18=boom18 | 0x08;
 }
 
-
-
 void toprint()
 {
     angle_send = 1;
 }
 
-void debugging_leds( void )
+int getCheckSum(char *string)
 {
-}
-
-int getCheckSum(char *string) {
-int i;
-int XOR;
-int c;
+    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;
+    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()
@@ -1272,7 +862,7 @@
     int x=0;
     bluetooth.baud(115200);
     gps.baud(38400);
-pc.baud(38400);
+    pc.baud(38400);
 
 //JH prepare and send version info
     vTimer.start();
@@ -1290,8 +880,9 @@
     bluetooth.puts(version);
     lastsend_version=vTimer.read_ms()-18000;
     
-     _ID = Config_GetID();
-     Config_Save();
+    Config_Startup();
+    _ID = Config_GetID();
+    Config_Save();
   
     boom1.attach_asserted_held( &boom1PressedHeld );
     boom1.attach_deasserted_held( &boom1ReleasedHeld );
@@ -1314,51 +905,38 @@
     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);
+    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();
     
     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();
-          //  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);
+                 checksumm = getCheckSum(msg);
+                 gps_analyse(msg);
             }
         }
-
-
         if ( bluetooth.readable()) 
         {
            if (getline2()) 
@@ -1366,18 +944,12 @@
                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
@@ -1394,29 +966,21 @@
             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) 
+        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);
+           // pc.puts(output);
+            bluetooth.puts(output);
             angle_send = 0;
         }
-*/
     }
 }
\ No newline at end of file