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@58:e8426e488925, 2015-04-01 (annotated)

Committer:
maximbolduc
Date:
Wed Apr 01 16:08:05 2015 +0000
Revision:
58:e8426e488925
Child:
59:3e5e5b97ca75
classe done

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 58:e8426e488925 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 58:e8426e488925 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 58:e8426e488925 73 // pc.printf("POS,%f %f %f %f\r\n",forward_pos.GetX(),forward_pos.GetY(),pos.GetX(),pos.GetY());
maximbolduc 58:e8426e488925 74
maximbolduc 58:e8426e488925 75 double heading_to_line = CalculateHeading(pos.GetX(), pos.GetY(), forward_pos.GetX(), forward_pos.GetY());
maximbolduc 58:e8426e488925 76 double error = heading_err(antenna_avg, heading_to_line);
maximbolduc 58:e8426e488925 77
maximbolduc 58:e8426e488925 78 heading_to_line = angle_normal(heading_to_line,-180,180);
maximbolduc 58:e8426e488925 79 //pc.printf("ANGLES,%f %f %f \r\n",antenna_avg,heading_to_line,error);
maximbolduc 58:e8426e488925 80 return heading_to_line;
maximbolduc 58:e8426e488925 81 }