Diff: main.cpp

Revision:

47:d3123bb4f673

Parent:

46:d7d6dc429153

Child:

48:5d9c63364c94

--- a/main.cpp	Sat Mar 14 01:01:19 2015 +0000
+++ b/main.cpp	Sat Mar 14 01:28:37 2015 +0000
@@ -8,12 +8,12 @@
 #include "Config.h"
 #include "imu_functions.h"
 #include "atoh.h"
+#include "string_utilities.h"
+
 #include "checksum.h"
 #include <string.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
+#include "gps.h"
+#include "autosteer.h"
 
 char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
 long lastsend_version=0;
@@ -83,17 +83,12 @@
 char msg2[256];//PC line buffer
 char* result;
 int printing;
-int num_of_gps_sats;
-
-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;
@@ -128,7 +123,6 @@
 double yaw;
 double pitch;
 double roll;
-
 double a_x;
 double a_y;
 double a_z;
@@ -138,189 +132,14 @@
 
 int readings[3];
 double Freepilot_bearing;
-
 int time_till_stop = 200;
 volatile bool newline_detected = false;
 
-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());
-}
-
-double get_yaw()
-{
-    double yaw_angle = imuFilter.getYaw() * -1 ; // *-1 to reverse polarity as gyro is up-side-down
-    return yaw_angle;
-}
-
-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(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);
-}
-
-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;
-    }
-    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
-int isLeft( Point P0, Point P1, Point P2 )
-{
-    double isleft = ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
-    if ( isleft > 0 ) {
-        isleft = 1;
-    } else {
-        isleft = -1;
-    }
-    return (int)isleft;
-}
-
-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;
-    }
-    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;
-    }
-}
-
 void autosteer_done()
 {
     enable_motor = 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)));
-}
-
 Point compensation;
 
 void yaw_compensate()
@@ -357,306 +176,55 @@
     }
 }
 
-char dms[128];
-char* To_DMS(double dec_deg)
-{
-    dec_deg = abs(dec_deg);
-    int d = (int)(dec_deg);
-    sprintf(dms,"%0.2i\0",d);
-    double m = (double)(((double)dec_deg - (double)d) * 60.0);
-    if (m < 10 ) {
-        sprintf(dms,"%s0%0.9f\0",dms,m);
-    } else {
-        sprintf(dms,"%s%0.9f\0",dms,m);
-    }
-    return dms;
-}
-
-char* To_DMS_lon(double dec_deg)
-{
-    dec_deg = abs(dec_deg);
-    int d = (int)(dec_deg);
-    sprintf(dms,"%0.3i\0",d);
-    double m = (double)(((double)dec_deg - (double)d) * 60.0);
-    if (m < 10 ) {
-        sprintf(dms,"%s0%0.9f\0",dms,m);
-    } else {
-        sprintf(dms,"%s%0.9f\0",dms,m);
-    }
-    return dms;
-}
-
 //sets pwm1 and pwm2 and enable_motor
 void process_ASTEER(char* asteer,bool from_pc)
 {
-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;
+    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++;
         }
-        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;
+        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;
+                } 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;
+                //}
             }
-            //  if(Authenticated)
-            // {
-            pwm1 = pwm1_speed;
-            pwm2 = pwm2_speed;
-            enable_motor = 1;
-            //}
         }
     }
