Diff: main.cpp

Revision:

46:d7d6dc429153

Parent:

45:ecd8c2e27948

Child:

47:d3123bb4f673

--- a/main.cpp	Tue Mar 10 23:14:18 2015 +0000
+++ b/main.cpp	Sat Mar 14 01:01:19 2015 +0000
@@ -387,7 +387,9 @@
 }
 
 //sets pwm1 and pwm2 and enable_motor
-void process_ASTEER(char* asteer)
+void process_ASTEER(char* asteer,bool from_pc)
+{
+if ( freepilot_v2 && !from_pc || from_pc && !freepilot_v2)
 {
     char *token;
     int  token_counter = 0;
@@ -433,6 +435,7 @@
             //}
         }
     }
+    }
 }
 
 char *strsep(char **stringp, char *delim)
@@ -776,7 +779,7 @@
         sprintf(command,"%s\r\n\0",steering.c_str());    //(int)((((val/2)-127)/scale)+127),500);
         pc.puts(command);
 
-        process_ASTEER(command);
+        process_ASTEER(command,false);
     }
     string rmc = "";
     if(time!= '\0') {
@@ -860,13 +863,6 @@
                 line_lon1 = token;
                 break;
             case 5:
-             /*  for (int n=0; n < sizeof(token); n++) {
-                    if ( token[n] == '*' ) {
-                        break;
-                    } else {
-                        bearing += token[n];
-                    }
-                }*/
                 bearing = token;
                 break;
         }
@@ -886,8 +882,8 @@
 
     active_AB = 1;
 
-    sprintf(output, "$ABLINE:%f , %f, %f, %f,  %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY(),Freepilot_bearing);
-    pc.puts(output);
+   // sprintf(output, "$ABLINE:%f , %f, %f, %f,  %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY(),Freepilot_bearing);
+    //pc.puts(output);
 }
 
 void process_FGPSAUTO(char* FGPSAUTO)
@@ -961,6 +957,7 @@
     pc.puts(pc_string);
     if (!strncmp(pc_string, "$ASTEER", 7)) {
         //stop sending when already exists
+        process_ASTEER(pc_string,true);
     } else if (!strncmp(pc_string, "$BANY",5)) {
         _ID = Config_GetID();
         // Config_Save();
@@ -990,6 +987,14 @@
         // 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();
@@ -1220,9 +1225,7 @@
     boom4.setSamplesTillAssert(5);
     boom4.setSamplesTillHeld(5);
     boom4.setSampleFrequency();
-
-    //motor_switch.setSampleFrequency(10000);
-
+    
     motor_switch.setSamplesTillAssert(5);
     motor_switch.setSamplesTillHeld(5);
     motor_switch.setSampleFrequency();