Diff: main.cpp

Revision:

50:07dfcda65732

Parent:

49:c43fa54dd990

Child:

51:f874574674c0

--- a/main.cpp	Sat Mar 14 02:00:23 2015 +0000
+++ b/main.cpp	Sat Mar 14 15:44:33 2015 +0000
@@ -18,7 +18,7 @@
 long lastsend_version=0;
 Timer vTimer; //this timer is int based! Max is 30 minutes
 
-int checksumm;
+//int checksumm;
 double distance_from_line;
 double cm_per_deg_lon;
 double cm_per_deg_lat;
@@ -73,10 +73,10 @@
 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 msg2_changed = 1;
+//char* buffer;
+//double meter_lat = 0;
+//double meter_lon = 0;
 
 char msg[256]; //GPS line buffer
 char msg2[256];//PC line buffer
@@ -86,23 +86,23 @@
 float latitude;
 char ns, ew;
 int lock;
-int flag_gga;
-int reading;
+//int flag_gga;
+//int reading;
 
 int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
 int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
 
 int angle_send = 0;
 int correct_rmc = 1;
-double m_lat = 0;
-double m_lon = 0;
+//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;
+//double compensation_vector;
 char output[256];
 
 double yaw;
@@ -118,7 +118,7 @@
 int readings[3];
 double Freepilot_bearing;
 int time_till_stop = 200;
-volatile bool newline_detected = false;
+//volatile bool newline_detected = false;
 
 void autosteer_done()
 {
@@ -140,6 +140,7 @@
     compensation.SetY(antennaheight * tan(roll) * cos(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * sin(real_bearing/ 57.295779513));
 }
 
+
 void process_GPSHEIGHT(char* height_string)
 {
     char *token;
@@ -162,57 +163,55 @@
 }
 
 //sets pwm1 and pwm2 and enable_motor
-void process_ASTEER(char* asteer,bool from_pc)
+void process_ASTEER(char* asteer)
 {
-    if ( freepilot_v2 && !from_pc || from_pc && !freepilot_v2) {
-        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++;
+    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;
         }
-        if ( asteer_speed && asteer_time ) {
-            motorspeed = atof(asteer_speed);
-            enable_time = atof(asteer_time);
-            autosteer_timeout.reset();
-            time_till_stop = atoi(asteer_time);
-            if ( motor_enable == 0 ) {
-            } else {
-                if ( motorspeed > 127.0 ) {
-                    pwm2_speed = 0.0;
-                    pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;
+        token = strtok((char *)NULL, ",");
+        token_counter++;
+    }
+    if ( asteer_speed && asteer_time ) {
+        motorspeed = atof(asteer_speed);
+        enable_time = atof(asteer_time);
+        autosteer_timeout.reset();
+        time_till_stop = atoi(asteer_time);
+        if ( motor_enable == 0 ) {
+        } else {
+            if ( motorspeed > 127.0 ) {
+                pwm2_speed = 0.0;
+                pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;
 
-                } else if ( motorspeed < 127.0 ) {
-                    pwm1_speed = 0.0;
-                    pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
-                } else {
-                    pwm1_speed = 0;
-                    pwm2_speed = 0;
-                    enable_motor = 0;
-                }
-                //  if(Authenticated)
-                // {
-                pwm1 = pwm1_speed;
-                pwm2 = pwm2_speed;
-                enable_motor = 1;
-                //}
+            } else if ( motorspeed < 127.0 ) {
+                pwm1_speed = 0.0;
+                pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
+            } else {
+                pwm1_speed = 0;
+                pwm2_speed = 0;
+                enable_motor = 0;
             }
+            //  if(Authenticated)
+            // {
+            pwm1 = pwm1_speed;
+            pwm2 = pwm2_speed;
+            enable_motor = 1;
+            //}
         }
     }
+
 }
 
-
 void action_on_rmc()
 {
     double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
@@ -250,27 +249,30 @@
 
     char command[128];
     sprintf(command,"%s\r\n\0",steering.c_str());    //(int)((((val/2)-127)/scale)+127),500);
-    pc.puts(command);
+    // pc.puts(command);
 
-    process_ASTEER(command,false);
+    process_ASTEER(command);
 
     string rmc = "";
 
     rmc = "$GPRMC,";
-    rmc +=  string(time_s) + ",";
-    rmc +=(string(stat) + ",");
-    rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
-    rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
-    rmc += (string(vel) + ",");
-    rmc += string(trk) + ",";
-    rmc += string(date) + ",";
-    rmc += string(magv) + ",";
-    rmc += string(magd) + ",W";
+    rmc +=  not_equal(string(time_s));
+    rmc += not_equal(string(stat));
+    rmc += string(To_DMS(position.GetX())) + "," + not_equal(string(lat_dir));
+    rmc += (string(To_DMS_lon(position.GetY())) + "," + not_equal(string(lon_dir)));
+    rmc += not_equal(string(vel));
+    rmc += not_equal(string(trk));
+    rmc += not_equal(string(date));
+    rmc += not_equal(string(magv));
+    rmc += not_equal(string(magd)) + "W";
 
     char test[256];
     sprintf(test,"%s*\0",rmc.c_str());
     sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
-
+    pc.puts("\r\n");
+    pc.puts(output);
+    pc.puts("\r\n");
+    bluetooth.puts("\r\n");
     bluetooth.puts(output);
 }
 
@@ -388,14 +390,17 @@
 
 void pc_analyse(char* pc_string)
 {
-    pc.puts(pc_string);
+    // pc.puts(pc_string);
     if (!strncmp(pc_string, "$ASTEER", 7)) {
         //stop sending when already exists
-        process_ASTEER(pc_string,true);
+        //      process_ASTEER(pc_string);
     } else if (!strncmp(pc_string, "$BANY",5)) {
         _ID = Config_GetID();
         // Config_Save();
-    } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
+    } else if (!strncmp(pc_string, "$FGPS-BAUD",10)) {
+        pc.puts("\r\n");
+        pc.puts(pc_string);
+        pc.puts("\r\n");
 
         process_GPSBAUD(pc_string);
         // Config_Save();
@@ -405,6 +410,7 @@
         bluetooth.puts(output);
         // Config_Save();
     } else if (!strncmp(pc_string, "$FGPS,",6)) {
+        pc.puts(pc_string);
         int i=5;
         char c=pc_string[i];
         while (c!=0) {
@@ -412,7 +418,7 @@
             if (i>255) break; //protect msg buffer!
             c=pc_string[i];
             gps.putc(c);
-            pc.putc(c);
+            //   pc.putc(c);
         }
     } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
         process_GPSHEIGHT(pc_string);
@@ -421,10 +427,6 @@
         // Config_Save();
     } else if (!strncmp(pc_string, "$FGPSAB",7)) {
         process_FGPSAB(pc_string);
-    } else if(strcmp(pc_string, "$V2,1") == 0) {
-        freepilot_v2 = true;
-    } else if(strcmp(pc_string, "$V2,0") == 0) {
-        freepilot_v2 = false;
     } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
         calibrateGyroscope();
         calibrateAccelerometer();
@@ -445,23 +447,45 @@
         gyro_pos = atoi(Data[1]);
         //    Config_Save();
     } else {
+        pc.puts("\r\n");
+        pc.puts(pc_string);
+        pc.puts("\r\n");
+
     }
 }
 
 void gps_analyse(char* gps_string)
 {
+    string temp(gps_string);
+    while ( temp.find ("\r\n") != string::npos ) {
+        temp.erase ( temp.find ("\r\n"), 2 );
+    }
+    sprintf(gps_string,"%s",temp.c_str());
+
     if (!strncmp(gps_string, "$GPRMC", 6)) {
+
         if (nmea_rmc(gps_string)) {
-            void action_on_rmc();
+            action_on_rmc();
         } //analyse and decompose the rmc string
+        else {
+            char test[256];
+            sprintf(test,"%s*",temp.c_str());
+            sprintf(output,"%s*%02X\r\n\0",temp.c_str(),getCheckSum(test));
+            bluetooth.puts(output);
+            pc.puts(output);
+        }
     } else {
-        bluetooth.puts(gps_string);
+        char test[256];
+        sprintf(test,"%s*\0",gps_string);
+        sprintf(output,"%s*%02X\r\n\0",gps_string,getCheckSum(test));
+        bluetooth.puts(output);
+        pc.puts(output);
     }
 }
 