-    }
-}
-
-char *strsep(char **stringp, char *delim)
-{
-    char *s;
-    const char *spanp;
-    int c, sc;
-    char *tok;
-    if ((s = *stringp) == NULL)
-        return (NULL);
-    for (tok = s;;) {
-        c = *s++;
-        spanp = delim;
-        do {
-            if ((sc = *spanp++) == c) {
-                if (c == 0)
-                    s = NULL;
-                else
-                    s[-1] = 0;
-                *stringp = s;
-                return (tok);
-            }
-        } while (sc != 0);
-    }
-    /* NOTREACHED */
-}
-
-//Maybe you rather use the routine I currently use in FarmerGPS? It uses a lookahead and simply distance to AB-line.
-//No heading error at all.
-
-//ControlSteerFilter is the main routine. ActiveTime in ms, typically under 200ms, distAUTOL is distance to AB line at lookahead position
-/*#include "mbed.h"
-#include <string.h>
-#include <math.h>
-#include <stdlib.h>*/
-
-#ifndef PI
-#define PI 3.14159265359
-#endif
-
-double m_Tcenter[5] = {0,0,0,0,0};
-double mphaseadv[5]= {0,0,0,0,0};
-double morder[5]= {0,0,0,0,0};
-int order;
-double B0[5]= {0,0,0,0,0};
-double B1[5]= {0,0,0,0,0};
-double B2[5]= {0,0,0,0,0};
-double B3[5]= {0,0,0,0,0};
-double A_1[5]= {0,0,0,0,0};
-double A_2[5]= {0,0,0,0,0};
-double A_3[5]= {0,0,0,0,0};
-
-double mx[5]= {0,0,0,0,0};
-double my[5]= {0,0,0,0,0};
-double mz[5]= {0,0,0,0,0};
-int Err_aPort = 0;
-
-double OutputValue = 0;
-double OutputTime = 0;
-double LastOutputValue = 0;
-
-double SpeedN = 1;
-int porder = 0;
-
-std::string itos(int n)
-{
-    const int max_size = std::numeric_limits<int>::digits10 + 1 /*sign*/ + 1 /*0-terminator*/;
-    char buffer[max_size] = {0};
-    sprintf(buffer, "%d", n);
-    return std::string(buffer);
-}
-
-void SetDigitalFilter(double phaseadv, double _Tcenter, int filternumber)
-{
-    m_Tcenter[filternumber] = _Tcenter;
-    mphaseadv[filternumber] = phaseadv;
-    morder[filternumber] = porder;
-    _Tcenter = _Tcenter / 2 / PI;
-    order = porder;
-
-    double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
-    double _T1 = sqrt(T1T2ratio) * _Tcenter;
-    double _T2 =_T1 / T1T2ratio;
-    double _T = 0.2;
-    double _K = (1 + 2 * _T1 / _T);
-    double _L = (1 - 2 * _T1 / _T);
-    double _M = (1 + 2 * _T2 / _T);
-    double _N = (1 - 2 * _T2 / _T);
-//   order = 2;
-    //version 1,
-    switch (order) {
-        case 3:
-            B0[filternumber] = pow(_K, 3) / pow(_M, 3);
-            B1[filternumber] = 3 * pow(_K, 2) * _L / pow(_M, 3);
-            B2[filternumber] = 3 * _K * pow(_L, 2) / pow(_M, 3);
-            B3[filternumber] = pow(_L, 3) / pow(_M, 3);
-
-            A_1[filternumber] = 3 * _N / _M;
-            A_2[filternumber] = 3 * pow(_N, 2) / pow(_M, 2);
-            A_3[filternumber] = pow(_N, 3) / pow(_M, 3);
-            break;
-        case 2:
-            B0[filternumber] = pow(_K, 2) / pow(_M, 2);
-            B1[filternumber] = 2 * _K * _L / pow(_M, 2);
-            B2[filternumber] = pow(_L, 2) / pow(_M, 2);
-            B3[filternumber] = 0;
-
-            A_1[filternumber] = 2 * _N / _M;
-            A_2[filternumber] = pow(_N, 2) / pow(_M, 2);
-            A_3[filternumber] = 0;
-            break;
-        case 1:
-        case 0:
-            B0[filternumber] = _K / _M;
-            B1[filternumber] = _L / _M;
-            B2[filternumber] = 0;
-            B3[filternumber] = 0;
-
-            A_1[filternumber] = _N / _M;
-            A_2[filternumber] = 0;
-            A_3[filternumber] = 0;
-            break;
-    }
-}
-//double d = 0;
-
-string Steer(int ActiveTime,int value)
-{
-    string sRet = "";
-    //f ((Err_aPort == 0)) {
-    //  if (ActiveTime > 300) ActiveTime = 300;
-    if (value < 0) value = 0;
-    if ((value > 255)) value = 255;
-    OutputValue = value;
-    OutputTime = ActiveTime;
-
-    // d =  //= DateTime.Now - autosteer.LastCommunication;
-
-    //no need to send repeated 127=do nothing
-    //if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
-    sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
-    LastOutputValue = OutputValue;
-    //  autosteer.Timer1counter = 0;
-    //  autosteer.LastCommunication = DateTime.Now;
-    //}
-//  == }
-    return (sRet);
-}
-
-string ControlSteerFilter(int ActiveTime,  double distAUTOL, double speed, double FilterGain, double min, double max,double SCALE)
-{
-    string sRet = "";
-
-    int N = 3;
-    double y = 0;
-    //  if (B0[0] == 9999.0) {
-    //     SetDigitalFilter(47, 4.2 / 2 / PI, 0);
-    // }
-    //  if (distAUTOL == 5000) distAUTOL = 0; //not set
-//   if (speed < 1) steer=false;
-
-    mx[N - 3] = mx[N - 2];
-    mx[N - 2] = mx[N - 1];
-    mx[N - 1] = mx[N];
-    if ( FilterGain > 0 ) {
-        if ( abs(distAUTOL) > 0 ) {
-            mx[N] = distAUTOL * FilterGain;
-
-
-            my[N] = -A_1[0] * (double)my[N - 1] - A_2[0] * (double)my[N - 2] - A_3[0] * (double)my[N - 3] + B0[0] * (double)mx[N] + B1[0] * (double)mx[N - 1] + B2[0] * (double)mx[N - 2] + B3[0] * (double)mx[N - 3];
-            mz[N] = my[N];
-
-            my[N - 3] = my[N - 2];
-            my[N - 2] = my[N - 1];
-            my[N - 1] = my[N];
-
-            mz[N - 3] = mz[N - 2];
-            mz[N - 2] = mz[N - 1];
-            mz[N - 1] = mz[N];
-        }
-    }
-    double y1 = (double)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
-    //pc.printf("%f\r\n",y1);
-
-    //modify scale depending on distance!
-    double mscale = SCALE;
-    //if (abs(distAUTOL) > 0.5) mscale = SCALE * 1.5;
-    //if (abs(distAUTOL) > 1.5) mscale = SCALE * 1.5;
-    mscale = 0.85 * abs(distAUTOL) + SCALE;
-
-    y = (double)(y1 * mscale);      // scale it here   if neccesary
-    // pc.printf("%f\r\n",y);
-
-    // y = (double) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
-    y = (double)(y * pow((10.0 / speed), SpeedN)); //SpeedN varies the gain factor with speed  n=0 to 1 or more. n=0 should give  y=
-
-    y = y * SCALE;
-
-    if ( y < -max ) {
-        y = -max;
-    } else if ( y > max) {
-        y = max;
-    }
-    y = 127 + y;
-
-    if (y <= 127) y = y - (min / 2.0);
-    if (y >= 127) y = y + (min / 2.0);
-
-    //  y = y + 127;
-    if (y >= 255) y = 255;
-    if (y <= 0) y = 0;
-
-    int value = (int)y;
-
-    if (speed > 1.0 ) {
-        sRet= Steer( ActiveTime, value);
-    } else {
-        sRet = Steer( ActiveTime, 127 );
-    }
-
-    return (sRet);
 }
 
 Point old_position;
@@ -882,7 +450,7 @@
 
     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);
+    // 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);
 }
 
@@ -987,13 +555,9 @@
         // Config_Save();
     } else if (!strncmp(pc_string, "$FGPSAB",7)) {
         process_FGPSAB(pc_string);
-    } 
-        else if(strcmp(pc_string, "$V2,1") == 0) 
-    {
+    } else if(strcmp(pc_string, "$V2,1") == 0) {
         freepilot_v2 = true;
-    } 
-        else if(strcmp(pc_string, "$V2,0") == 0) 
-    {
+    } else if(strcmp(pc_string, "$V2,0") == 0) {
         freepilot_v2 = false;
     } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
         calibrateGyroscope();
@@ -1225,7 +789,7 @@
     boom4.setSamplesTillAssert(5);
     boom4.setSamplesTillHeld(5);
     boom4.setSampleFrequency();
-    
+
     motor_switch.setSamplesTillAssert(5);
     motor_switch.setSamplesTillHeld(5);
     motor_switch.setSampleFrequency();