Diff: main.cpp

Revision:

33:3e71c418e90d

Parent:

32:c57bc701d65c

Child:

34:c2bc9f9be7ff

diff -r c57bc701d65c -r 3e71c418e90d main.cpp
--- a/main.cpp	Thu Jan 29 02:49:07 2015 +0000
+++ b/main.cpp	Mon Feb 02 18:24:03 2015 +0000
@@ -9,6 +9,10 @@
 #include "imu_functions.h"
 #include "atoh.h"
 
+#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
+
 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
@@ -149,14 +153,63 @@
     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 get_yaw()
+{
+    double yaw_angle = imuFilter.getYaw() * -1 ; // *-1 to reverse polarity as gyro is up-side-down 
+    return yaw_angle;
+}
+
+void update_motor()
+{
+    
+}
+
+double get_roll()
+{
+    double roll_angle = 0;
+    if ( gyro_pos == 0 ) 
+    {
+        roll_angle = imuFilter.getRoll();
+    } 
+    else if ( gyro_pos == 1 ) 
+    {
+        roll_angle = imuFilter.getRoll() * -1;
+    } 
+    else if( gyro_pos == 2 ) 
+    {
+        roll_angle = imuFilter.getPitch() * -1;
+    } 
+    else if ( gyro_pos == 3 ) 
+    {
+        roll_angle = imuFilter.getPitch();
+    }
+    return roll_angle;
+}
+
+double get_pitch()
+{
+    double pitch_angle = 0;
+    if ( gyro_pos == 0 )
+     {
+         pitch_angle = imuFilter.getPitch();
+     }
+     else if ( gyro_pos == 1 )
+     {
+        pitch_angle = imuFilter.getPitch() * -1;
+     }
+     else if( gyro_pos == 2 )
+     {
+        pitch_angle = imuFilter.getRoll();
+     }
+     else if ( gyro_pos == 3 )
+     {
+            pitch_angle = imuFilter.getRoll() * -1;
+     }
+    return pitch_angle;
+}
 
 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);
 
@@ -292,17 +345,25 @@
 //antenna compensation in cm
 void tilt_compensate()
 {
-    roll = imuFilter.getRoll();
+    roll = get_roll();
     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));
+    compensation.SetX(compensation_vector * cos(get_yaw() *  -1 - (3.14159265359 / 2)));// 57.295779513));
+    compensation.SetY(compensation_vector * sin(get_yaw() *  -1 - (3.14159265359 / 2)));//  57.295779513));
+}
+void pitch_compensate()
+{
+    pitch = get_pitch();
+    compensation_vector = antennaheight * sin(pitch);
+    compensation.SetX(compensation.GetX() + compensation_vector * cos(get_yaw() *  -1 - (3.14159265359 / 2)));// /57.295779513));
+    compensation.SetY(compensation.GetY() + compensation_vector * sin(get_yaw() *  -1 - (3.14159265359 / 2)));// / 57.295779513));
 }
 
 void yaw_compensate()
 {
-    yaw = imuFilter.getYaw();
+    yaw = get_yaw();
 }
 
+
 void process_GPSHEIGHT(char* height_string)
 {
     char *token;
@@ -382,8 +443,6 @@
             case 6:
                 lon_dir   = token;
                 break;
-           /* case 11:
-                process_cs(token);*/
         }
         token = strtok((char *)NULL, ",");
         token_counter++;
@@ -400,7 +459,6 @@
         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];
@@ -411,15 +469,16 @@
         position.SetY(decimal_lon);
         cm_per_deg_lat = 11054000;
         cm_per_deg_lon = 11132000 * cos(decimal_latitude);
-        tilt_compensate(); //in centimeters
+        
+        // tilt_compensate(); //in centimeters
+        // pitch_compensate();
+        // yaw_compensate();
+        
         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);
@@ -435,56 +494,58 @@
             distance_to_line = -distance_to_line; 
         }
 
+        //modify_rmc();
+        
         sprintf(output,"$DIST_TO_LINE: % .12f   %f\r\n\0",distance_to_line * filtering, sign);
         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) 
+    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) 
         {
-            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++;
+            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;
         }
-            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);
+        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()
@@ -615,14 +676,10 @@
     }
 }
             
-
-
-
 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, "$BANY",5)) 
@@ -639,7 +696,6 @@
     } 
     else if (!strncmp(pc_string, "$FGPS,",6)) 
     {
-       //process_initstring(pc_string);
        int i=5;
        char c=pc_string[i];
        while (c!=0)
@@ -668,9 +724,6 @@
     } 
     else if (!strncmp(pc_string, "$FGPSAB",7)) 
     {
-      //  sprintf(output,"FOUND AB %s\r\n",pc_string);
-      //  bluetooth.puts(output);
-      //  pc.puts(output);
         process_FGPSAB(pc_string);
     } 
     else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) 
@@ -685,24 +738,19 @@
         char* pointer;
         char* Data[5];
         int index = 0;
-
         //Split data at commas
         pointer = strtok(pc_string, ",");
