Diff: main.cpp

Revision:

58:e8426e488925

Parent:

57:0299098b2d0e

Child:

59:3e5e5b97ca75

--- a/main.cpp	Wed Apr 01 01:19:30 2015 +0000
+++ b/main.cpp	Wed Apr 01 16:08:05 2015 +0000
@@ -14,6 +14,7 @@
 #include "gps.h"
 #include "autosteer.h"
 #include "utilities.h"
+#include "angle_error.h"
 
 char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
 long lastsend_version=0;
@@ -218,90 +219,13 @@
     }
 }
 
-Point on_linePoint(double lookahead, Point pos, Point start, Point end, double bearing)
-{
-    Point forward;
-
-    double mline = (end.GetY()-start.GetY())/(end.GetX()-start.GetX());
-    double mp3 = - 1 / mline;
-    double b = pos.GetY() - ( mp3 * pos.GetX());
-    double bline = end.GetY()-mline*end.GetX();
-    double x = (b-bline)/(mline-mp3);
-    double y = mline * x + bline;
-
-    double x5;
-    double y5;
-    get_latlon_byangle(x,y,lookahead,bearing,y5,x5);
-    pc.printf("bearing,%f  %f  %f  %f  %f \r\n",x,y,x5,y5,bearing);
-    forward.SetX(x5);
-    forward.SetY(y5);
-
-    return forward;
-}
-
-void bearing_to_line(double *z_array, double *track_array,double la,Point pos, Point start, Point end, double lookahead_time,double scl_, double avgp, Point forw)
-{
-    avgp = abs(avgp);
-    double gyroDPS = z_array[0]*5;
-    double antenna_avg = arr_avg(track_array,avgp);
-
-    for ( int i = 0; i < avgp / 2; i++ ) {
-        antenna_avg += z_array[i];
-    }
-
-    antenna_avg = angle_normal(antenna_avg, 0, 360);
-    Point forward_pos = on_linePoint(la, pos,start,end,track_array[0]);
-
-    pc.printf("POS,%f   %f   %f   %f\r\n",forward_pos.GetX(),forward_pos.GetY(),pos.GetX(),pos.GetY());
-    
-    double heading_to_line = CalculateHeading(pos.GetX(), pos.GetY(), forward_pos.GetX(), forward_pos.GetY());
-    double error = heading_err(antenna_avg, heading_to_line);
-
-    heading_to_line = angle_normal(heading_to_line,-180,180);
-    pc.printf("ANGLES,%f    %f    %f \r\n",antenna_avg,heading_to_line,error);
-
-/*    double DPS_error = error / lookahead_time;// - gyroDPS;
-    DPS_error = constrains(DPS_error, -30,30);
-    DPS_error *= scl_;
-    DPS_error = constrains(DPS_error,-127,127);
-    DPS_error = dead_band(DPS_error,20);
-    DPS_error += 127;
-
-
-
-    string steering = "ASTEER,";
-    steering += string(itos((int)DPS_error));
-    steering +=  ",250\r\n";
-    sprintf(output,"%s",steering.c_str());
-    pc.puts(output);
-
-    process_ASTEER(output,false);*/
-}
-
-
-
-
-double new_bearing(double track_, int avgpos_, double *track_arr_, double *z_arr_)
-{
-    arr_pushback((track_-90)*-1,track_arr_,10);
-    double track_avg_ = arr_avg(track_arr_,abs(avgpos_));
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    //  double z_summ = z_arr_[0]*abs(avgpos_)*2;//arr_sum((int)5, z_arr_);///////////////////////////////////////
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    double z_summ = z_arr_[0]*abs(kp);////////////////////////////////////////////////////////////////////////////
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    z_summ = constrains(z_summ,-45.0,45.0);
-    track_avg_ -= z_summ;
-    // pc.printf("$NEWANGLE,%f,    %f,    %f\r\n",track_avg_, track_arr[0], z_summ);
-
-    return track_avg_;
-}
-
 void action_on_rmc()
 {
     line_inverted();
 
     double track_avg = new_bearing(track, avgpos, track_arr,z_arr);
+    
+    
     pitch_and_roll(track_avg);// as to be real bearing
 
     cm_per_deg_lat = 11054000;
@@ -328,7 +252,7 @@
      process_ASTEER(command,false);
     */
 
-    bearing_to_line(z_arr, track_arr, lookaheaddistance, position, line_start, line_end, lookaheadtime, scale * -1,avgpos, looked_ahead);
+    double bearing_error = bearing_to_line(z_arr, track_arr, lookaheaddistance, position, line_start, line_end, lookaheadtime, scale * -1,avgpos, looked_ahead);
 
 
 
@@ -820,23 +744,8 @@
             lastboom18=boom18;
         }
         if ( print_euler == 1 && angle_send == 1 ) {
-            lastpitch = 0.9 * lastpitch + 0.1 * (toDegrees(get_pitch()*3.5));
-            lastroll = 0.9 * lastroll + 0.1 * toDegrees(get_roll()*3.5);
-
-            static double x_;
-            static double y_;
-            static double z_;
-            get_angle(z_,y_,x_);
-            angle_send = 0;
-            arr_pushback(z_/5.0, z_arr, 5);
-            //z_summ = arr_sum((int)5, z_arr);
-            //          z_summ *= 5;
-            //        z_summ = constrains(z_summ,-45.0,45.0);
-
-            sprintf(output,"$EULER,%f,%f,%f\r\n",x_,y_,z_);
-            bluetooth.puts(output);
+            update_euler(z_arr,output, angle_send);
             pc.puts(output);
-
         }
     }
 }
\ No newline at end of file