-int i2 = 0;
-bool end2 = false;
-bool start2 = false;
+static int i2 = 0;
+static bool end2 = false;
+static bool start2 = false;
 
 bool getline2()
 {
@@ -498,20 +522,20 @@
     return gotstring;
 }
 
-
-int i=0;
-bool start=false;
-bool end=false;
+static int i=0;
+static bool start=false;
+static bool end=false;
 
 bool getline(bool forward)
 {
     while (1) {
+        //   gps.putc('\0');
         if( !gps.readable() ) {
             break;
         }
         char c = gps.getc();
         if (forward) { //simply forward all to Bluetooth
-            bluetooth.putc(c);
+            pc.putc(c);
         }
         if (c == 36 ) {
             start=true;
@@ -676,13 +700,12 @@
     activate_antenna();
     autosteer_timeout.start();
 
-    //setTunings(0.25, 5, 1);  //for PID
     SetDigitalFilter(phaseadv,tcenter, 0);  //for FGPS guidance
 
     while(1) {
         //JH send version information every 10 seconds to keep Bluetooth alive
         if ((vTimer.read_ms()-lastsend_version)>25000) {
-            pc.puts(version);
+            // pc.puts(version);
             bluetooth.puts(version);
             vTimer.reset();
             lastsend_version=vTimer.read_ms();
@@ -695,19 +718,20 @@
         if ( antenna_active == 1 && gps.readable()) {
             if (getline(false)) {
                 if ( validate_checksum(msg,result)) {
-                    //pc.puts(msg);
+                    // pc.puts(msg);
                     gps_analyse(result);
                 } else {
-                    pc.puts("INVALID!!!!\r\n");
+                    pc.puts(result);
                 }
-            }
+            }//
         }
         if ( bluetooth.readable()) {
             if (getline2()) {
                 if ( validate_checksum(msg2,result)) {
                     btTimer.reset();
                     lastgetBT=  btTimer.read_ms();
-                    //     pc.puts(msg2);
+
+                    pc.puts(msg2);
                     pc_analyse(result);
                 }
             }
@@ -749,4 +773,4 @@
             }
         }
     }
-}
+}
\ No newline at end of file