-
         if(pointer == NULL)
             Data[0] = pc_string;
-
         while(pointer != NULL) 
         {
             Data[index] = pointer;
             pointer = strtok(NULL, ",");
             index++;
         }
-
+        //int temp_pos = 
         gyro_pos = atoi(Data[1]);
-        pc.puts("POSITION=");
-        pc.puts(Data[1]);
-        pc.puts("\r\n");
+        pc.printf("POSITION=%i\r\n",gyro_pos);//("POSITION=");
         Config_Save();
     }
     else 
@@ -717,7 +765,7 @@
     if (!strncmp(gps_string, "$GPRMC", 6)) 
     {
         nmea_rmc(gps_string); //analyse and decompose the rmc string
-    } 
+    }
 }
 
 int i2 = 0;
@@ -727,17 +775,22 @@
 bool getline2()
 {
     int gotstring=false;
-    while (1)
-    {    
-        if( !bluetooth.readable() )
+    while (1) 
+    {
+        if( !bluetooth.readable() ) 
         {
             break;
         }
         char c = bluetooth.getc();
-        if (c == 36 ){start2=true;end2 = false; i2 = 0;}
-        if ((start2) && (c == 10))
+        if (c == 36 ) 
         {
-            end2=true; 
+            start2=true;
+            end2 = false;
+            i2 = 0;
+        }
+        if ((start2) && (c == 10)) 
+        {
+            end2=true;
             start2 = false;
         }
         if (start2) 
@@ -746,9 +799,9 @@
             i2++;
             if (i2>255) break; //protect msg buffer!
         }
-        if (end2)
+        if (end2) 
         {
-            msg2[i2]=c;   
+            msg2[i2]=c;
             msg2[i2+1] = 0;
             start2 = false;
             gotstring = true;
@@ -864,14 +917,12 @@
 }
 
 char* checksum2;
-
 int getCheckSum(char *string)
 {
     int i;
     int XOR;
     int c;
     bool started = false;
-// Calculate checksum ignoring any $'s in the string
     for (XOR = 0, i = 0; i < strlen(string); i++) 
     {
         c = (unsigned char)string[i];
@@ -881,7 +932,6 @@
         {
         if (c == '*')
         {
-          //  sprintf(checksum2,"%c%c",string2[i+ 1],string2[i+2]);
             break;
         }
         if (c != '$') XOR ^= c;
@@ -890,6 +940,24 @@
     return XOR;
 }
 
+bool validate_checksum(char* validating)
+{
+    bool valid = false;
+    int tempo = getCheckSum(msg);
+    string token, mystring(msg);
+    while(token != mystring) 
+    {
+        token = mystring.substr(0,mystring.find_first_of("*"));
+        mystring = mystring.substr(mystring.find_first_of("*") + 1,2);
+    }
+    checksumm = atoh <uint16_t>(token.c_str());
+    if (tempo == checksumm) 
+    {
+        valid = true;
+    }
+    return valid;
+}
+                
 int main()
 {
     int x=0;
@@ -904,7 +972,10 @@
     motTimer.reset();
     lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
     motor_enable_state = "$ENABLE,0\r\n";
-     
+         initializeAccelerometer();
+    calibrateAccelerometer();
+    initializeGyroscope();
+    calibrateGyroscope();
     btTimer.start();
     btTimer.reset();
     lastgetBT=  btTimer.read_ms();
@@ -941,10 +1012,7 @@
     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);
     gyroscopeTicker.attach(&sampleGyroscope, 0.005);
     filterTicker.attach(&filter, FILTER_RATE);
@@ -966,32 +1034,14 @@
         {
             if (getline(true)) 
             {
-             int tempo = getCheckSum(msg);
-
-                //c//hecksumm = getCheckSum(msg);
-                string token, mystring(msg);
-                while(token != mystring) 
+                if ( validate_checksum(msg)) 
                 {
-                    token = mystring.substr(0,mystring.find_first_of("*"));
-                    mystring = mystring.substr(mystring.find_first_of("*") + 1,2);
+                    gps_analyse(msg);
+                } 
+                else 
+                {
+                    pc.puts("INVALID!!!!\r\n");
                 }
-             //   char* test;
-                // sprintf(test,"%s",token.c_str());
-               // checksum = atoh(checksum,2);
-                checksumm = atoh <uint16_t>(token.c_str());
-              if (tempo == checksumm)
-                {
-                    sprintf(output,"\r\nVALID %s %X\r\n",token.c_str(),checksumm);
-                    pc.puts(output);
-                }
-                else
-                {
-                    sprintf(output,"\r\nNOTVALID %s %X\r\n",token.c_str(),checksumm);
-                    pc.puts(output);
-
-                }
-                    
-                 gps_analyse(msg);
             }
         }
         if ( bluetooth.readable()) 
@@ -1031,8 +1081,8 @@
         }
         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);
+            sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(get_roll()),toDegrees(get_pitch()),toDegrees(get_yaw()));
+            pc.puts(output);
             bluetooth.puts(output);
             angle_send = 0;
         }