Agra-GPS / FreePilot_V2-3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreePilot PinDetect mbed-src

Fork of FreePilot_V2-2 by Agra-GPS

angle_error.h@59:3e5e5b97ca75, 2015-04-04 (annotated)

Committer:
maximbolduc
Date:
Sat Apr 04 01:22:04 2015 +0000
Revision:
59:3e5e5b97ca75
Parent:
58:e8426e488925 
Current code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 58:e8426e488925 1 extern int angle_send;
maximbolduc 58:e8426e488925 2
maximbolduc 58:e8426e488925 3 void update_euler(double *z_arr, char*output, int &angle_send)
maximbolduc 58:e8426e488925 4 {
maximbolduc 58:e8426e488925 5 lastpitch = 0.9 * lastpitch + 0.1 * (toDegrees(get_pitch()*3.5));
maximbolduc 58:e8426e488925 6 lastroll = 0.9 * lastroll + 0.1 * toDegrees(get_roll()*3.5);
maximbolduc 58:e8426e488925 7
maximbolduc 58:e8426e488925 8 static double x_;
maximbolduc 58:e8426e488925 9 static double y_;
maximbolduc 58:e8426e488925 10 static double z_;
maximbolduc 58:e8426e488925 11 get_angle(z_,y_,x_);
maximbolduc 58:e8426e488925 12 angle_send = 0;
maximbolduc 58:e8426e488925 13 arr_pushback(z_/5.0, z_arr, 5);
maximbolduc 58:e8426e488925 14 //z_summ = arr_sum((int)5, z_arr);
maximbolduc 58:e8426e488925 15 // z_summ *= 5;
maximbolduc 58:e8426e488925 16 // z_summ = constrains(z_summ,-45.0,45.0);
maximbolduc 58:e8426e488925 17
maximbolduc 58:e8426e488925 18 sprintf(output,"$EULER,%f,%f,%f\r\n",x_,y_,z_);
maximbolduc 58:e8426e488925 19 // bluetooth.puts(output);
maximbolduc 58:e8426e488925 20 //pc.puts(output);
maximbolduc 58:e8426e488925 21 }
maximbolduc 58:e8426e488925 22
maximbolduc 58:e8426e488925 23 double new_bearing(double track_, int avgpos_, double *track_arr_, double *z_arr_)
maximbolduc 58:e8426e488925 24 {
maximbolduc 58:e8426e488925 25 arr_pushback((track_-90)*-1,track_arr_,10);
maximbolduc 58:e8426e488925 26 double track_avg_ = arr_avg(track_arr_,abs(avgpos_));
maximbolduc 58:e8426e488925 27 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 58:e8426e488925 28 // double z_summ = z_arr_[0]*abs(avgpos_)*2;//arr_sum((int)5, z_arr_);///////////////////////////////////////
maximbolduc 58:e8426e488925 29 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 58:e8426e488925 30 double z_summ = z_arr_[0]*abs(kp);////////////////////////////////////////////////////////////////////////////
maximbolduc 58:e8426e488925 31 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 58:e8426e488925 32 z_summ = constrains(z_summ,-45.0,45.0);
maximbolduc 58:e8426e488925 33 track_avg_ -= z_summ;
maximbolduc 58:e8426e488925 34 // pc.printf("$NEWANGLE,%f, %f, %f\r\n",track_avg_, track_arr[0], z_summ);
maximbolduc 58:e8426e488925 35
maximbolduc 58:e8426e488925 36 return track_avg_;
maximbolduc 58:e8426e488925 37 }
maximbolduc 58:e8426e488925 38
maximbolduc 58:e8426e488925 39 Point on_linePoint(double lookahead, Point pos, Point start, Point end, double bearing)
maximbolduc 58:e8426e488925 40 {
maximbolduc 58:e8426e488925 41 Point forward;
maximbolduc 58:e8426e488925 42
maximbolduc 58:e8426e488925 43 double mline = (end.GetY()-start.GetY())/(end.GetX()-start.GetX());
maximbolduc 58:e8426e488925 44 double mp3 = - 1 / mline;
maximbolduc 58:e8426e488925 45 double b = pos.GetY() - ( mp3 * pos.GetX());
maximbolduc 58:e8426e488925 46 double bline = end.GetY()-mline*end.GetX();
maximbolduc 58:e8426e488925 47 double x = (b-bline)/(mline-mp3);
maximbolduc 58:e8426e488925 48 double y = mline * x + bline;
maximbolduc 58:e8426e488925 49
maximbolduc 58:e8426e488925 50 double x5;
maximbolduc 58:e8426e488925 51 double y5;
maximbolduc 58:e8426e488925 52 get_latlon_byangle(x,y,lookahead,bearing,y5,x5);
maximbolduc 59:3e5e5b97ca75 53 // pc.printf("bearing,%f %f %f %f %f \r\n",x,y,x5,y5,bearing);
maximbolduc 58:e8426e488925 54 forward.SetX(x5);
maximbolduc 58:e8426e488925 55 forward.SetY(y5);
maximbolduc 58:e8426e488925 56
maximbolduc 58:e8426e488925 57 return forward;
maximbolduc 58:e8426e488925 58 }
maximbolduc 58:e8426e488925 59
maximbolduc 59:3e5e5b97ca75 60 /*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)
maximbolduc 58:e8426e488925 61 {
maximbolduc 58:e8426e488925 62 avgp = abs(avgp);
maximbolduc 58:e8426e488925 63 // double gyroDPS = z_array[0]*5;
maximbolduc 58:e8426e488925 64 double antenna_avg = arr_avg(track_array,avgp);
maximbolduc 58:e8426e488925 65
maximbolduc 58:e8426e488925 66 for ( int i = 0; i < avgp / 2; i++ ) {
maximbolduc 58:e8426e488925 67 antenna_avg += z_array[i];
maximbolduc 58:e8426e488925 68 }
maximbolduc 58:e8426e488925 69
maximbolduc 58:e8426e488925 70 antenna_avg = angle_normal(antenna_avg, 0, 360);
maximbolduc 58:e8426e488925 71 Point forward_pos = on_linePoint(la, pos,start,end,track_array[0]);
maximbolduc 58:e8426e488925 72
maximbolduc 59:3e5e5b97ca75 73 // double bearing = atan2(cos(pos.GetY())*sin(forward_pos.GetY())-
maximbolduc 59:3e5e5b97ca75 74 // sin(pos.GetY())*cos(forward_pos.GetY())*cos(forward_pos.GetX()-pos.GetX()),
maximbolduc 59:3e5e5b97ca75 75 // sin(forward_pos.GetX()-pos.GetX())*cos(forward_pos.GetX()));
maximbolduc 59:3e5e5b97ca75 76 // =ATAN2(COS(lat1)*SIN(lat2)-SIN(lat1)*COS(lat2)*COS(lon2-lon1),
maximbolduc 59:3e5e5b97ca75 77
maximbolduc 59:3e5e5b97ca75 78 // double y = sin(forward_pos.GetX()-pos.GetX()) * cos(forward_pos.GetX());
maximbolduc 59:3e5e5b97ca75 79 //double x = cos(pos.GetX())*sin(forward_pos.GetX()) - sin(pos.GetX())*cos(forward_pos.GetX())*cos(forward_pos.GetY()-pos.GetY());
maximbolduc 59:3e5e5b97ca75 80 //double bearing = ToDeg(atan2(y, x));
maximbolduc 59:3e5e5b97ca75 81 //bearing = angle_normal(bearing,-180,180);
maximbolduc 59:3e5e5b97ca75 82 // SIN(lon2-lon1)*COS(lat2))
maximbolduc 59:3e5e5b97ca75 83
maximbolduc 59:3e5e5b97ca75 84 // XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
maximbolduc 59:3e5e5b97ca75 85 //(positive XTD means right of course, negative means left)
maximbolduc 59:3e5e5b97ca75 86 //(If the point A is the N. or S. Pole replace crs_AD-cr
maximbolduc 59:3e5e5b97ca75 87 //double bearing=asin(sqrt( (sin(dist_AD))^2 - (sin(XTD))^2 )/cos(XTD))
maximbolduc 59:3e5e5b97ca75 88
maximbolduc 59:3e5e5b97ca75 89 //XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
maximbolduc 59:3e5e5b97ca75 90
maximbolduc 59:3e5e5b97ca75 91 //b = arccos ( cos (90 - lat2) * cos (90 - lat1) + sin (90 - lat2) * sin (90 - lat1) * cos (lon2 - lon1) )
maximbolduc 59:3e5e5b97ca75 92 //A = arcsin ( sin (90 - lat2) * sin (lon2 - lon1) / sin (b) )
maximbolduc 59:3e5e5b97ca75 93
maximbolduc 59:3e5e5b97ca75 94 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()) ));
maximbolduc 59:3e5e5b97ca75 95 double bearing = asin ( sin (ToRad(90 - forward_pos.GetX())) * sin (ToRad(forward_pos.GetY() - pos.GetY())) / sin (b) );
maximbolduc 59:3e5e5b97ca75 96
maximbolduc 59:3e5e5b97ca75 97 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())));
maximbolduc 59:3e5e5b97ca75 98 double d = asin ( sin (ToRad(90 - end.GetY())) * sin (ToRad(end.GetX() - start.GetX())) / sin (c) );
maximbolduc 59:3e5e5b97ca75 99
maximbolduc 59:3e5e5b97ca75 100 double dot = ToRad(pos.GetX())*ToRad(forward_pos.GetX()) + ToRad(pos.GetY())*ToRad(forward_pos.GetX());// # dot product
maximbolduc 59:3e5e5b97ca75 101 double det = ToRad(pos.GetX())*ToRad(forward_pos.GetY()) - ToRad(pos.GetY())*ToRad(forward_pos.GetX());// # determinant
maximbolduc 59:3e5e5b97ca75 102 double angl = atan2(det, dot);
maximbolduc 59:3e5e5b97ca75 103 //dot = x1*x2 + y1*y2 # dot product
maximbolduc 59:3e5e5b97ca75 104 //det = x1*y2 - y1*x2 # determinant
maximbolduc 59:3e5e5b97ca75 105 //angle = atan2(det, dot)
maximbolduc 59:3e5e5b97ca75 106 pc.printf("POS,%f %f %f %f\r\n",forward_pos.GetX(),forward_pos.GetY(),pos.GetX(),pos.GetY());
maximbolduc 58:e8426e488925 107
maximbolduc 58:e8426e488925 108 double heading_to_line = CalculateHeading(pos.GetX(), pos.GetY(), forward_pos.GetX(), forward_pos.GetY());
maximbolduc 58:e8426e488925 109 double error = heading_err(antenna_avg, heading_to_line);
maximbolduc 58:e8426e488925 110
maximbolduc 59:3e5e5b97ca75 111 //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())))));
maximbolduc 59:3e5e5b97ca75 112 // heading_to_line = angle_normal(heading_to_line,-180,180);
maximbolduc 59:3e5e5b97ca75 113 // 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));
maximbolduc 59:3e5e5b97ca75 114 // 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));
maximbolduc 59:3e5e5b97ca75 115
maximbolduc 58:e8426e488925 116 return heading_to_line;
maximbolduc 59:3e5e5b97ca75 117 }*/