Diff: angle_error.h

Revision:

59:3e5e5b97ca75

Parent:

58:e8426e488925

--- a/angle_error.h	Wed Apr 01 16:08:05 2015 +0000
+++ b/angle_error.h	Sat Apr 04 01:22:04 2015 +0000
@@ -50,14 +50,14 @@
     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);
+    //  pc.printf("bearing,%f  %f  %f  %f  %f \r\n",x,y,x5,y5,bearing);
     forward.SetX(x5);
     forward.SetY(y5);
 
     return forward;
 }
 
-double 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)
+/*double 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;
@@ -70,12 +70,48 @@
     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 bearing = atan2(cos(pos.GetY())*sin(forward_pos.GetY())-
+    //            sin(pos.GetY())*cos(forward_pos.GetY())*cos(forward_pos.GetX()-pos.GetX()),
+    //           sin(forward_pos.GetX()-pos.GetX())*cos(forward_pos.GetX()));
+    // =ATAN2(COS(lat1)*SIN(lat2)-SIN(lat1)*COS(lat2)*COS(lon2-lon1),
+
+//     double  y = sin(forward_pos.GetX()-pos.GetX()) * cos(forward_pos.GetX());
+//double x = cos(pos.GetX())*sin(forward_pos.GetX()) - sin(pos.GetX())*cos(forward_pos.GetX())*cos(forward_pos.GetY()-pos.GetY());
+//double bearing = ToDeg(atan2(y, x));
+//bearing = angle_normal(bearing,-180,180);
+    // SIN(lon2-lon1)*COS(lat2))
+
+    //        XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
+//(positive XTD means right of course, negative means left)
+//(If the point A is the N. or S. Pole replace crs_AD-cr
+//double bearing=asin(sqrt( (sin(dist_AD))^2 - (sin(XTD))^2 )/cos(XTD))
+
+//XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
+
+//b = arccos ( cos (90 - lat2) * cos (90 - lat1) + sin (90 - lat2) * sin (90 - lat1) * cos (lon2 - lon1) )
+//A = arcsin ( sin (90 - lat2) * sin (lon2 - lon1) / sin (b) )
+
+    double b = acos ( cos (ToRad(90 - forward_pos.GetX())) * cos (ToRad(90 - pos.GetX())) + sin (ToRad(90 - forward_pos.GetX())) * sin (ToRad(90 - pos.GetX())) * cos (ToRad(forward_pos.GetY() - pos.GetY()) ));
+    double bearing = asin ( sin (ToRad(90 - forward_pos.GetX())) * sin (ToRad(forward_pos.GetY() - pos.GetY())) / sin (b) );
+
+double c = acos ( cos (ToRad(90 - end.GetY())) * cos (ToRad(90 - start.GetY())) + sin (ToRad(90 - end.GetY())) * sin (ToRad(90 - start.GetY())) * cos (ToRad(end.GetX() - start.GetX())));
+    double d = asin ( sin (ToRad(90 - end.GetY())) * sin (ToRad(end.GetX() - start.GetX())) / sin (c) );
+
+double dot = ToRad(pos.GetX())*ToRad(forward_pos.GetX()) + ToRad(pos.GetY())*ToRad(forward_pos.GetX());//      # dot product
+double det = ToRad(pos.GetX())*ToRad(forward_pos.GetY()) - ToRad(pos.GetY())*ToRad(forward_pos.GetX());//      # determinant
+double angl = atan2(det, dot); 
+//dot = x1*x2 + y1*y2      # dot product
+//det = x1*y2 - y1*x2      # determinant
+//angle = atan2(det, dot) 
+    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 error_angle = ToDeg(asin((forward_pos.GetX()-pos.GetX())/(sqrt((forward_pos.GetX()-pos.GetX())*(forward_pos.GetX()-pos.GetX())+(forward_pos.GetY()-pos.GetY())*(forward_pos.GetY()-pos.GetY())))));
+ //   heading_to_line = angle_normal(heading_to_line,-180,180);
+   // pc.printf("ANGLES,%f   %f    %f    %f     %f   %f    %f \r\n",error_angle,antenna_avg,heading_to_line,error, ToDeg(bearing), ToDeg(d), ToDeg(angl));
+   //     pc.printf("ANGLES,%f   %f    %f    %f     %f   %f    %f \r\n",error_angle,antenna_avg,heading_to_line,error, ToDeg(bearing), ToDeg(d), ToDeg(angl));
+
     return heading_to_line;
-}
\ No newline at end of file
+}*/
\ No newline at end of file